Init

.gitignore

@@ -0,0 +1,132 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+pip-wheel-metadata/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+sandbox.py
+waypoint_traj.py

LICENSE.md

@@ -0,0 +1,7 @@
+Copyright 2023 RotorPy Authors
+
+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.

README.md

@@ -0,0 +1,62 @@
+# RotorPy
+A Python-based multirotor simulation environment with aerodynamic wrenches, useful for education and research in estimation, planning, and control for UAVs.
+
+## Purpose and Scope
+The purpose of this simulator is to add lumped parameter representations of the UAV aerodynamics to to a multirotor UAV (quadrotor unless otherwise noted). 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), these aerodynamic effects quickly become noticeable. 
+
+The engine is intended to be lightweight, easy to install with limited dependencies or requirements, and readable. The simulator is designed to gain intuition 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.
+
+# Installation
+
+First, clone this repository into a folder of your choosing.  
+
+It is recommended that you use a virtual environment to install this simulator, as some of the package dependencies are slightly out of date, including `matplotlib` which uses an older version for some of the 3D plotting. 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
+
+A good place to start would be to reference the `basic_usage.py` script found in the home directory of this simulator. 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. 
+
+# 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, I'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 advise 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. 
+
+# In Literature
+
+The following is a selection of papers that have used a previous version 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.
+
+# Acknowledgements
+
+We would like to acknowledge [Jimmy Paulos](https://github.com/jpaulos) who wrote the majority of the underlying engine for \textit{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). 

examples/basic_usage.py

@@ -0,0 +1,103 @@
+"""
+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
+
+# 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. 
+
+"""
+Instantiation
+"""
+
+# 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=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_fname  = 'double_pillar',            # 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. 
+                           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)

examples/wind_filter_example.py

@@ -0,0 +1,313 @@
+"""
+This example will demonstrate the capability of RotorPy by evaluating an
+Unscented Kalman Filter designed to do wind estimation for a quadrotor UAV. 
+
+First, we'll import some useful Python packages.
+"""
+
+import numpy as np
+import scipy as sp
+import copy
+import pandas as pd
+import matplotlib.pyplot as plt
+
+np.random.seed(0)    # For repeatability we will set the seed for the RNG
+
+"""
+Next, let's import the simulator and its important modules: a vehicle, 
+controller, some trajectories, and wind. 
+"""
+
+from rotorpy.environments import Environment
+from rotorpy.vehicles.multirotor import Multirotor
+from rotorpy.vehicles.hummingbird_params import quad_params
+from rotorpy.controllers.quadrotor_control import SE3Control
+from rotorpy.trajectories.hover_traj import HoverTraj
+from rotorpy.trajectories.polynomial_traj import Polynomial
+from rotorpy.wind.dryden_winds import DrydenGust
+
+from rotorpy.utils.postprocessing import unpack_sim_data
+
+"""
+Last for imports we'll import the two filters we want to evaluate.
+These were written prior and are found in rotorpy/estimators/
+"""
+
+from rotorpy.estimators.wind_ukf import WindUKF
+from rotorpy.estimators.wind_ekf import WindEKF
+
+"""
+Next we'll define a useful function. It's a script for automatically 
+fitting a quadratic drag model to the calibration data we'll collect. 
+"""
+
+def auto_system_identification(mass, body_vel, abs_acceleration, plot=False):
+    """
+    AUTOMATIC SYSTEM IDENTIFICATION
+    Inputs:
+        mass, the mass of the UAV, kg
+        body_vel, the velocity of the UAV in the body axes, m/s
+        abs_acceleration, absolute value of accelerometer measurements minus thrust, m/s/s
+        plot, bool to plot accelerometer vs speed. 
+    Outputs:
+        Fitted drag coefficients for a quadratic drag model.
+    """
+    def parabola(x, A):
+        return A*(x**2)
+    
+    # The main assumption is that the only other forces acting on the vehicle are drag. 
+    # We assume the only source of drag is parastic drag, of the form: F_D = -c_D*|V|*V
+
+    Ax, _ = sp.optimize.curve_fit(parabola, body_vel[:,0], abs_acceleration[:,0])
+    Ay, _ = sp.optimize.curve_fit(parabola, body_vel[:,1], abs_acceleration[:,1])
+    Az, _ = sp.optimize.curve_fit(parabola, body_vel[:,2], abs_acceleration[:,2])
+
+    c_Dx = Ax*mass
+    c_Dy = Ay*mass
+    c_Dz = Az*mass
+
+    if plot:
+          (fig, axes) = plt.subplots(nrows=3, ncols=1, sharex=True, num="Fitting Data")
+          ax = axes[0]
+          ax.plot(body_vel[:,0], abs_accel[:,0], 'r.', markersize=6, label="Data")
+          ax.plot(body_vel[:,0], (c_Dx/mass)*(body_vel[:,0]**2), 'k.', markersize=6, label="Quadratic Fit")
+          ax.set_ylabel("|Accel X|")
+          ax = axes[1]
+          ax.plot(body_vel[:,1], abs_accel[:,1], 'g.', markersize=6, label="Data")
+          ax.plot(body_vel[:,1], (c_Dy/mass)*(body_vel[:,1]**2), 'k.', markersize=6, label="Quadratic Fit")
+          ax.set_ylabel("|Accel Y|")
+          ax = axes[2]
+          ax.plot(body_vel[:,2], abs_accel[:,2], 'b.', markersize=6, label="Data")
+          ax.plot(body_vel[:,2], (c_Dz/mass)*(body_vel[:,2]**2), 'k.', markersize=6, label="Quadratic Fit")
+          ax.set_xlabel("Body Velocity, m/s")
+          ax.set_ylabel("|Accel Z|")
+
+    return (c_Dx[0], c_Dy[0], c_Dz[0])
+
+"""
+Next, we'll set up a calibration trajectory. The quadrotor will fly in straight lines
+back and forth on each axis.
+"""
+
+dist= 5
+points = np.array([[0,0,0],
+                   [dist,0,0],
+                   [0,0,0],
+                   [0,dist,0],
+                   [0,0,0],
+                   [0,0,dist],
+                   [0,0,0]])
+calibration_traj = Polynomial(points, v_avg=1)
+
+"""
+Run the Monte Carlo simulations
+"""
+num_trials = 50
+
+# Filter parameters: process noise and initial covariance
+Q = 0.1*np.diag(np.concatenate([0.5*np.ones(3),
+                                0.7*np.ones(3),
+                                0.3*np.ones(3)
+                                ]))
+P0 = 1*np.eye(9)
+
+# Arrays for analysis
+mass = np.zeros((num_trials,))
+cD = np.zeros((num_trials, 3))
+k_d = np.zeros((num_trials,))
+k_z = np.zeros((num_trials,))
+wind_rmse = np.zeros((num_trials, 3))
+
+best_wind_rmse = 10000
+
+for i in range(num_trials):
+    print("Trial %d/%d" % (i+1, num_trials))
+    """
+    Setup: randomly select quadrotor parameters and wind parameters.
+    Instantiate the quadrotor and a stabilizing controller. 
+    """
+    # Randomize the parameters for the quadrotor
+    trial_params = copy.deepcopy(quad_params)  # Get a copy of the quadrotor parameters (this prevents overwriting)
+    trial_params['mass'] *= np.random.uniform(low=0.75, high=1.25)
+    trial_params['c_Dx'] *= np.random.uniform(low=0, high=2)
+    trial_params['c_Dy'] *= np.random.uniform(low=0, high=2)
+    trial_params['c_Dz'] *= np.random.uniform(low=0, high=2)
+    trial_params['k_d'] *= np.random.uniform(low=0, high=10)
+    trial_params['k_z'] *= np.random.uniform(low=0, high=10)
+
+    # Randomize the wind input for this trial
+    wx = np.random.uniform(low=-3, high=3)
+    wy = np.random.uniform(low=-3, high=3)
+    wz = np.random.uniform(low=-3, high=3)
+    sx = np.random.uniform(low=30, high=60)
+    sy = np.random.uniform(low=30, high=60)
+    sz = np.random.uniform(low=30, high=60)
+    wind_profile = DrydenGust(dt=1/100, avg_wind=np.array([wx,wy,wz]), sig_wind=np.array([sx,sy,sz]))
+
+    # Create a vehicle and controller with the ground truth parameters
+    quadrotor = Multirotor(trial_params)
+    se3_controller = SE3Control(trial_params)
+
+    """
+    Generate the calibration data. 
+    """
+    print("Generating calibration data...")
+    # Initialize the quadrotor hovering. 
+    hover_speed = np.sqrt(trial_params['mass']*9.81/(4*trial_params['k_eta']))
+    x0 = calibration_traj.update(0)['x']
+    initial_state = {'x': x0,
+                     '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.ones((4,))*hover_speed}
+    
+    calibration_instance = Environment(vehicle=quadrotor,
+                              controller=se3_controller,
+                              trajectory=calibration_traj,
+                              estimator=WindUKF(trial_params))
+    calibration_instance.vehicle.initial_state = initial_state
+
+    calibration_results = calibration_instance.run(t_final = 30,     # The duration of the environment in seconds
+                                                   plot    = False   # Boolean: plots the vehicle states and commands
+                                          )
+    print("Done.")
+
+    """
+    Unpack the calibration data and do system identification
+    """
+    calibration_data = unpack_sim_data(calibration_results)
+
+    # Extract the orientation and use it to express the velocity in the body frame
+    q = calibration_data[['qx', 'qy', 'qz', 'qw']].to_numpy()
+    vel = (calibration_data[['xdot', 'ydot', 'zdot']].to_numpy())[..., np.newaxis]
+    R = sp.spatial.transform.Rotation.from_quat(q).as_matrix()
+    RT = np.transpose(R, axes=[0,2,1]) 
+    body_vel = (RT@vel)[:,:,0]
+
+    # Extract the acceleration, compute the acceleration minus the commanded thrust to isolate aerodynamic forces
+    accel = calibration_data[['ax', 'ay', 'az']].to_numpy()
+    accel[:,2] -= (calibration_data['thrustdes']/trial_params['mass'])
+    abs_accel = np.abs(accel)
+
+    # Fit aero parameters
+    (fit_c_Dx, fit_c_Dy, fit_c_Dz) = auto_system_identification(trial_params['mass'], body_vel, abs_accel, plot=False)
+    print("Fitted aero parameters.")
+
+    """
+    Set up the estimators. There is an EKF and UKF. You can choose 
+    which one to evaluate when setting up the evaluation instance of the 
+    simulator. 
+    """
+    # Copy the trial parameters. The filtes use the parasitic drag coefficients
+    # for the process model, so use the fitted coefficients from the previous step. 
+    estimator_params = copy.deepcopy(trial_params)
+    estimator_params['c_Dx'] = fit_c_Dx
+    estimator_params['c_Dy'] = fit_c_Dy
+    estimator_params['c_Dz'] = fit_c_Dz
+
+    # Initialize the filters using the mean wind speed. 
+    xhat0 = np.array([0.0, 0.0, 0.0, 0.01, 0.01, 0.01, wx, wy, wz])
+    wind_ukf = WindUKF(estimator_params,Q=Q,xhat0=xhat0,P0=P0,dt=1/100,alpha=1e-3,beta=2,kappa=-1)
+    wind_ekf = WindEKF(estimator_params,Q=Q,xhat0=xhat0,P0=P0,dt=1/100)
+
+    """
+    Run the evaluation. The quadrotor will hover while being subject to 
+    Dryden Gust. You can switch between the UKF and EKF by changing the 
+    'estimator' argument accordingly. 
+    """
+    print("Evaluating the filter...")
+    evaluation_traj = HoverTraj()  
+    evaluation_instance = Environment(vehicle=quadrotor,
+                                      controller=se3_controller,
+                                      trajectory=evaluation_traj,
+                                      wind_profile=wind_profile,
+                                      estimator=wind_ukf)
+
+    evaluation_results = evaluation_instance.run(t_final = 10,       # The maximum duration of the environment in seconds
+                                                 plot = False,
+                                                 verbose= True)
+    
+    """
+    Post process the evaluation results. 
+    """
+    evaluation_data = unpack_sim_data(evaluation_results)
+
+    # Extract the orientation and use it to express the wind in the body frame
+    q = evaluation_data[['qx', 'qy', 'qz', 'qw']].to_numpy()
+    wind = (evaluation_data[['windx', 'windy', 'windz']].to_numpy())[..., np.newaxis]
+    R = sp.spatial.transform.Rotation.from_quat(q).as_matrix()
+    RT = np.transpose(R, axes=[0,2,1]) 
+    body_wind = (RT@wind)[:,:,0]
+
+    # Extract the wind states from the filter. 
+    wind_est = evaluation_data[['xhat_6', 'xhat_7', 'xhat_8']].to_numpy()
+
+    """
+    Compute the RMSE between the wind estimate and the ground truth estimate. 
+    """
+    print("Computing and saving metrics...")
+    # Compute root mean square error
+    rmse = np.sqrt(((wind_est - body_wind)**2).mean(axis=0))
+
+    avg_rmse = np.mean(rmse)
+    if avg_rmse < best_wind_rmse:
+         # Save this instance for plotting
+         plot_body_wind = body_wind
+         plot_wind_est = wind_est
+         plot_time = evaluation_data['time'].to_numpy()
+
+    """
+    Save the vehicle parameters and the filter performance for plotting. 
+    """
+
+    mass[i] = trial_params['mass']
+    cD[i,:] = np.array([trial_params['c_Dx'], trial_params['c_Dy'], trial_params['c_Dz']])
+    k_d[i] = trial_params['k_d']
+    k_z[i] = trial_params['k_z']
+    wind_rmse[i,:] = rmse
+    print("Done.")
+    print("------------------------------------------------------------------")
+
+"""
+Plot the RMSE for each body axis. 
+"""
+
+# Box and Whisker plot
+boxprops = dict(linestyle='-', linewidth=1.5, color='k')
+flierprops = dict(marker='o', markerfacecolor='k', markersize=4,
+                  linestyle='none')
+medianprops = dict(linestyle='-', linewidth=1.5, color='r')
+whiskerprops = dict(linestyle='-', linewidth=1.5, color='k')
+capprops = dict(linestyle='-', linewidth=1.5, color='k')
+
+plt.rcParams["font.family"] = "serif"
+plt.rcParams["figure.autolayout"] = True
+RMSE_data = pd.DataFrame({"X Axis": wind_rmse[:,0], "Y Axis": wind_rmse[:,1], "Z Axis": wind_rmse[:,2]})
+ax = RMSE_data[['X Axis', 'Y Axis', 'Z Axis']].plot(kind='box', title="", 
+                                                    boxprops=boxprops, medianprops=medianprops, flierprops=flierprops, 
+                                                    whiskerprops=whiskerprops, capprops=capprops)
+ax.set_ylabel("Wind RMSE, m/s")
+
+# Plot the best performance of the filter.
+linew = 1.5  # Width of the plot lines
+(fig, axes) = plt.subplots(nrows=3, ncols=1, sharex=True, num="Body Wind Comparison")
+ax = axes[0]
+ax.plot(plot_time, plot_body_wind[:,0], 'rx', linewidth=linew, label="Ground Truth")
+ax.plot(plot_time, plot_wind_est[:,0], 'k', linewidth=linew, label="UKF Estimate")
+ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.2), ncol=2, fancybox=True, shadow=True)
+ax.set_ylabel("$w_x$, m/s")
+ax = axes[1]
+ax.plot(plot_time, plot_body_wind[:,1], 'gx', linewidth=linew, label="Ground Truth")
+ax.plot(plot_time, plot_wind_est[:,1], 'k', linewidth=linew, label="UKF Estimate")
+# ax.legend()
+ax.set_ylabel("$w_y$, m/s")
+ax = axes[2]
+ax.plot(plot_time, plot_body_wind[:,2], 'bx', linewidth=linew, label="Ground Truth")
+ax.plot(plot_time, plot_wind_est[:,2], 'k', linewidth=linew, label="UKF Estimate")
+# ax.legend()
+ax.set_ylabel("$w_z$, m/s")
+ax.set_xlabel("Time, s")
+
+print("Average RMSE: ", np.mean(wind_rmse,axis=0))
+plt.show()

rotorpy/README.md

@@ -0,0 +1,25 @@
+# RotorPy
+
+RotorPy is a simple dynamics simulator for teaching aerial robotics planning and control and preliminary probing of interesting research questions.
+
+It was developed initially to cater to the University of Pennsylvania aerial robotics class, and so it has unique aspirations.
+* Small. Unlike a black box simulator, the engine is intended to be transparent from top to bottom to a student of robotics.
+* Easy to install. Dependencies are minimized to ensure easy native installation on Linux, Windows, and Mac.
+* Pedagogical. Structure and notation mirror course content, and documentation should be precise.
+
+# License
+
+The package rotorpy itself is released under the MIT License. Contributions of improvements, bug fixes, and documentation are welcome. Course instructors would enjoy hearing if you use rotorpy as a template for other projects.
+
+Please note that rotorpy (and its open source license) does not include course materials such as assignments, solutions, and server integrations.
+
+# RotorPy Contributors (nonexhaustive) 
+
+James Paulos (Instructor 2020-2021)
+Dan Mox (TA 2020)
+Rebecca Li (TA 2020)
+Laura Jarin-Lipschitz (TA 2020)
+Austin Chen (TA 2020)
+Spencer Folk (TA 2021)
+Xu Liu (TA 2021)
+Fernando Cladera (TA 2021)

rotorpy/controllers/README.md

@@ -0,0 +1,5 @@
+# Controller Module
+
+The simulator is packaged with a geometric tracking controller for a quadrotor, found in `quadrotor_control.py`. Based on [this paper](https://mathweb.ucsd.edu/~mleok/pdf/LeLeMc2010_quadrotor.pdf) the controller takes flat outputs (position and yaw) and outputs a dictionary containing different control abstractions (angle, rate, motor speeds). The `Multirotor` vehicle will use the commanded motor speeds in the dynamics. 
+
+Other controllers can be developed but must complement the vehicle and the trajectory it is trying to stabilize to. 

rotorpy/controllers/controller_template.py

@@ -0,0 +1,61 @@
+"""
+Imports
+"""
+import numpy as np
+from scipy.spatial.transform import Rotation  # This is a useful library for working with attitude.
+
+class ControlTemplate(object):
+    """
+    The controller is implemented as a class with two required methods: __init__() and update(). 
+    The __init__() is used to instantiate the controller, and this is where any model parameters or 
+    controller gains should be set. 
+    In update(), the current time, state, and desired flat outputs are passed into the controller at 
+    each simulation step. The output of the controller should be the commanded motor speeds, 
+    commanded thrust, commanded moment, and commanded attitude (in quaternion [x,y,z,w] format). 
+    """
+    def __init__(self, vehicle_params):
+        """
+
+        Parameters:
+            vehicle_params, dict with keys specified in a python file under /rotorpy/vehicles/
+
+        """
+
+    def update(self, t, state, flat_output):
+        """
+        This function receives the current time, true state, and desired flat
+        outputs. It returns the command inputs.
+
+        Inputs:
+            t, present time in seconds
+            state, a dict describing the present state with keys
+                x, position, m
+                v, linear velocity, m/s
+                q, quaternion [i,j,k,w]
+                w, angular velocity, rad/s
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+
+        Outputs:
+            control_input, a dict describing the present computed control inputs with keys
+                cmd_motor_speeds, rad/s
+                cmd_thrust, N (for debugging and laboratory; not used by simulator)
+                cmd_moment, N*m (for debugging; not used by simulator)
+                cmd_q, quaternion [i,j,k,w] (for laboratory; not used by simulator)
+        """
+        cmd_motor_speeds = np.zeros((4,))
+        cmd_thrust = 0
+        cmd_moment = np.zeros((3,))
+        cmd_q = np.array([0,0,0,1])
+
+        control_input = {'cmd_motor_speeds':cmd_motor_speeds,
+                         'cmd_thrust':cmd_thrust,
+                         'cmd_moment':cmd_moment,
+                         'cmd_q':cmd_q}
+        return control_input

rotorpy/controllers/quadrotor_control.py

@@ -0,0 +1,153 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+class SE3Control(object):
+    """
+
+    """
+    def __init__(self, quad_params):
+        """
+        Parameters:
+            quad_params, dict with keys specified in rotorpy/vehicles
+        """
+
+        # Quadrotor physical parameters.
+        # Inertial parameters
+        self.mass            = quad_params['mass'] # kg
+        self.Ixx             = quad_params['Ixx']  # kg*m^2
+        self.Iyy             = quad_params['Iyy']  # kg*m^2
+        self.Izz             = quad_params['Izz']  # kg*m^2
+        self.Ixy             = quad_params['Ixy']  # kg*m^2
+        self.Ixz             = quad_params['Ixz']  # kg*m^2
+        self.Iyz             = quad_params['Iyz']  # kg*m^2
+
+        # Frame parameters
+        self.c_Dx            = quad_params['c_Dx']  # drag coeff, N/(m/s)**2
+        self.c_Dy            = quad_params['c_Dy']  # drag coeff, N/(m/s)**2
+        self.c_Dz            = quad_params['c_Dz']  # drag coeff, N/(m/s)**2
+
+        self.num_rotors      = quad_params['num_rotors']
+        self.rotor_pos       = quad_params['rotor_pos']
+
+        # Rotor parameters    
+        self.rotor_speed_min = quad_params['rotor_speed_min'] # rad/s
+        self.rotor_speed_max = quad_params['rotor_speed_max'] # rad/s
+
+        self.k_eta           = quad_params['k_eta']     # thrust coeff, N/(rad/s)**2
+        self.k_m             = quad_params['k_m']       # yaw moment coeff, Nm/(rad/s)**2
+        self.k_d             = quad_params['k_d']       # rotor drag coeff, N/(m/s)
+        self.k_z             = quad_params['k_z']       # induced inflow coeff N/(m/s)
+        self.k_flap          = quad_params['k_flap']    # Flapping moment coefficient Nm/(m/s)
+
+        # Motor parameters
+        self.tau_m           = quad_params['tau_m']     # motor reponse time, seconds
+
+        # You may define any additional constants you like including control gains.
+        self.inertia = np.array([[self.Ixx, self.Ixy, self.Ixz],
+                                 [self.Ixy, self.Iyy, self.Iyz],
+                                 [self.Ixz, self.Iyz, self.Izz]]) # kg*m^2
+        self.g = 9.81 # m/s^2
+
+        # Gains  
+        self.kp_pos = np.array([6.5,6.5,15])
+        self.kd_pos = np.array([4.0, 4.0, 9])
+        self.kp_att = 544
+        self.kd_att = 46.64
+
+        # Linear map from individual rotor forces to scalar thrust and vector
+        # moment applied to the vehicle.
+        k = self.k_m/self.k_eta
+
+        # 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.
+        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]), np.array([k*(-1)**i for i in range(self.num_rotors)]).reshape(1,-1)))
+        self.TM_to_f = np.linalg.inv(self.f_to_TM)
+
+    def update(self, t, state, flat_output):
+        """
+        This function receives the current time, true state, and desired flat
+        outputs. It returns the command inputs.
+
+        Inputs:
+            t, present time in seconds
+            state, a dict describing the present state with keys
+                x, position, m
+                v, linear velocity, m/s
+                q, quaternion [i,j,k,w]
+                w, angular velocity, rad/s
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+
+        Outputs:
+            control_input, a dict describing the present computed control inputs with keys
+                cmd_motor_speeds, rad/s
+                cmd_thrust, N 
+                cmd_moment, N*m
+                cmd_q, quaternion [i,j,k,w]
+        """
+        cmd_motor_speeds = np.zeros((4,))
+        cmd_thrust = 0
+        cmd_moment = np.zeros((3,))
+        cmd_q = np.zeros((4,))
+
+        def normalize(x):
+            """Return normalized vector."""
+            return x / np.linalg.norm(x)
+
+        def vee_map(S):
+            """Return vector corresponding to given skew symmetric matrix."""
+            return np.array([-S[1,2], S[0,2], -S[0,1]])
+
+        # Desired force vector.
+        pos_err  = state['x'] - flat_output['x']
+        dpos_err = state['v'] - flat_output['x_dot']
+        F_des = self.mass * (- self.kp_pos*pos_err
+                             - self.kd_pos*dpos_err
+                             + flat_output['x_ddot']
+                             + np.array([0, 0, self.g]))
+
+        # Desired thrust is force projects onto b3 axis.
+        R = Rotation.from_quat(state['q']).as_matrix()
+        b3 = R @ np.array([0, 0, 1])
+        u1 = np.dot(F_des, b3)
+
+        # Desired orientation to obtain force vector.
+        b3_des = normalize(F_des)
+        yaw_des = flat_output['yaw']
+        c1_des = np.array([np.cos(yaw_des), np.sin(yaw_des), 0])
+        b2_des = normalize(np.cross(b3_des, c1_des))
+        b1_des = np.cross(b2_des, b3_des)
+        R_des = np.stack([b1_des, b2_des, b3_des]).T
+
+        # Orientation error.
+        S_err = 0.5 * (R_des.T @ R - R.T @ R_des)
+        att_err = vee_map(S_err)
+
+        # Angular velocity error (this is oversimplified).
+        w_des = np.array([0, 0, flat_output['yaw_dot']])
+        w_err = state['w'] - w_des
+
+        # Angular control; vector units of N*m.
+        u2 = self.inertia @ (-self.kp_att*att_err - self.kd_att*w_err)
+
+        # Compute motor speeds. Avoid taking square root of negative numbers.
+        TM = np.array([u1, u2[0], u2[1], u2[2]])
+        cmd_motor_forces = self.TM_to_f @ TM
+        cmd_motor_speeds = cmd_motor_forces / self.k_eta
+        cmd_motor_speeds = np.sign(cmd_motor_speeds) * np.sqrt(np.abs(cmd_motor_speeds))
+
+        cmd_thrust = u1
+        cmd_moment = u2
+        cmd_q = Rotation.from_matrix(R_des).as_quat()
+
+        control_input = {'cmd_motor_speeds':cmd_motor_speeds,
+                         'cmd_thrust':cmd_thrust,
+                         'cmd_moment':cmd_moment,
+                         'cmd_q':cmd_q}
+        return control_input

rotorpy/data_out/.gitignore

@@ -0,0 +1,5 @@
+# Never commit output data.
+*.mp4
+*.json
+*.pdf
+*.csv

rotorpy/environments.py

@@ -0,0 +1,178 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+import matplotlib.pyplot as plt
+import time as clk
+
+from rotorpy.simulate import simulate
+from rotorpy.utils.plotter import *
+from rotorpy.world import World
+
+from rotorpy.utils.postprocessing import unpack_sim_data
+
+import os
+
+
+class Environment():
+    """
+    Sandbox represents an instance of the simulation environment containing a unique vehicle, 
+    controller, trajectory generator, wind profile. 
+
+    """
+
+    def __init__(self, vehicle,                 # vehicle object, must be specified. 
+                       controller,              # controller object, must be specified.
+                       trajectory,              # trajectory object, must be specified.
+                       wind_profile = None,     # wind profile object, if none is supplied it will choose no wind. 
+                       imu = None,              # imu sensor object, if none is supplied it will choose a default IMU sensor.
+                       mocap = None,            # mocap sensor object, if none is supplied it will choose a default mocap.  
+                       estimator    = None,     # estimator object
+                       sim_rate     = 100,      # The update frequency of the simulator in Hz
+                       world_fname  = None,     # The world filename, when specified it will load the file located in ../worlds
+                       safety_margin = 0.25,    # The radius of the safety region around the robot. 
+                       ):
+
+        self.sim_rate = sim_rate
+        self.vehicle = vehicle
+        self.controller = controller
+        self.trajectory = trajectory
+
+        self.safety_margin = safety_margin
+
+        if world_fname is None:
+            # If no world JSON file is specified, assume that it means that the intended world is free space.
+            wbound = 3 
+            self.world = World.empty((-wbound, wbound, -wbound, 
+                                       wbound, -wbound, wbound))
+        else:
+            self.world = World.from_file(os.path.abspath(os.path.join(os.path.dirname(__file__),'worlds',world_fname+'.json')))
+
+        if wind_profile is None:
+            # If wind is not specified, default to no wind. 
+            from rotorpy.wind.default_winds import NoWind
+            self.wind_profile = NoWind()
+        else:
+            self.wind_profile = wind_profile
+        
+        if imu is None:
+            # In the event of specified IMU, default to 0 bias with white noise with default parameters as specified below. 
+            from rotorpy.sensors.imu import Imu
+            self.imu = Imu(p_BS = np.zeros(3,),
+                           R_BS = np.eye(3),
+                           sampling_rate=sim_rate)
+        else:
+            self.imu = imu
+
+        if mocap is None:
+            # If no mocap is specified, set a default mocap. 
+            # Default motion capture properties. Pretty much made up based on qualitative comparison with real data from Vicon. 
+            mocap_params = {'pos_noise_density': 0.0005*np.ones((3,)),  # noise density for position 
+                    'vel_noise_density': 0.0010*np.ones((3,)),          # noise density for velocity
+                    'att_noise_density': 0.0005*np.ones((3,)),          # noise density for attitude 
+                    'rate_noise_density': 0.0005*np.ones((3,)),         # noise density for body rates
+                    'vel_artifact_max': 5,                              # maximum magnitude of the artifact in velocity (m/s)
+                    'vel_artifact_prob': 0.001,                         # probability that an artifact will occur for a given velocity measurement
+                    'rate_artifact_max': 1,                             # maximum magnitude of the artifact in body rates (rad/s)
+                    'rate_artifact_prob': 0.0002                        # probability that an artifact will occur for a given rate measurement
+            }
+            from rotorpy.sensors.external_mocap import MotionCapture
+            self.mocap = MotionCapture(sampling_rate=sim_rate, mocap_params=mocap_params, with_artifacts=False)
+        else:
+            self.mocap = mocap
+
+        if estimator is None:
+            # In the likely case where an estimator is not supplied, default to the null state estimator. 
+            from rotorpy.estimators.nullestimator import NullEstimator
+            self.estimator = NullEstimator()
+        else:
+            self.estimator = estimator
+
+        return 
+
+    def run(self,   t_final      = 10,       # The maximum duration of the environment in seconds
+                    use_mocap    = False,    # boolean determines if the controller should use
+                    terminate    = False,
+                    plot            = False,    # 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    = False,    # Boolean: determines if the animation of vehicle state will play. 
+                    verbose         = False,    # Boolean: will print statistics regarding the simulation. 
+                    fname   = None      # Filename is specified if you want to save the animation. Default location is the home directory. 
+                    ):
+
+        """
+        Run the simulator
+        """
+
+        self.t_step = 1/self.sim_rate
+        self.t_final = t_final
+        self.t_final = t_final
+        self.terminate = terminate
+        self.use_mocap = use_mocap
+
+        start_time = clk.time()
+        (time, state, control, flat, imu_measurements, imu_gt, mocap_measurements, state_estimate, exit) = simulate(self.world,
+                                                                                                                    self.vehicle.initial_state,
+                                                                                                                    self.vehicle,
+                                                                                                                    self.controller,
+                                                                                                                    self.trajectory,
+                                                                                                                    self.wind_profile,
+                                                                                                                    self.imu,
+                                                                                                                    self.mocap,
+                                                                                                                    self.estimator,
+                                                                                                                    self.t_final,
+                                                                                                                    self.t_step,
+                                                                                                                    self.safety_margin,
+                                                                                                                    self.use_mocap,
+                                                                                                                    terminate=self.terminate,
+                                                                                                                    )
+        if verbose:
+            # Print relevant statistics or simulator status indicators here
+            print('-------------------RESULTS-----------------------')
+            print('SIM TIME -- %3.2f seconds | WALL TIME -- %3.2f seconds' % (min(self.t_final, time[-1]) , (clk.time()-start_time)))
+            print('EXIT STATUS -- '+exit.value)
+
+        self.result = dict(time=time, state=state, control=control, flat=flat, imu_measurements=imu_measurements, imu_gt=imu_gt, mocap_measurements=mocap_measurements, state_estimate=state_estimate, exit=exit)
+
+        visualizer = Plotter(self.result, self.world)
+        if animate_bool:
+            # Do animation here
+            visualizer.animate_results(fname=fname)
+        if plot:
+            # Do plotting here
+            visualizer.plot_results(plot_mocap=plot_mocap,plot_estimator=plot_estimator,plot_imu=plot_imu)
+            if not animate_bool:
+                plt.show()
+
+        return self.result
+    
+    def save_to_csv(self, fname):
+        """
+        Save the simulation data in self.results to a file. 
+        """
+
+        if self.result is None:
+            print("Error: cannot save if no results have been generated! Aborting save.")
+            return
+        else:
+            if not ".csv" in fname:
+                fname = fname + ".csv"
+            path = os.path.join(os.path.dirname(__file__),'data_out',fname)
+            dataframe = unpack_sim_data(self.result)
+            dataframe.to_csv(path)
+
+
+if __name__=="__main__":
+    from rotorpy.vehicles.crazyflie_params import quad_params
+    from rotorpy.trajectories.hover_traj import HoverTraj
+
+    from rotorpy.vehicles.multirotor import Multirotor
+    from rotorpy.controllers.quadrotor_control import SE3Control
+
+    sim = Environment(vehicle=Multirotor(quad_params),
+                      controller=SE3Control(quad_params),
+                      trajectory=HoverTraj(),
+                      sim_rate=100
+                      )
+    
+    result = sim.run(t_final=1, plot=True)

rotorpy/estimators/README.md

@@ -0,0 +1,13 @@
+# Estimator Module
+
+Estimators take measurements from sensors and an internal model of the dynamcis to provide filtered estimates of states or quantities of interest. 
+
+Currently we provide two examples of estimators that estimate the local wind vector acting on the vehicle center of mass based on measurements from the IMU and external motion capture--one is a hand-implemented EKF and the other is a UKF that uses the `FilterPy` library for easy implementation.
+
+Both models use a simplified model of the dynamics for their process models. The filter states are: $$\boldsymbol{x}_{filter} = [\psi, \theta, \phi, v_x, v_y, v_z, w_x, w_y, w_z]^\top $$ where $[\psi, \theta, \phi]$ are the ZYX Euler angles, $[v_x, v_y, v_z]$ are translational velocities in the body frame, and $[w_x, w_y, w_z]$ are the wind components in the body frame. Small angle approximations near hover are used to simplify the process model. The model assumes the predominant form of drag is parasitic, or quadratic with airspeed. In other words, rotor drag is neglected. These lumped drag coefficients can be identified using flight data in the absence of wind. 
+
+The inputs to the process model are the commanded thrust from the controller, $f_c$, and the body rates measured by the external motion capture sensor, $\Omega$. 
+
+Noisy measurements of acceleration are provided by the IMU sensor, and attitude and body velocities are measured by the external motion capture sensor. 
+
+If you'd like to write your own estimator, whether it is for parameter or state estimation, you can reference `nullestimator.py` for a template. 

rotorpy/estimators/nullestimator.py

@@ -0,0 +1,77 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+import copy
+
+"""
+This is the null estimator, which outputs nothing. This code serves two purposes: 
+1) When the user does not supply an estimator, the null estimator is the default "estimator" for the simulator. 
+2) If a user wants to write their own estimator, the null estimator is good stub code because it specifies the input/output structure used by the simulator. 
+"""
+class NullEstimator:
+    """
+    NullEstimator
+        Description of your filter goes here. 
+
+        State space: 
+            Describe your filter state space here, being as specific as necessary and ideally specifying types, too. 
+                e.g. X = [x,y,z,xdot,ydot,zdot]
+        Measurement space: 
+            Describe your filter measurement space here, being as specific as necessary and ideally specifying types, too. 
+                e.g. Y = [imu_x, imu_y, imu_z]
+    """
+
+    def __init__(self):
+        """
+        Here you would specify important design parameters for your estimator, such as noise matrices, initial state, or other tuning parameters. 
+        For the null estimator, the initial state and covariance are specified (but are default values anyways)
+        """
+        
+    def step(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
+        """
+        The step command will update the filter based on the following. 
+        Inputs:
+            ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors. 
+            controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model. 
+            imu_measurement, contains measurements from an inertial measurement unit. These measurements are noisy, potentially biased, and potentially off-axis. The measurement
+                        is specific acceleration, i.e., total force minus gravity. 
+            mocap_measurement, provides noisy measurements of pose and twist. 
+
+        Outputs:
+            A dictionary with the following keys: 
+                filter_state, containing the current filter estimate.
+                covariance, containing the current covariance matrix.
+
+        The ground truth state is supplied in case you would like to have "knowns" in your filter, or otherwise manipulate the state to create a custom measurement of your own desires. 
+        IMU measurements are commonly used in filter setups, so we already supply these measurements as an input into the system. 
+        Motion capture measurements are useful if you want noisy measurements of the pose and twist of the vehicle. 
+        """
+        self.propagate(ground_truth_state, controller_command)
+        self.update(ground_truth_state, controller_command, imu_measurement, mocap_measurement)
+
+        return {'filter_state': [], 'covariance': []}
+
+    """ OPTIONAL """
+    def propagate(self, ground_truth_state, controller_command):
+        """
+        Propagate occurs whenever an action u is taken.
+        Inputs:
+            ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors. 
+            controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model. 
+        """
+
+        return {'filter_state': [], 'covariance': []}
+
+
+    """ OPTIONAL """
+    def update(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
+        """
+        Update the estimate based on new sensor measurments. 
+        Inputs:
+            ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors. 
+            controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model. 
+            imu_measurement, contains measurements from an inertial measurement unit. These measurements are noisy, potentially biased, and potentially off-axis. The measurement
+                        is specific acceleration, i.e., total force minus gravity. 
+            mocap_measurement, provides noisy measurements of pose and twist. 
+        """
+
+        return {'filter_state': [], 'covariance': []}

rotorpy/estimators/wind_ekf.py

@@ -0,0 +1,239 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+import copy
+
+
+class WindEKF:
+    """
+    Wind EKF: 
+        Given approximate dynamics near level flight, the wind EKF will produce an estimate of the local wind vector acting on the body. 
+        It requires knowledge of the effective drag coefficient on each axis, which would be determined either from real flight or computed in simulation, and the mass of the vehicle. 
+        The inputs to the filter are the mass normalized collective thrust and the body rates on each axis. 
+        Measurements of body velocity, Euler angles, and acceleration are provided to the vehicle. 
+        The filter estimates the Euler angles, body velocities, and wind velocities. 
+
+        State space: 
+            xhat = [phi, theta, psi, u, v, w, windx, windy, windz]
+        Input space: 
+            u = [T/m, p, q, r]
+    """
+
+    def __init__(self, quad_params, Q=np.diag(np.concatenate([0.5*np.ones(3),0.7*np.ones(3),0.1*np.ones(3)])),
+                                    R=np.diag(np.concatenate([0.0005*np.ones(3),0.0010*np.ones(3),np.sqrt(100/2)*(0.38**2)*np.ones(3)])), 
+                                    xhat0=np.array([0,0,0, 0.1,0.05,0.02, 1.5,1.5,1.5]), 
+                                    P0=1*np.eye(9),
+                                    dt=1/100):
+        """
+        Inputs:
+            quad_params, dict with keys specified in quadrotor_params.py
+            Q, the process noise covariance
+            R, the measurement noise covariance
+            x0, the initial filter state
+            P0, the initial state covariance
+            dt, the time between predictions
+        """
+        # Quadrotor physical parameters.
+        # Inertial parameters
+        self.mass            = quad_params['mass'] # kg
+
+        # Frame parameters
+        self.c_Dx            = quad_params['c_Dx']  # drag coeff, N/(m/s)**2
+        self.c_Dy            = quad_params['c_Dy']  # drag coeff, N/(m/s)**2
+        self.c_Dz            = quad_params['c_Dz']  # drag coeff, N/(m/s)**2
+
+        self.g = 9.81 # m/s^2
+
+        # Filter parameters
+        self.Q = Q
+        self.R = R
+        self.xhat = xhat0
+        self.P = P0
+
+        self.innovation = np.zeros((9,))
+
+        self.dt = dt
+
+        # Initialize the Jacobians at starting position and assuming hover thrust.  
+        self.computeJacobians(self.xhat, np.array([self.g, 0, 0, 0]))
+
+    def step(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
+        """
+        This will perform both a propagate and update step in one for the sake of readability in other parts of the code.
+        """
+        self.propagate(ground_truth_state, controller_command)
+        self.update(ground_truth_state, controller_command, imu_measurement, mocap_measurement)
+
+        return self.pack_results()
+
+    def propagate(self, ground_truth_state, controller_command):
+        """
+        Propagate occurs whenever an action u is taken.
+        Inputs:
+            ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors. 
+            controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model. 
+
+        Outputs:
+            xhat, the current state estimate after propagation
+            P, the current covariance matrix after propagation
+        """
+
+        # Extract the appropriate u vector based on the controller commands.
+        uk = self.construct_control_vector(ground_truth_state, controller_command)
+
+        # First propagate the dynamics using the process model
+        self.xhat = self.process_model(self.xhat, uk)
+
+        # Update the covariance matrix using the linearized version of the dynamics
+        self.computeJacobians(self.xhat, uk)
+        self.P = self.A@self.P@(self.A.T) + self.Q
+
+        return self.pack_results
+
+    def update(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
+        """
+        Update the estimate based on new sensor measurments. 
+        Inputs:
+            ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors. 
+            controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model. 
+            imu_measurement, contains measurements from an inertial measurement unit. These measurements are noisy, potentially biased, and potentially off-axis. The measurement
+                        is specific acceleration, i.e., total force minus gravity. 
+            mocap_measurement, provides noisy measurements of pose and twist. 
+        
+        Outputs:
+            xhat, the current state estimate after measurement update
+            P, the current covariance matrix after measurement update
+        """
+
+        # Extract the appropriate u vector based on the controller commands.
+        uk = self.construct_control_vector(ground_truth_state, controller_command)
+
+        # Construct the measurement vector yk
+        orientation = Rotation.from_quat(copy.deepcopy(mocap_measurement['q']))
+        euler_angles = orientation.as_euler('zyx', degrees=False)  # Get Euler angles from current orientation
+        inertial_speed = mocap_measurement['v']
+        body_speed = (orientation.as_matrix()).T@inertial_speed
+        yk = np.array([euler_angles[0],                 # phi
+                       euler_angles[1],                 # theta
+                       euler_angles[2],                 # psi
+                       body_speed[0],                   # vx
+                       body_speed[1],                   # vy
+                       body_speed[2],                   # vz
+                       imu_measurement['accel'][0],     # body x acceleration
+                       imu_measurement['accel'][1],     # body y acceleration
+                       imu_measurement['accel'][2]      # body z acceleration
+                       ])
+        
+        # First linearize the measurement model. 
+        self.computeJacobians(self.xhat, uk)
+
+        # Now compute the Kalman gain
+        K = self.P@(self.C.T)@np.linalg.inv(self.C@self.P@(self.C.T) + self.R)
+
+        # Next compute the posteriori distribution
+        self.innovation = yk - self.measurement_model(self.xhat, uk)
+        self.xhat = self.xhat + K@self.innovation
+        self.P = (np.eye(self.xhat.shape[0]) - K@self.C)@self.P
+
+        return self.pack_results()
+
+    def process_model(self, xk, uk):
+        """
+        Process model
+        """
+
+        va = np.sqrt((xk[3]-xk[6])**2 + (xk[4]-xk[7])**2 + (xk[5]-xk[8])**2)  # Compute the norm of the airspeed vector
+
+        # The process model is integrated using forward Euler. 
+        xdot = np.array([uk[1] + xk[0]*xk[1]*uk[2] + xk[2]*uk[3], 
+                        uk[2] - xk[0]*uk[3], 
+                        xk[0]*uk[2] + uk[3], 
+                        -self.c_Dx/self.mass*(xk[3]-xk[6])*va + self.g*xk[1] + xk[4]*uk[3] - xk[5]*uk[1],
+                        -self.c_Dy/self.mass*(xk[4]-xk[7])*va - self.g*xk[0] + xk[5]*uk[1] - xk[3]*uk[3], 
+                        uk[0] - self.c_Dz/self.mass*(xk[5]-xk[8])*va - self.g + xk[3]*uk[2] - xk[4]*uk[1], 
+                        0,
+                        0,
+                        0])
+
+        xkp1 = xk + xdot*self.dt
+
+        return xkp1
+    
+    def measurement_model(self, xk, uk):
+        """
+        Measurement model
+        """
+
+        h = np.zeros(xk.shape)
+
+        va = np.sqrt((xk[3]-xk[6])**2 + (xk[4]-xk[7])**2 + (xk[5]-xk[8])**2)  # Compute the norm of the airspeed vector
+
+        h[0:3] = np.hstack((np.eye(3), np.zeros((3,6))))@(xk)
+        h[3:6] = np.hstack((np.zeros((3,3)), np.eye(3), np.zeros((3,3))))@(xk)
+
+        h[6:] = np.array([-self.c_Dx/self.mass*(xk[3]-xk[6])*va, 
+                          -self.c_Dy/self.mass*(xk[4]-xk[7])*va,
+                          uk[0]-self.c_Dz/self.mass*(xk[5]-xk[8])*va])
+
+        return h
+
+    def computeJacobians(self, x, u):
+        """
+        Compute the Jacobians of the process and measurement model at the operating points x and u.
+        """
+
+        va = np.sqrt((x[3]-x[6])**2 + (x[4]-x[7])**2 + (x[5]-x[8])**2)  # Compute the norm of the airspeed vector
+
+        # Partial derivatives of va for chain rule
+        dvadu = (x[3]-x[6])/va
+        dvadv = (x[4]-x[7])/va
+        dvadw = (x[5]-x[8])/va
+        dvadwx = -dvadu
+        dvadwy = -dvadv
+        dvadwz = -dvadw
+
+        kx = self.c_Dx/self.mass
+        ky = self.c_Dy/self.mass
+        kz = self.c_Dz/self.mass
+
+        vax = x[3] - x[6]
+        vay = x[4] - x[7]
+        vaz = x[5] - x[8]
+
+        self.A = np.array([[x[1]*u[2], x[0]*u[2] + u[3], 0, 0, 0, 0, 0, 0, 0],
+                           [-u[3], 0, 0, 0, 0, 0, 0, 0, 0],
+                           [ u[2], 0, 0, 0, 0, 0, 0, 0, 0],
+                           [0, self.g, 0, -kx*(dvadu*vax + va), u[3] - kx*(dvadv*vax), -u[1] - kx*(dvadw*vax), -kx*(dvadwx*vax-va), -kx*(dvadwy*vax), -kx*(dvadwz*vax)],
+                           [-self.g, 0, 0, -ky*(dvadu*vay)-u[3], -ky*(dvadv*vay + va), u[1]-ky*(dvadw*vay), -ky*(dvadwx*vay), -ky*(dvadv*vay - va), -ky*(dvadw*vay)],
+                           [0, 0, 0, u[2] - kz*(dvadu*vaz), -u[1] - kz*(dvadv*vaz), -kz*(dvadw*vaz + va), -kz*(dvadwx*vaz), -kz*(dvadwy*vaz), -kz*(dvadwz*vaz - va)],
+                           [0, 0, 0, 0, 0, 0, 0, 0, 0],
+                           [0, 0, 0, 0, 0, 0, 0, 0, 0],
+                           [0, 0, 0, 0, 0, 0, 0, 0, 0]])
+
+        self.C = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0],
+                           [0, 1, 0, 0, 0, 0, 0, 0, 0],
+                           [0, 0, 1, 0, 0, 0, 0, 0, 0],
+                           [0, 0, 0, 1, 0, 0, 0, 0, 0],
+                           [0, 0, 0, 0, 1, 0, 0, 0, 0],
+                           [0, 0, 0, 0, 0, 1, 0, 0, 0],
+                           [0, 0, 0, -kx*(dvadu*vax + va),  -kx*(dvadv*vax), -kx*(dvadw*vax), -kx*(dvadwx*vax-va), -kx*(dvadwy*vax), -kx*(dvadwz*vax)],
+                           [0, 0, 0, -ky*(dvadu*vay), -ky*(dvadv*vay + va), -ky*(dvadw*vay), -ky*(dvadwx*vay), -ky*(dvadv*vay - va), -ky*(dvadw*vay)],
+                           [0, 0, 0, -kz*(dvadu*vaz), -kz*(dvadv*vaz), -kz*(dvadw*vaz + va), -kz*(dvadwx*vaz), -kz*(dvadwy*vaz), -kz*(dvadwz*vaz - va)]])
+
+        return
+
+    def construct_control_vector(self, ground_truth_state, controller_command):
+        """
+        Constructs control vector
+        """
+        uk = np.array([controller_command['cmd_thrust']/self.mass,    # Compute mass normalized thrust from the command thrust, note that this is not the actual thrust...
+                       ground_truth_state['w'][0],                      # Body rate in x axis
+                       ground_truth_state['w'][1],                      # Body rate in y axis
+                       ground_truth_state['w'][2]]                      # Body rate in z axis
+                       )
+
+        return uk
+
+    def pack_results(self):
+        # return {'euler_est': self.xhat[0:3], 'v_est': self.xhat[3:6], 'wind_est': self.xhat[6:9],
+        #          'covariance': self.P, 'innovation': self.innovation}
+        return {'filter_state': self.xhat, 'covariance': self.P}

rotorpy/estimators/wind_ukf.py

@@ -0,0 +1,179 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+import copy
+
+from filterpy.kalman import UnscentedKalmanFilter
+from filterpy.kalman import MerweScaledSigmaPoints
+
+"""
+The Wind UKF uses the same model as the EKF found in wind_ekf.py, but instead applies the Unscented Kalman Filter. The benefit
+of this approach is the accuracy of the UKF is 3rd order (compared to EKF's 1st order), and Jacobians do not need to be computed. 
+"""
+class WindUKF:
+    """
+    WindUKF
+        Given approximate dynamics near level flight, the wind EKF will produce an estimate of the local wind vector acting on the body. 
+        It requires knowledge of the effective drag coefficient on each axis, which would be determined either from real flight or computed in simulation, and the mass of the vehicle. 
+        The inputs to the filter are the mass normalized collective thrust and the body rates on each axis. 
+        Measurements of body velocity, Euler angles, and acceleration are provided to the vehicle. 
+        The filter estimates the Euler angles, body velocities, and wind velocities.
+
+        State space: 
+            xhat = [psi, theta, phi, u, v, w, windx, windy, windz]
+        Measurement space: 
+            u = [T/m, p, q, r]
+    """
+
+    def __init__(self, quad_params,
+                       Q=np.diag(np.concatenate([0.05*np.ones(3),0.07*np.ones(3),0.01*np.ones(3)])),
+                       R=np.diag(np.concatenate([0.0005*np.ones(3),0.0010*np.ones(3),np.sqrt(100/2)*(0.38**2)*np.ones(3)])), 
+                       xhat0=np.array([0,0,0, 0.1,0.05,0.02, 1.5,1.5,1.5]), 
+                       P0=1*np.eye(9),
+                       dt=1/100,
+                       alpha=0.1,
+                       beta=2.0,
+                       kappa=-1):
+        """
+        Inputs:
+            quad_params, dict with keys specified in quadrotor_params.py
+            Q, the process noise covariance
+            R, the measurement noise covariance
+            x0, the initial filter state
+            P0, the initial state covariance
+            dt, the time between predictions
+        """
+
+        self.mass            = quad_params['mass'] # kg
+
+        # Frame parameters
+        self.c_Dx            = quad_params['c_Dx']  # drag coeff, N/(m/s)**2
+        self.c_Dy            = quad_params['c_Dy']  # drag coeff, N/(m/s)**2
+        self.c_Dz            = quad_params['c_Dz']  # drag coeff, N/(m/s)**2
+
+        self.g = 9.81 # m/s^2
+
+        # Filter parameters
+        self.xhat = xhat0
+        self.P = P0
+
+        self.dt = dt
+
+        self.N = self.xhat.shape[0]
+
+        self.points = MerweScaledSigmaPoints(self.N, alpha=alpha, beta=beta, kappa=kappa)
+
+        self.uk = np.array([self.g, 0, 0, 0])
+
+        self.filter = UnscentedKalmanFilter(dim_x=self.N, dim_z=self.N, dt=dt, fx=self.f, hx=self.h, points=self.points)
+        self.filter.x = xhat0
+        self.filter.P = P0
+        self.filter.R = R
+        self.filter.Q = Q
+
+        
+    def step(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
+        """
+        The step command will update the filter based on the following. 
+        Inputs:
+            ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors. 
+            controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model. 
+            imu_measurement, contains measurements from an inertial measurement unit. These measurements are noisy, potentially biased, and potentially off-axis. The measurement
+                        is specific acceleration, i.e., total force minus gravity. 
+            mocap_measurement, provides noisy measurements of pose and twist. 
+
+        Outputs:
+            A dictionary with the following keys: 
+                filter_state, containing the current filter estimate.
+                covariance, containing the current covariance matrix.
+
+        The ground truth state is supplied in case you would like to have "knowns" in your filter, or otherwise manipulate the state to create a custom measurement of your own desires. 
+        IMU measurements are commonly used in filter setups, so we already supply these measurements as an input into the system. 
+        Motion capture measurements are useful if you want noisy measurements of the pose and twist of the vehicle. 
+        """
+
+        # Extract the appropriate u vector based on the controller commands.
+        self.uk = self.construct_control_vector(ground_truth_state, controller_command)
+
+        # Construct the measurement vector yk
+        orientation = Rotation.from_quat(copy.deepcopy(mocap_measurement['q']))
+        euler_angles = orientation.as_euler('zyx', degrees=False)  # Get Euler angles from current orientation
+        inertial_speed = mocap_measurement['v']
+        body_speed = (orientation.as_matrix()).T@inertial_speed
+        zk = np.array([euler_angles[0],                 # phi
+                       euler_angles[1],                 # theta
+                       euler_angles[2],                 # psi
+                       body_speed[0],                   # vx
+                       body_speed[1],                   # vy
+                       body_speed[2],                   # vz
+                       imu_measurement['accel'][0],     # body x acceleration
+                       imu_measurement['accel'][1],     # body y acceleration
+                       imu_measurement['accel'][2]      # body z acceleration
+                       ])
+
+        self.filter.predict()
+        self.filter.update(zk)
+
+        return {'filter_state': self.filter.x, 'covariance': self.filter.P}
+
+    def f(self, xk, dt):
+        """
+        Process model
+        """
+
+        va = np.sqrt((xk[3]-xk[6])**2 + (xk[4]-xk[7])**2 + (xk[5]-xk[8])**2)  # Compute the norm of the airspeed vector
+
+        # The process model is integrated using forward Euler. Below assumes Euler angles are given in order of [phi, theta, psi] (XYZ)
+        # xdot = np.array([self.uk[1] + xk[0]*xk[1]*self.uk[2] + xk[2]*self.uk[3], 
+        #                 self.uk[2] - xk[0]*self.uk[3], 
+        #                 xk[0]*self.uk[2] + self.uk[3], 
+        #                 -self.c_Dx/self.mass*(xk[3]-xk[6])*va + self.g*xk[1] + xk[4]*self.uk[3] - xk[5]*self.uk[1],
+        #                 -self.c_Dy/self.mass*(xk[4]-xk[7])*va - self.g*xk[0] + xk[5]*self.uk[1] - xk[3]*self.uk[3], 
+        #                 self.uk[0] - self.c_Dz/self.mass*(xk[5]-xk[8])*va - self.g + xk[3]*self.uk[2] - xk[4]*self.uk[1], 
+        #                 0,
+        #                 0,
+        #                 0])
+
+        # The process model, below assumes Euler angles are given in the order of [psi, theta, phi] (ZYX)
+        xdot = np.array([xk[2]*self.uk[2] + self.uk[3], 
+                        self.uk[2] - xk[2]*self.uk[3], 
+                        self.uk[1] + xk[2]*xk[1]*self.uk[2] + xk[0]*self.uk[3], 
+                        -self.c_Dx/self.mass*(xk[3]-xk[6])*va + self.g*xk[1] + xk[4]*self.uk[3] - xk[5]*self.uk[1],
+                        -self.c_Dy/self.mass*(xk[4]-xk[7])*va - self.g*xk[2] + xk[5]*self.uk[1] - xk[3]*self.uk[3], 
+                        self.uk[0] - self.c_Dz/self.mass*(xk[5]-xk[8])*va - self.g + xk[3]*self.uk[2] - xk[4]*self.uk[1], 
+                        0,
+                        0,
+                        0])
+
+        xkp1 = xk + xdot*dt
+
+        return xkp1
+
+    def h(self, xk):
+        """
+        Measurement model
+        """
+
+        h = np.zeros(xk.shape)
+
+        va = np.sqrt((xk[3]-xk[6])**2 + (xk[4]-xk[7])**2 + (xk[5]-xk[8])**2)  # Compute the norm of the airspeed vector
+
+        h[0:3] = np.hstack((np.eye(3), np.zeros((3,6))))@(xk)
+        h[3:6] = np.hstack((np.zeros((3,3)), np.eye(3), np.zeros((3,3))))@(xk)
+
+        h[6:] = np.array([-self.c_Dx/self.mass*(xk[3]-xk[6])*va, 
+                          -self.c_Dy/self.mass*(xk[4]-xk[7])*va,
+                          self.uk[0]-self.c_Dz/self.mass*(xk[5]-xk[8])*va])
+
+        return h
+
+    def construct_control_vector(self, ground_truth_state, controller_command):
+        """
+        Constructs control vector
+        """
+        uk = np.array([controller_command['cmd_thrust']/self.mass,    # Compute mass normalized thrust from the command thrust, note that this is not the actual thrust...
+                       ground_truth_state['w'][0],                      # Body rate in x axis
+                       ground_truth_state['w'][1],                      # Body rate in y axis
+                       ground_truth_state['w'][2]]                      # Body rate in z axis
+                       )
+
+        return uk

rotorpy/sensors/README.md

@@ -0,0 +1,11 @@
+# Sensor Module
+
+Sensors are implemented as ways to convert the current ground truth vehicle state (and possibly obstacles and/or control commands) to measurements that reflects information that might be available in the real world. 
+
+The most straightforward examples of sensors are in `imu.py` and `external_mocap.py` which mimic an inertial measurement unit and external motion capture system (e.g. Vicon), respectively. 
+
+The IMU sensor can be arbitrarily placed and oriented with respect to the body axes. Bias and noise intensity can be specified for each sensor axis, in addition to bias drift (diffusion). 
+
+The external motion capture sensor provides noisy measurements of pose and twist. The sensor is placed at the center of mass aligned with the body axes. There is also a parameter that enables "artifacts", which are the consequence of numerical differentiation or glitches in the system causing spikes in the measurements. 
+
+See each sensor file for more information. 

rotorpy/sensors/external_mocap.py

@@ -0,0 +1,202 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+import copy
+
+def hat_map(s):
+        """
+        Given vector s in R^3, return associate skew symmetric matrix S in R^3x3
+        """
+        return np.array([[    0, -s[2],  s[1]],
+                         [ s[2],     0, -s[0]],
+                         [-s[1],  s[0],     0]])
+
+class MotionCapture():
+    """
+    The external motion capture is able to provide pose and twist measurements of the vehicle. 
+    Given the current ground truth state of the vehicle, it will output noisy measurements of the 
+    pose and twist. Artifacts can be introduced 
+    """
+    def __init__(self, sampling_rate, 
+                 mocap_params={'pos_noise_density': 0.0005*np.ones((3,)),  # noise density for position 
+                                                    'vel_noise_density': 0.005*np.ones((3,)),          # noise density for velocity
+                                                    'att_noise_density': 0.0005*np.ones((3,)),          # noise density for attitude 
+                                                    'rate_noise_density': 0.0005*np.ones((3,)),         # noise density for body rates
+                                                    'vel_artifact_max': 5,                              # maximum magnitude of the artifact in velocity (m/s)
+                                                    'vel_artifact_prob': 0.001,                         # probability that an artifact will occur for a given velocity measurement
+                                                    'rate_artifact_max': 1,                             # maximum magnitude of the artifact in body rates (rad/s)
+                                                    'rate_artifact_prob': 0.0002                        # probability that an artifact will occur for a given rate measurement
+                                                    }, 
+                with_artifacts=False):
+        """
+        Parameters: 
+            sampling_rate, Hz, the rate at which this sensor is being sampled. Used for computing the noise.
+            mocap_params, a dict with keys
+                pos_noise_density, position noise density, [m/sqrt(Hz)] 
+                vel_noise_density, velocity noise density, [m/s / sqrt(Hz)]
+                att_noise_density, attitude noise density, [rad / sqrt(Hz)]
+                rate_noise_density, attitude rate noise density, [rad/s /sqrt(Hz)]
+                vel_artifact_prob, probability that a spike will occur for a given velocity measurement
+                vel_artifact_max, the maximum magnitude of the artifact spike. [m/s]
+                rate_artifact_prob, probability that a spike will occur for a given body rate measurement
+                rate_artifact_max, the maximum magnitude of hte artifact spike. [rad/s]
+        """
+
+        self.rate_scale = np.sqrt(sampling_rate/2)
+
+        # Noise densities
+        self.pos_density = mocap_params['pos_noise_density']
+        self.vel_density = mocap_params['vel_noise_density']
+        self.att_density = mocap_params['att_noise_density']
+        self.rate_density = mocap_params['rate_noise_density']
+
+        # Artifacts
+        self.vel_artifact_prob = mocap_params['vel_artifact_prob']
+        self.vel_artifact_max = mocap_params['vel_artifact_max']
+        self.rate_artifact_prob = mocap_params['rate_artifact_prob']
+        self.rate_artifact_max = mocap_params['rate_artifact_max']
+
+        self.initialized = True
+        self.with_artifacts = with_artifacts
+
+    def measurement(self, state, with_noise=False, with_artifacts=False):
+        """
+        Computes and returns the sensor measurement at a time step. 
+        Inputs:
+            state, a dict describing the state with keys
+                    x, position, m, shape=(3,)
+                    v, linear velocity, m/s, shape=(3,)
+                    q, quaternion [i,j,k,w], shape=(4,)
+                    w, angular velocity (in LOCAL frame!), rad/s, shape=(3,)
+            with_noise, a boolean to indicate if noise is added
+            with_artifacts, a boolean to indicate if artifacts are added. 
+                    Artifacts are added to the velocity and angular rates, and are due 
+                    to the numerical differentiation scheme used by motion capture systems. 
+                    They will appear as random spikes in the data. 
+        Outputs:
+            observation, a dictionary with keys
+                    x_m, noisy position measurement, m, shape=(3,)
+                    v_m, noisy linear velocity, m/s, shape=(3,)
+                    q_m, noisy quaternion [i,j,k,w], shape=(4,)
+                    w_m, noisy angular velocity (in LOCAL frame!), rad/s, shape=(3,)
+        """
+        x_measured = copy.deepcopy(state['x']).astype(float)
+        v_measured = copy.deepcopy(state['v']).astype(float)
+        q_measured = Rotation.from_quat(copy.deepcopy(state['q']))
+        w_measured = copy.deepcopy(state['w']).astype(float)
+
+        if with_noise:
+            # Add noise to the measurements based on the provided measurement noise.
+            x_measured += self.rate_scale * np.random.normal(scale=np.abs(self.pos_density))
+            v_measured += self.rate_scale * np.random.normal(scale=np.abs(self.vel_density))
+            w_measured += self.rate_scale * np.random.normal(scale=np.abs(self.rate_density))
+
+            # Noise has to be treated differently with quaternions... 
+            # Following https://www.iri.upc.edu/people/jsola/JoanSola/objectes/notes/kinematics.pdf  pg 43
+            # First, let's produce a perturbation vector in R3
+            delta_phi = self.rate_scale*np.random.normal(scale=np.abs(self.att_density))
+
+            # Now convert that to a rotation matrix
+            delta_rotation = Rotation.from_matrix(np.eye(3) + hat_map(delta_phi))
+
+            # Now apply that rotation to the quaternion
+            q_measured = (q_measured * delta_rotation).as_quat()
+        else:
+            q_measured = q_measured.as_quat()
+
+        if with_artifacts:
+            # If including artifacts, first roll the dice on whether or not a spike should occur for each measurement: 
+            vel_spike_bool = np.random.choice([0,1], p=[1-self.vel_artifact_prob, self.vel_artifact_prob])
+            rate_spike_bool = np.random.choice([0,1], p=[1-self.rate_artifact_prob, self.rate_artifact_prob])
+
+            # Choose the axis that the spike will occur on
+            vel_axis = np.random.choice([0,1,2])
+            rate_axis = np.random.choice([0,1,2])
+
+            # Choose the sign of the spike
+            vel_sign = np.random.choice([-1,1])
+            rate_sign = np.random.choice([-1,1])
+
+            if vel_spike_bool:
+                v_measured[vel_axis] += vel_sign*np.random.uniform(low=0, high=self.vel_artifact_max)
+            if rate_spike_bool:
+                w_measured[rate_axis] += rate_sign*np.random.uniform(low=0, high=self.rate_artifact_max)
+
+        return {'x': x_measured, 'q': q_measured, 'v': v_measured, 'w': w_measured}
+
+if __name__=="__main__":
+
+    import matplotlib.pyplot as plt
+
+    def merge_dicts(dicts_in):
+        """
+        Concatenates contents of a list of N state dicts into a single dict by
+        prepending a new dimension of size N. This is more convenient for plotting
+        and analysis. Requires dicts to have consistent keys and have values that
+        are numpy arrays.
+        """
+        dict_out = {}
+        for k in dicts_in[0].keys():
+            dict_out[k] = []
+            for d in dicts_in:
+                dict_out[k].append(d[k])
+            dict_out[k] = np.array(dict_out[k])
+        return dict_out
+
+    sim_rate = 1/500
+    mocap_params = {'pos_noise_density': 0.0005*np.ones((3,)),  # noise density for position 
+                'vel_noise_density': 0.005*np.ones((3,)),          # noise density for velocity
+                'att_noise_density': 0.0005*np.ones((3,)),          # noise density for attitude 
+                'rate_noise_density': 0.0005*np.ones((3,)),         # noise density for body rates
+                'vel_artifact_max': 5,                              # maximum magnitude of the artifact in velocity (m/s)
+                'vel_artifact_prob': 0.001,                         # probability that an artifact will occur for a given velocity measurement
+                'rate_artifact_max': 1,                             # maximum magnitude of the artifact in body rates (rad/s)
+                'rate_artifact_prob': 0.0002                        # probability that an artifact will occur for a given rate measurement
+                }
+    sensor = MotionCapture(sampling_rate=sim_rate, mocap_params=mocap_params, with_artifacts=True)
+
+    measurements = []
+
+    state = {'x': np.zeros((3,)), 'v': np.zeros((3,)), 'q': np.array([0,0,0,1]), 'w': np.zeros((3,))}
+    for i in range(1000):
+        state = {'x': np.array([np.sin(2*np.pi*i/1000), np.sin(2*np.pi*i/1000 - np.pi/2), np.sin(2*np.pi*i/1000 - np.pi/5)]), 
+                 'v': np.array([np.sin(2*np.pi*i/1000), np.sin(2*np.pi*i/1000 - np.pi/2), np.sin(2*np.pi*i/1000 - np.pi/5)]), 
+                 'q': np.array([0,0,0,1]), 
+                 'w': np.array([np.sin(2*np.pi*i/1000), np.sin(2*np.pi*i/1000 - np.pi/2), np.sin(2*np.pi*i/1000 - np.pi/5)])}
+        current = sensor.measurement(state, with_noise=True, with_artifacts=True)
+        measurements.append(current)
+
+    measurements = merge_dicts(measurements)
+
+    x_m = measurements['x']
+    v_m = measurements['v']
+    q_m = measurements['q']
+    w_m = measurements['w']
+
+    q_norm = np.linalg.norm(q_m, axis=1)
+    
+    (fig, axes) = plt.subplots(nrows=4, ncols=1, sharex=True, num="Measurements")
+    fig.set_figwidth(9)
+    fig.set_figheight(9)
+    axe = axes[0]
+    axe.plot(x_m[:,0], 'r', markersize=2)
+    axe.plot(x_m[:,1], 'g', markersize=2)
+    axe.plot(x_m[:,2], 'b', markersize=2)
+    axe.set_ylim(bottom=-1.5, top=1.5)
+    axe = axes[1]
+    axe.plot(v_m[:,0], 'r', markersize=2)
+    axe.plot(v_m[:,1], 'g', markersize=2)
+    axe.plot(v_m[:,2], 'b', markersize=2)
+    axe.set_ylim(bottom=-1.5, top=1.5)
+    axe = axes[2]
+    axe.plot(q_m[:,0], 'r', markersize=2)
+    axe.plot(q_m[:,1], 'g', markersize=2)
+    axe.plot(q_m[:,2], 'b', markersize=2)
+    axe.plot(q_m[:,3], 'm', markersize=2)
+    axe.plot(q_norm, 'k', markersize=2)
+    axe = axes[3]
+    axe.plot(w_m[:,0], 'r', markersize=2)
+    axe.plot(w_m[:,1], 'g', markersize=2)
+    axe.plot(w_m[:,2], 'b', markersize=2)
+    axe.set_ylim(bottom=-1.5, top=1.5)
+
+    plt.show()

rotorpy/sensors/imu.py

@@ -0,0 +1,147 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+import copy
+
+class Imu:
+    """
+    Simulated IMU measurement given
+      1) quadrotor's ground truth state and acceleration, and
+      2) IMU's pose in quadrotor body frame.
+
+      CREDIT:
+      Partial implementation from Yu-Ming Chen and MEAM 620 Sp2022 Teaching Staff
+      Finishing touches added by Alexander Spinos, checked by Spencer Folk.
+    """
+    def __init__(self, accelerometer_params={'initial_bias': np.array([0.,0.,0.]), # m/s^2
+                                             'noise_density': (0.38**2)*np.ones(3,), # m/s^2 / sqrt(Hz)
+                                             'random_walk': np.zeros(3,) # m/s^2 * sqrt(Hz)
+                                             }, 
+                gyroscope_params={'initial_bias': np.array([0.,0.,0.]), # m/s^2
+                                  'noise_density': (0.01**2)*np.ones(3,), # rad/s / sqrt(Hz)
+                                  'random_walk': np.zeros(3,) # rad/s * sqrt(Hz)
+                                  }, 
+                R_BS = np.eye(3),
+                p_BS = np.zeros(3,),
+                sampling_rate=500, 
+                gravity_vector=np.array([0,0,-9.81])):
+        """
+        Parameters:
+            R_BS, the rotation matrix of sensor frame S in body frame B
+            p_BS, the position vector from frame B's origin to frame S's origin, expressed in frame B
+            accelerometer_params, a dict with keys:
+                initial_bias, accelerometer contant bias,    [ m / s^2 ]
+                noise_density, accelerometer "white noise",  [ m / s^2 / sqrt(Hz) ]
+                random_walk, accelerometer bias diffusion,   [ m / s^2 * sqrt(Hz) ]
+            gyroscope_params, a dict with keys:
+                initial_bias, gyro contant bias,             [ m / s^2 ]
+                noise_density, gyro "white noise",           [ rad / s / sqrt(Hz) ]
+                random_walk, gyro bias diffusion,            [ rad / s * sqrt(Hz) ]
+            sampling_rate, the sampling rate of the sensor, Hz (1/s)
+            gravity_vector, the gravitational vector in world frame (should be ~ [0, 0 , -9.81])
+        """
+
+        # A few checks
+        if type(R_BS) != np.ndarray:
+            raise TypeError("R_BS's type is not numpy.ndarray")
+        if type(p_BS) != np.ndarray:
+            raise TypeError("p_BS's type is not numpy.ndarray")
+        if type(gravity_vector) != np.ndarray:
+            raise TypeError("gravity_vector's type is not numpy.ndarray")
+        if R_BS.shape != (3, 3):
+            raise ValueError("R_BS's size is not (3, 3)")
+        if p_BS.shape != (3,):
+            raise ValueError("p_BS's size is not (3,)")
+        if gravity_vector.shape != (3,):
+            raise ValueError("gravity_vector's size is not (3,)")
+
+        self.R_BS = R_BS
+        self.p_BS = p_BS
+        self.rate_scale = np.sqrt(sampling_rate/2)
+        self.gravity_vector = gravity_vector
+
+        self.accel_variance = accelerometer_params['noise_density'].astype('float64')
+        self.accel_random_walk = accelerometer_params['random_walk'].astype('float64')
+        self.accel_bias = accelerometer_params['initial_bias'].astype('float64')
+        self.gyro_variance = gyroscope_params['noise_density'].astype('float64')
+        self.gyro_random_walk = gyroscope_params['random_walk'].astype('float64')
+        self.gyro_bias = gyroscope_params['initial_bias'].astype('float64')
+
+    def bias_step(self):
+        """Simulate bias drift"""
+
+        self.accel_bias += np.random.normal(scale=self.accel_random_walk) / self.rate_scale
+        self.gyro_bias += np.random.normal(scale=self.gyro_random_walk) / self.rate_scale
+
+        return
+
+    def measurement(self, state, acceleration, with_noise=True):
+        """
+        Computes and returns the IMU measurement at a time step.
+
+        Inputs:
+            state, a dict describing the state with keys
+                x, position, m, shape=(3,)
+                v, linear velocity, m/s, shape=(3,)
+                q, quaternion [i,j,k,w], shape=(4,)
+                w, angular velocity (in LOCAL frame!), rad/s, shape=(3,)
+            acceleration, a dict describing the acceleration with keys
+                vdot, quadrotor's linear acceleration expressed in world frame, m/s^2, shape=(3,)
+                wdot, quadrotor's angular acceleration expressed in world frame, rad/s^2, shape=(3,)
+        Outputs:
+            observation, a dict describing the IMU measurement with keys
+                accel, simulated accelerometer measurement, m/s^2, shape=(3,)
+                gyro, simulated gyroscope measurement, rad/s^2, shape=(3,)
+        """
+        q_WB = state['q']
+        w_WB = state['w']
+        alpha_WB_W = acceleration['wdot']
+        a_WB_W = acceleration['vdot']
+
+        # Rotation matrix of the body frame B in world frame W
+        R_WB = Rotation.from_quat(q_WB).as_matrix()
+
+        # Sensor position in body frame expressed in world coordinates
+        p_BS_W = R_WB @ self.p_BS
+
+        # Linear acceleration of point S (the imu) expressed in world coordinates W.
+        a_WS_W = a_WB_W + np.cross(alpha_WB_W, p_BS_W) + np.cross(w_WB, np.cross(w_WB, p_BS_W))
+
+        # Rotation from world to imu: R_SW = R_SB * R_BW
+        R_SW = self.R_BS.T @ R_WB.T
+
+        # Rotate to local frame
+        accelerometer_measurement = R_SW @ (a_WS_W - self.gravity_vector)
+        gyroscope_measurement = copy.deepcopy(w_WB).astype(float)
+
+        # Add the bias drift (default 0)
+        self.bias_step()
+
+        # Add biases and noises
+        accelerometer_measurement += self.accel_bias
+        gyroscope_measurement += self.gyro_bias
+        if with_noise:
+            accelerometer_measurement += self.rate_scale * np.random.normal(scale=np.abs(self.accel_variance))
+            gyroscope_measurement +=  self.rate_scale * np.random.normal(scale=np.abs(self.gyro_variance))
+
+        return {'accel': accelerometer_measurement, 'gyro': gyroscope_measurement}
+
+if __name__=="__main__":
+    # The default 
+    sim_rate = 100
+    accelerometer_params = {'initial_bias': np.array([0,0,0]), # m/s^2
+                                    'noise_density': (0.38**2)*np.ones(3,), # m/s^2 / sqrt(Hz)
+                                    'random_walk': np.zeros(3,) # m/s^2 * sqrt(Hz)
+                                    }
+    gyroscope_params     = {'initial_bias': np.array([0,0,0]), # m/s^2
+                            'noise_density': (0.01**2)*np.ones(3,), # rad/s / sqrt(Hz)
+                            'random_walk': np.zeros(3,) # rad/s * sqrt(Hz)
+                            }
+
+    imu = Imu(accelerometer_params,
+                gyroscope_params,
+                p_BS = np.zeros(3,),
+                R_BS = np.eye(3),
+                sampling_rate=sim_rate)
+
+    print(imu.measurement({'x': np.array([0,0,0]), 'v': np.array([0,0,0]), 'q': np.array([0,0,0,1]), 'w': np.array([0,0,0])}, 
+                    {'vdot': np.array([0,0,0]), 'wdot': np.array([0,0,0])}))

rotorpy/simulate.py

@@ -0,0 +1,236 @@
+from enum import Enum
+import copy
+import numpy as np
+from numpy.linalg import norm
+from scipy.spatial.transform import Rotation
+
+class ExitStatus(Enum):
+    """ Exit status values indicate the reason for simulation termination. """
+    COMPLETE     = 'Success: End reached.'
+    TIMEOUT      = 'Timeout: Simulation end time reached.'
+    INF_VALUE    = 'Failure: Your controller returned inf motor speeds.'
+    NAN_VALUE    = 'Failure: Your controller returned nan motor speeds.'
+    OVER_SPEED   = 'Failure: Your quadrotor is out of control; it is going faster than 100 m/s. The Guinness World Speed Record is 73 m/s.'
+    OVER_SPIN    = 'Failure: Your quadrotor is out of control; it is spinning faster than 100 rad/s. The onboard IMU can only measure up to 52 rad/s (3000 deg/s).'
+    FLY_AWAY     = 'Failure: Your quadrotor is out of control; it flew away with a position error greater than 20 meters.'
+    COLLISION    = 'Failure: Your quadrotor collided with an object.'
+
+def simulate(world, initial_state, vehicle, controller, trajectory, wind_profile, imu, mocap, estimator, t_final, t_step, safety_margin, use_mocap, terminate=None):
+    """
+    Perform a vehicle simulation and return the numerical results.
+
+    Inputs:
+        world, a class representing the world it is flying in, including objects and world bounds. 
+        initial_state, a dict defining the vehicle initial conditions with appropriate keys
+        vehicle, Vehicle object containing the dynamics
+        controller, Controller object containing the controller
+        trajectory, Trajectory object containing the trajectory to follow
+        wind_profile, Wind Profile object containing the wind generator. 
+        t_final, maximum duration of simulation, s
+        t_step, the time between each step in the simulator, s
+        safety_margin, the radius of the ball surrounding the vehicle position to determine if a collision occurs
+        imu, IMU object that generates accelerometer and gyroscope readings from the vehicle state
+        terminate, None, False, or a function of time and state that returns
+            ExitStatus. If None (default), terminate when hover is reached at
+            the location of trajectory with t=inf. If False, never terminate
+            before timeout or error. If a function, terminate when returns not
+            None.
+        mocap, a MotionCapture object that provides noisy measurements of pose and twist with artifacts. 
+        use_mocap, a boolean to determine in noisy measurements from mocap should be used for quadrotor control
+        estimator, an estimator object that provides estimates of a portion or all of the vehicle state.
+
+    Outputs:
+        time, seconds, shape=(N,)
+        state, a dict describing the state history with keys
+            x, position, m, shape=(N,3)
+            v, linear velocity, m/s, shape=(N,3)
+            q, quaternion [i,j,k,w], shape=(N,4)
+            w, angular velocity, rad/s, shape=(N,3)
+            rotor_speeds, motor speeds, rad/s, shape=(N,n) where n is the number of rotors
+            wind, wind velocity, m/s, shape=(N,3)
+        control, a dict describing the command input history with keys
+            cmd_motor_speeds, motor speeds, rad/s, shape=(N,4)
+            cmd_q, commanded orientation (not used by simulator), quaternion [i,j,k,w], shape=(N,4)
+            cmd_w, commanded angular velocity (not used by simulator), rad/s, shape=(N,3)
+        flat, a dict describing the desired flat outputs from the trajectory with keys
+            x,        position, m
+            x_dot,    velocity, m/s
+            x_ddot,   acceleration, m/s**2
+            x_dddot,  jerk, m/s**3
+            x_ddddot, snap, m/s**4
+            yaw,      yaw angle, rad
+            yaw_dot,  yaw rate, rad/s
+        imu_measurements, a dict containing the biased and noisy measurements from an accelerometer and gyroscope
+            accel,  accelerometer, m/s**2
+            gyro,   gyroscope, rad/s
+        imu_gt, a dict containing the ground truth (no noise, no bias) measurements from an accelerometer and gyroscope
+            accel,  accelerometer, m/s**2
+            gyro,   gyroscope, rad/s
+        mocap_measurements, a dict containing noisy measurements of pose and twist for the vehicle. 
+            x, position (inertial)
+            v, velocity (inertial)
+            q, orientation of body w.r.t. inertial frame.
+            w, body rates in the body frame. 
+        exit_status, an ExitStatus enum indicating the reason for termination.
+    """
+
+    # Coerce entries of initial state into numpy arrays, if they are not already.
+    initial_state = {k: np.array(v) for k, v in initial_state.items()}
+
+    if terminate is None:    # Default exit. Terminate at final position of trajectory.
+        normal_exit = traj_end_exit(initial_state, trajectory, using_vio = False)
+    elif terminate is False: # Never exit before timeout.
+        normal_exit = lambda t, s: None
+    else:                    # Custom exit.
+        normal_exit = terminate
+
+    time    = [0]
+    state   = [copy.deepcopy(initial_state)]
+    state[0]['wind'] = wind_profile.update(0, state[0]['x'])   # TODO: move this line elsewhere so that other objects that don't have wind as a state can work here. 
+    imu_measurements = []
+    mocap_measurements = []
+    imu_gt = []
+    state_estimate = []
+    flat    = [sanitize_trajectory_dic(trajectory.update(time[-1]))]
+    mocap_measurements.append(mocap.measurement(state[-1], with_noise=True, with_artifacts=False))
+    if use_mocap:
+        # In this case the controller will use the motion capture estimate of the pose and twist for control. 
+        control = [sanitize_control_dic(controller.update(time[-1], mocap_measurements[-1], flat[-1]))]
+    else:
+        control = [sanitize_control_dic(controller.update(time[-1], state[-1], flat[-1]))]
+    state_dot =  vehicle.statedot(state[0], control[0]['cmd_motor_speeds'], t_step)
+    imu_measurements.append(imu.measurement(state[-1], state_dot, with_noise=True))
+    imu_gt.append(imu.measurement(state[-1], state_dot, with_noise=False))
+    state_estimate.append(estimator.step(state[0], control[0], imu_measurements[0], mocap_measurements[0]))
+
+    exit_status = None
+
+    while True:
+        exit_status = exit_status or safety_exit(world, safety_margin, state[-1], flat[-1], control[-1])
+        exit_status = exit_status or normal_exit(time[-1], state[-1])
+        exit_status = exit_status or time_exit(time[-1], t_final)
+        if exit_status:
+            break
+        time.append(time[-1] + t_step)
+        state[-1]['wind'] = wind_profile.update(time[-1], state[-1]['x'])
+        state.append(vehicle.step(state[-1], control[-1]['cmd_motor_speeds'], t_step))
+        flat.append(sanitize_trajectory_dic(trajectory.update(time[-1])))
+        mocap_measurements.append(mocap.measurement(state[-1], with_noise=True, with_artifacts=mocap.with_artifacts))
+        state_estimate.append(estimator.step(state[-1], control[-1], imu_measurements[-1], mocap_measurements[-1]))
+        if use_mocap:
+            control.append(sanitize_control_dic(controller.update(time[-1], mocap_measurements[-1], flat[-1])))
+        else:
+            control.append(sanitize_control_dic(controller.update(time[-1], state[-1], flat[-1])))
+        state_dot = vehicle.statedot(state[-1], control[-1]['cmd_motor_speeds'], t_step)
+        imu_measurements.append(imu.measurement(state[-1], state_dot, with_noise=True))
+        imu_gt.append(imu.measurement(state[-1], state_dot, with_noise=False))
+
+    time    = np.array(time, dtype=float)    
+    state   = merge_dicts(state)
+    imu_measurements = merge_dicts(imu_measurements)
+    imu_gt = merge_dicts(imu_gt)
+    mocap_measurements = merge_dicts(mocap_measurements)
+    control         = merge_dicts(control)
+    flat            = merge_dicts(flat)
+    state_estimate  = merge_dicts(state_estimate)
+
+    return (time, state, control, flat, imu_measurements, imu_gt, mocap_measurements, state_estimate, exit_status)
+
+def merge_dicts(dicts_in):
+    """
+    Concatenates contents of a list of N state dicts into a single dict by
+    prepending a new dimension of size N. This is more convenient for plotting
+    and analysis. Requires dicts to have consistent keys and have values that
+    are numpy arrays.
+    """
+    dict_out = {}
+    for k in dicts_in[0].keys():
+        dict_out[k] = []
+        for d in dicts_in:
+            dict_out[k].append(d[k])
+        dict_out[k] = np.array(dict_out[k])
+    return dict_out
+
+
+def traj_end_exit(initial_state, trajectory, using_vio = False):
+    """
+    Returns a exit function. The exit function returns an exit status message if
+    the quadrotor is near hover at the end of the provided trajectory. If the
+    initial state is already at the end of the trajectory, the simulation will
+    run for at least one second before testing again.
+    """
+
+    xf = trajectory.update(np.inf)['x']
+    yawf = trajectory.update(np.inf)['yaw']
+    rotf = Rotation.from_rotvec(yawf * np.array([0, 0, 1])) # create rotation object that describes yaw
+    if np.array_equal(initial_state['x'], xf):
+        min_time = 1.0
+    else:
+        min_time = 0
+
+    def exit_fn(time, state):
+        cur_attitude = Rotation.from_quat(state['q'])
+        err_attitude = rotf * cur_attitude.inv() # Rotation between current and final
+        angle = norm(err_attitude.as_rotvec()) # angle in radians from vertical
+        # Success is reaching near-zero speed with near-zero position error.
+        if using_vio:
+            # set larger threshold for VIO due to noisy measurements
+            if time >= min_time and norm(state['x'] - xf) < 1 and norm(state['v']) <= 1 and angle <= 1:
+                return ExitStatus.COMPLETE
+        else:
+            if time >= min_time and norm(state['x'] - xf) < 0.02 and norm(state['v']) <= 0.03 and angle <= 0.02:
+                return ExitStatus.COMPLETE
+        return None
+    return exit_fn
+
+def time_exit(time, t_final):
+    """
+    Return exit status if the time exceeds t_final, otherwise None.
+    """
+    if time >= t_final:
+        return ExitStatus.TIMEOUT
+    return None
+
+def safety_exit(world, margin, state, flat, control):
+    """
+    Return exit status if any safety condition is violated, otherwise None.
+    """
+    if np.any(np.isinf(control['cmd_motor_speeds'])):
+        return ExitStatus.INF_VALUE
+    if np.any(np.isnan(control['cmd_motor_speeds'])):
+        return ExitStatus.NAN_VALUE
+    if np.any(np.abs(state['v']) > 100):
+        return ExitStatus.OVER_SPEED
+    if np.any(np.abs(state['w']) > 100):
+        return ExitStatus.OVER_SPIN
+    if np.any(np.abs(state['x'] - flat['x']) > 20):
+        return ExitStatus.FLY_AWAY
+
+    if len(world.world.get('blocks', [])) > 0:
+        # If a world has objects in it we need to check for collisions.  
+        collision_pts = world.path_collisions(state['x'], margin)
+        no_collision = collision_pts.size == 0
+        if not no_collision:
+            return ExitStatus.COLLISION
+    return None
+
+def sanitize_control_dic(control_dic):
+    """
+    Return a sanitized version of the control dictionary where all of the elements are np arrays
+    """
+    control_dic['cmd_motor_speeds'] = np.asarray(control_dic['cmd_motor_speeds'], np.float64).ravel()
+    control_dic['cmd_moment'] = np.asarray(control_dic['cmd_moment'], np.float64).ravel()
+    control_dic['cmd_q'] = np.asarray(control_dic['cmd_q'], np.float64).ravel()
+    return control_dic
+
+def sanitize_trajectory_dic(trajectory_dic):
+    """
+    Return a sanitized version of the trajectory dictionary where all of the elements are np arrays
+    """
+    trajectory_dic['x'] = np.asarray(trajectory_dic['x'], np.float64).ravel()
+    trajectory_dic['x_dot'] = np.asarray(trajectory_dic['x_dot'], np.float64).ravel()
+    trajectory_dic['x_ddot'] = np.asarray(trajectory_dic['x_ddot'], np.float64).ravel()
+    trajectory_dic['x_dddot'] = np.asarray(trajectory_dic['x_dddot'], np.float64).ravel()
+    trajectory_dic['x_ddddot'] = np.asarray(trajectory_dic['x_ddddot'], np.float64).ravel()
+
+    return trajectory_dic

rotorpy/trajectories/README.md

@@ -0,0 +1,7 @@
+# Trajectory Module
+
+Trajectories define the desired motion of the multirotor in time. There are a few options for trajectory generation. `minsnap.py` is an example of trajectory generators based on waypoints like in [Minimum Snap Trajectory Generation and Control for Quadrotors](https://ieeexplore.ieee.org/document/5980409), while the others are examples of time-parameterized shapes. 
+
+If you'd like to make your own trajectory you can reference `traj_template.py` to see the required structure. 
+
+Currently only trajectories that output flat outputs (position and yaw) are implemented to complement the SE3 controller, but you can develop your own trajectories that can be paired with different hierarchies of controllers. 

rotorpy/trajectories/circular_traj.py

@@ -0,0 +1,189 @@
+import numpy as np
+import sys
+
+class CircularTraj(object):
+    """
+    A circle. 
+    """
+    def __init__(self, center=np.array([0,0,0]), radius=1, freq=0.2, yaw_bool=False, plane='XY', direction='CCW'):
+        """
+        This is the constructor for the Trajectory object. A fresh trajectory
+        object will be constructed before each mission.
+
+        Inputs:
+            center, the center of the circle (m)
+            radius, the radius of the circle (m)
+            freq, the frequency with which a circle is completed (Hz)
+            yaw_bool, determines if yaw motion is desired
+            plane, the plane with which the circle lies on, 'XY', 'YZ', or 'XZ' 
+            direcition, the direction of the circle, 'CCW' or 'CW'
+        """
+
+        # Check and assign inputs
+        if plane == "XY" or plane == "YZ" or plane == "XZ":
+            self.plane = plane
+        else:
+            print("CircularTraj Error: incorrect specification of plane. Must be 'XY', 'YZ', or 'XZ' ")
+            sys.exit(1)
+
+        if direction == "CW" or direction == "CCW":
+            if direction == "CW":
+                self.sign = -1
+            else:
+                self.sign = 1
+        else:
+            print("CircularTraj Error: incorrect specification of direction. Must be 'CW' or 'CCW' ")
+            sys.exit(1)
+
+        self.center = center
+        self.cx, self.cy, self.cz = center[0], center[1], center[2]
+        self.radius = radius
+        self.freq = freq
+
+        self.omega = 2*np.pi*self.freq
+        self.yaw_bool = yaw_bool
+
+
+    def update(self, t):
+        """
+        Given the present time, return the desired flat output and derivatives.
+
+        Inputs
+            t, time, s
+        Outputs
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+        """
+
+        if self.plane == "XY":
+            x        = np.array([self.cx + self.radius*np.cos(self.sign*self.omega*t),
+                                self.cy + self.radius*np.sin(self.sign*self.omega*t),
+                                self.cz])
+            x_dot    = np.array([-self.radius*self.sign*self.omega*np.sin(self.sign*self.omega*t),
+                                self.radius*self.sign*self.omega*np.cos(self.sign*self.omega*t),
+                                0])
+            x_ddot   = np.array([-self.radius*((self.sign*self.omega)**2)*np.cos(self.sign*self.omega*t),
+                                -self.radius*((self.sign*self.omega)**2)*np.sin(self.sign*self.omega*t),
+                                0])
+            x_dddot  = np.array([self.radius*((self.sign*self.omega)**3)*np.sin(self.sign*self.omega*t),
+                                -self.radius*((self.sign*self.omega)**3)*np.cos(self.sign*self.omega*t),
+                                0])
+            x_ddddot = np.array([self.radius*((self.sign*self.omega)**4)*np.cos(self.sign*self.omega*t),
+                                self.radius*((self.sign*self.omega)**4)*np.sin(self.sign*self.omega*t),
+                                0])
+        elif self.plane == "YZ":
+            x        = np.array([self.cx,
+                                self.cy + self.radius*np.cos(self.sign*self.omega*t),
+                                self.cz + self.radius*np.sin(self.sign*self.omega*t)])
+            x_dot    = np.array([0,
+                                -self.radius*self.sign*self.omega*np.sin(self.sign*self.omega*t),
+                                self.radius*self.sign*self.omega*np.cos(self.sign*self.omega*t)])
+            x_ddot   = np.array([0,
+                                -self.radius*((self.sign*self.omega)**2)*np.cos(self.sign*self.omega*t),
+                                -self.radius*((self.sign*self.omega)**2)*np.sin(self.sign*self.omega*t)])
+            x_dddot  = np.array([0,
+                                self.radius*((self.sign*self.omega)**3)*np.sin(self.sign*self.omega*t),
+                                -self.radius*((self.sign*self.omega)**3)*np.cos(self.sign*self.omega*t)])
+            x_ddddot = np.array([0,
+                                self.radius*((self.sign*self.omega)**4)*np.cos(self.sign*self.omega*t),
+                                self.radius*((self.sign*self.omega)**4)*np.sin(self.sign*self.omega*t)])
+        elif self.plane == "XZ":
+            x        = np.array([self.cx + self.radius*np.cos(self.sign*self.omega*t),
+                                self.cy,
+                                self.cz + self.radius*np.sin(self.sign*self.omega*t)])
+            x_dot    = np.array([-self.radius*self.sign*self.omega*np.sin(self.sign*self.omega*t),
+                                0,
+                                self.radius*self.sign*self.omega*np.cos(self.sign*self.omega*t)])
+            x_ddot   = np.array([-self.radius*((self.sign*self.omega)**2)*np.cos(self.sign*self.omega*t),
+                                0,
+                                -self.radius*((self.sign*self.omega)**2)*np.sin(self.sign*self.omega*t)])
+            x_dddot  = np.array([self.radius*((self.sign*self.omega)**3)*np.sin(self.omega*t),
+                                0,
+                                -self.radius*((self.sign*self.omega)**3)*np.cos(self.sign*self.omega*t)])
+            x_ddddot = np.array([self.radius*((self.sign*self.omega)**4)*np.cos(self.sign*self.omega*t),
+                                0,
+                                self.radius*((self.sign*self.omega)**4)*np.sin(self.sign*self.omega*t)])
+
+        if self.yaw_bool:
+            yaw = np.pi/4*np.sin(np.pi*t)
+            yaw_dot = np.pi*np.pi/4*np.cos(np.pi*t)
+        else:
+            yaw = 0
+            yaw_dot = 0
+
+        flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
+                        'yaw':yaw, 'yaw_dot':yaw_dot}
+        return flat_output
+
+class ThreeDCircularTraj(object):
+    """
+
+    """
+    def __init__(self, center=np.array([0,0,0]), radius=np.array([1,1,1]), freq=np.array([0.2,0.2,0.2]), yaw_bool=False):
+        """
+        This is the constructor for the Trajectory object. A fresh trajectory
+        object will be constructed before each mission.
+
+        Inputs:
+            center, the center of the circle (m)
+            radius, the radius of the circle (m)
+            freq, the frequency with which a circle is completed (Hz)
+        """
+
+        self.center = center
+        self.cx, self.cy, self.cz = center[0], center[1], center[2]
+        self.radius = radius
+        self.freq = freq
+
+        self.omega = 2*np.pi*self.freq
+
+        self.yaw_bool = yaw_bool
+
+    def update(self, t):
+        """
+        Given the present time, return the desired flat output and derivatives.
+
+        Inputs
+            t, time, s
+        Outputs
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+        """
+        x        = np.array([self.cx + self.radius[0]*np.cos(self.omega[0]*t),
+                             self.cy + self.radius[1]*np.sin(self.omega[1]*t),
+                             self.cz + self.radius[2]*np.sin(self.omega[2]*t)])
+        x_dot    = np.array([-self.radius[0]*self.omega[0]*np.sin(self.omega[0]*t),
+                              self.radius[1]*self.omega[1]*np.cos(self.omega[1]*t),
+                              self.radius[2]*self.omega[2]*np.cos(self.omega[2]*t)])
+        x_ddot   = np.array([-self.radius[0]*(self.omega[0]**2)*np.cos(self.omega[0]*t),
+                             -self.radius[1]*(self.omega[1]**2)*np.sin(self.omega[1]*t),
+                             -self.radius[2]*(self.omega[2]**2)*np.sin(self.omega[2]*t)])
+        x_dddot  = np.array([ self.radius[0]*(self.omega[0]**3)*np.sin(self.omega[0]*t),
+                             -self.radius[1]*(self.omega[1]**3)*np.cos(self.omega[1]*t),
+                              self.radius[2]*(self.omega[2]**3)*np.cos(self.omega[2]*t)])
+        x_ddddot = np.array([self.radius[0]*(self.omega[0]**4)*np.cos(self.omega[0]*t),
+                             self.radius[1]*(self.omega[1]**4)*np.sin(self.omega[1]*t),
+                             self.radius[2]*(self.omega[2]**4)*np.sin(self.omega[2]*t)])
+
+        if self.yaw_bool:
+            yaw = 0.8*np.pi/2*np.sin(2.5*t)
+            yaw_dot = 0.8*2.5*np.pi/2*np.cos(2.5*t)
+        else:
+            yaw = 0
+            yaw_dot = 0
+
+        flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
+                        'yaw':yaw, 'yaw_dot':yaw_dot}
+        return flat_output

rotorpy/trajectories/hover_traj.py

@@ -0,0 +1,41 @@
+import numpy as np
+
+class HoverTraj(object):
+    """
+    This trajectory simply has the quadrotor hover at the origin indefinitely.
+    By modifying the initial condition, you can create step response
+    experiments.
+    """
+    def __init__(self):
+        """
+        This is the constructor for the Trajectory object. A fresh trajectory
+        object will be constructed before each mission.
+        """
+
+    def update(self, t):
+        """
+        Given the present time, return the desired flat output and derivatives.
+
+        Inputs
+            t, time, s
+        Outputs
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+        """
+        x    = np.zeros((3,))
+        x_dot = np.zeros((3,))
+        x_ddot = np.zeros((3,))
+        x_dddot = np.zeros((3,))
+        x_ddddot = np.zeros((3,))
+        yaw    = 0
+        yaw_dot = 0
+
+        flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
+                        'yaw':yaw, 'yaw_dot':yaw_dot}
+        return flat_output

rotorpy/trajectories/lissajous_traj.py

@@ -0,0 +1,75 @@
+import numpy as np
+"""
+Lissajous curves are defined by trigonometric functions parameterized in time. 
+See https://en.wikipedia.org/wiki/Lissajous_curve
+
+"""
+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):
+        """
+        This is the constructor for the Trajectory object. A fresh trajectory
+        object will be constructed before each mission.
+
+        Inputs:
+            A := amplitude on the X axis
+            B := amplitude on the Y axis
+            a := frequency on the X axis
+            b := frequency on the Y axis
+            delta := phase offset between the x and y parameterization
+            height := the z height that the lissajous occurs at
+            yaw_bool := determines whether the vehicle should yaw
+        """
+
+        self.A, self.B = A, B
+        self.a, self.b = a, b 
+        self.delta = delta
+        self.height = height
+
+        self.yaw_bool = yaw_bool
+
+    def update(self, t):
+        """
+        Given the present time, return the desired flat output and derivatives.
+
+        Inputs
+            t, time, s
+        Outputs
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                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),
+                             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),
+                             0])
+        x_ddot   = np.array([-(self.a)**2*self.A*np.sin(self.a*t + self.delta),
+                             -(self.b)**2*self.B*np.sin(self.b*t),
+                             0])
+        x_dddot  = np.array([-(self.a)**3*self.A*np.cos(self.a*t + self.delta),
+                             -(self.b)**3*self.B*np.cos(self.b*t),
+                             0])
+        x_ddddot = np.array([(self.a)**4*self.A*np.sin(self.a*t + self.delta),
+                             (self.b)**4*self.B*np.sin(self.b*t),
+                             0])
+
+        if self.yaw_bool:
+            yaw = np.pi/4*np.sin(np.pi*t)
+            yaw_dot = np.pi*np.pi/4*np.cos(np.pi*t)
+        else:
+            yaw = 0
+            yaw_dot = 0
+
+        flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
+                        'yaw':yaw, 'yaw_dot':yaw_dot}
+        return flat_output

rotorpy/trajectories/minsnap.py

@@ -0,0 +1,351 @@
+"""
+Imports
+"""
+import numpy as np
+import cvxopt
+from scipy.linalg import block_diag
+
+def cvxopt_solve_qp(P, q=None, G=None, h=None, A=None, b=None):
+    """
+    From https://scaron.info/blog/quadratic-programming-in-python.html . Infrastructure code for solving quadratic programs using CVXOPT. 
+    The structure of the program is as follows: 
+
+    min 0.5 xT P x + qT x
+    s.t. Gx <= h
+         Ax = b
+    Inputs:
+        P, numpy array, the quadratic term of the cost function
+        q, numpy array, the linear term of the cost function
+        G, numpy array, inequality constraint matrix
+        h, numpy array, inequality constraint vector
+        A, numpy array, equality constraint matrix
+        b, numpy array, equality constraint vector
+    Outputs:
+        The optimal solution to the quadratic program
+    """
+    P = .5 * (P + P.T)  # make sure P is symmetric
+    args = [cvxopt.matrix(P), cvxopt.matrix(q)]
+    if G is not None:
+        args.extend([cvxopt.matrix(G), cvxopt.matrix(h)])
+    if A is not None:
+        args.extend([cvxopt.matrix(A), cvxopt.matrix(b)])
+    sol = cvxopt.solvers.qp(*args)
+    if 'optimal' not in sol['status']:
+        return None
+    return np.array(sol['x']).reshape((P.shape[1],))
+
+def H_fun(dt):
+    """
+    Computes the cost matrix for a single segment in a single dimension.
+    *** Assumes that the decision variables c_i are e.g. x(t) = c_0 + c_1*t + c_2*t^2 + c_3*t^3 + c_4*t^4 + c_5*t^5 + c_6*t^6 + c_7*t^7
+    Inputs:
+        dt, scalar, the duration of the segment (t_(i+1) - t_i) 
+    Outputs:
+        H, numpy array, matrix containing the min snap cost function for that segment. Assumes the polynomial is of order 7.
+    """
+
+    cost = np.array([[576*dt, 1440*dt**2, 2880*dt**3, 5040*dt**4],
+                     [1440*dt**2, 4800*dt**3, 10800*dt**4, 20160*dt**5],
+                     [2880*dt**3, 10800*dt**4, 25920*dt**5, 50400*dt**6],
+                     [5040*dt**4, 20160*dt**5, 50400*dt**6, 100800*dt**7]])
+
+    H = np.zeros((8,8))
+    H[4:,4:] = cost
+
+    return H 
+
+def get_1d_equality_constraints(keyframes, delta_t, m, vmax=2):
+    """
+    Computes the equality constraints for the min snap problem. 
+    *** Assumes that the decision variables c_i are e.g. x(t) = c_0 + c_1*t + c_2*t^2 + c_3*t^3 + c_4*t^4 + c_5*t^5 + c_6*t^6 + c_7*t^7
+
+    Inputs:
+        keyframes, numpy array, a list of m waypoints IN ONE DIMENSION (x,y,z, or yaw)
+
+    """
+    # N = keyframes.shape[0]  # the number of keyframes
+    K = 8*m                 # the number of decision variables
+
+    A = np.zeros((8*m, K))
+    b = np.zeros((8*m,))
+
+    G = np.zeros((m, K))
+    h = np.zeros((m))
+
+
+    for i in range(m): # for each segment...
+        # Compute the segment duration
+        dt = delta_t[i]
+
+        # Position continuity at the beginning of the segment
+        A[i, 8*i:(8*i+8)] = np.array([1, 0, 0, 0, 0, 0, 0, 0])
+        b[i] = keyframes[i]
+
+        # Position continuity at the end of the segment
+        A[m+i, 8*i:(8*i+8)] = np.array([1, dt, dt**2, dt**3, dt**4, dt**5, dt**6, dt**7])
+        b[m+i] = keyframes[i+1]
+
+        ###### At this point we have 2*m constraints.. 
+
+        # Intermediate smoothness constraints
+        if i < (m-1): # we don't want to include the last segment for this loop
+            A[2*m + 6*i + 0, 8*i:(8*i+16)] = np.array([0, -1, -2*dt, -3*dt**2, -4*dt**3, -5*dt**4, -6*dt**5, -7*dt**6, 0, 1, 0, 0, 0, 0, 0, 0])     # Velocity
+            A[2*m + 6*i + 1, 8*i:(8*i+16)] = np.array([0, 0, -2, -6*dt, -12*dt**2, -20*dt**3, -30*dt**4, -42*dt**5, 0, 0, 2, 0, 0, 0, 0, 0])        # Acceleration
+            A[2*m + 6*i + 2, 8*i:(8*i+16)] = np.array([0, 0, 0, -6, -24*dt, -60*dt**2, -120*dt**3, -210*dt**4, 0, 0, 0, 6, 0, 0, 0, 0])             # Jerk
+            A[2*m + 6*i + 3, 8*i:(8*i+16)] = np.array([0, 0, 0, 0, -24, -120*dt, -360*dt**2, -840*dt**3, 0, 0, 0, 0, 24, 0, 0, 0])                  # Snap
+            A[2*m + 6*i + 4, 8*i:(8*i+16)] = np.array([0, 0, 0, 0, 0, -120, -720*dt, -2520*dt**2, 0, 0, 0, 0, 0, 120, 0, 0])                        # 5TH derivative
+            A[2*m + 6*i + 5, 8*i:(8*i+16)] = np.array([0, 0, 0, 0, 0, 0, -720, -5040*dt, 0, 0, 0, 0, 0, 0, 720, 0])                                 # 6TH derivative
+
+            b[2*m + 6*i + 0] = 0
+            b[2*m + 6*i + 1] = 0
+            b[2*m + 6*i + 2] = 0
+            b[2*m + 6*i + 3] = 0
+            b[2*m + 6*i + 4] = 0
+            b[2*m + 6*i + 5] = 0
+
+        G[i, 8*i:(8*i + 8)] = np.array([0, 1, 2*(0.5*dt), 3*(0.5*dt)**2, 4*(0.5*dt)**3, 5*(0.5*dt)**4, 6*(0.5*dt)**5, 7*(0.5*dt)**6])
+        h[i] = vmax
+
+    # Now we have added an addition 6*(m-1) constraints, total 2*m + 6*(m-1) = 2m + 8m - 6 = 8m - 6 constraints
+    # Last constraints are on the first and last trajectory segments. Higher derivatives are = 0
+    A[8*m - 6, 0:8] = np.array([0, 1, 0, 0, 0, 0, 0, 0])                                    # Velocity = 0 at start
+    b[8*m - 6] = 0
+    A[8*m - 5, -8:] = np.array([0, 1, 2*dt, 3*dt**2, 4*dt**3, 5*dt**4, 6*dt**5, 7*dt**6])   # Velocity = 0 at end
+    b[8*m - 5] = 0
+    A[8*m - 4, 0:8] = np.array([0, 0, 2, 0, 0, 0, 0, 0])                                    # Acceleration = 0 at start
+    b[8*m - 4] = 0
+    A[8*m - 3, -8:] = np.array([0, 0, 2, 6*dt,12*dt**2, 20*dt**3, 30*dt**4, 42*dt**5])      # Acceleration = 0 at end
+    b[8*m - 3] = 0
+    A[8*m - 2, 0:8] = np.array([0, 0, 0, 6, 0, 0, 0, 0])                                    # Jerk = 0 at start
+    b[8*m - 2] = 0
+    A[8*m - 1, -8:] = np.array([0, 0, 0, 6, 24*dt, 60*dt**2, 120*dt**3, 210*dt**4])         # Jerk = 0 at end
+    b[8*m - 1] = 0
+
+    return (A, b, G, h)
+
+class MinSnap(object):
+    """
+    MinSnap generates a minimum snap trajectory for the quadrotor, following https://ieeexplore.ieee.org/document/5980409. 
+    The trajectory is a piecewise 7th order polynomial (minimum degree necessary for snap optimality). 
+    """
+    def __init__(self, points, yaw_angles=None, v_avg=2):
+        """
+        Waypoints and yaw angles compose the "keyframes" for optimizing over. 
+        Inputs:
+            points, numpy array of m 3D waypoints. 
+            yaw_angles, numpy array of m yaw angles corresponding to each waypoint. 
+            v_avg, the average speed between waypoints, this is used to do the time allocation. 
+        """
+
+        if yaw_angles is None:
+            self.yaw = np.zeros((points.shape[0]))
+        else:
+            self.yaw = yaw_angles
+        self.v_avg = v_avg
+
+        # Compute the distances between each waypoint.
+        seg_dist = np.linalg.norm(np.diff(points, axis=0), axis=1)
+        seg_mask = np.append(True, seg_dist > 1e-3)
+        self.points = points[seg_mask,:]
+
+        self.null = False
+
+        m = self.points.shape[0]-1  # Get the number of segments
+
+        # Compute the derivatives of the polynomials
+        self.x_dot_poly    = np.zeros((m, 3, 7))
+        self.x_ddot_poly   = np.zeros((m, 3, 6))
+        self.x_dddot_poly  = np.zeros((m, 3, 5))
+        self.x_ddddot_poly = np.zeros((m, 3, 4))
+        self.yaw_dot_poly = np.zeros((m, 1, 7))
+        
+        # If two or more waypoints remain, solve min snap
+        if self.points.shape[0] >= 2:
+
+            ################## Time allocation
+            self.delta_t = seg_dist/self.v_avg # Compute the segment durations based on the average velocity
+            self.t_keyframes = np.concatenate(([0], np.cumsum(self.delta_t)))  # Construct time array which indicates when the quad should be at the i'th waypoint. 
+
+            ################## Cost function
+            # First get the cost segment for each matrix: 
+            H = [H_fun(self.delta_t[i]) for i in range(m)]
+
+            # Now concatenate these costs using block diagonal form:
+            P = block_diag(*H)
+
+            # Lastly the linear term in the cost function is 0
+            q = np.zeros((8*m,1))
+            
+            ################## Constraints for each axis
+            (Ax,bx,Gx,hx) = get_1d_equality_constraints(self.points[:,0], self.delta_t, m)
+            (Ay,by,Gy,hy) = get_1d_equality_constraints(self.points[:,1], self.delta_t, m)
+            (Az,bz,Gz,hz) = get_1d_equality_constraints(self.points[:,2], self.delta_t, m)
+            (Ayaw,byaw,Gyaw,hyaw) = get_1d_equality_constraints(self.yaw, self.delta_t, m)
+
+            ################## Solve for x, y, z, and yaw
+            c_opt_x = np.linalg.solve(Ax,bx)
+            c_opt_y = np.linalg.solve(Ay,by)
+            c_opt_z = np.linalg.solve(Az,bz)
+            c_opt_yaw = np.linalg.solve(Ayaw,byaw)
+
+            ################## Construct polynomials from c_opt
+            self.x_poly = np.zeros((m, 3, 8))
+            self.yaw_poly = np.zeros((m, 1, 8))
+            for i in range(m):
+                self.x_poly[i,0,:] = np.flip(c_opt_x[8*i:(8*i+8)])
+                self.x_poly[i,1,:] = np.flip(c_opt_y[8*i:(8*i+8)])
+                self.x_poly[i,2,:] = np.flip(c_opt_z[8*i:(8*i+8)])
+                self.yaw_poly[i,0,:] = np.flip(c_opt_yaw[8*i:(8*i+8)])
+
+            for i in range(m):
+                for j in range(3):
+                    self.x_dot_poly[i,j,:]    = np.polyder(self.x_poly[i,j,:], m=1)
+                    self.x_ddot_poly[i,j,:]   = np.polyder(self.x_poly[i,j,:], m=2)
+                    self.x_dddot_poly[i,j,:]  = np.polyder(self.x_poly[i,j,:], m=3)
+                    self.x_ddddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=4)
+            self.yaw_dot_poly[i,0,:] = np.polyder(self.yaw_poly[i,0,:], m=1)
+
+        else:
+            # Otherwise, there is only one waypoint so we just set everything = 0. 
+            self.null = True
+            m = 1
+            self.T = np.zeros((m,))
+            self.x_poly = np.zeros((m, 3, 6))
+            self.x_poly[0,:,-1] = points[0,:]
+
+    def update(self, t):
+        """
+        Given the present time, return the desired flat output and derivatives.
+
+        Inputs
+            t, time, s
+        Outputs
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+        """
+        x = np.zeros((3,))
+        x_dot = np.zeros((3,))
+        x_ddot = np.zeros((3,))
+        x_dddot = np.zeros((3,))
+        x_ddddot = np.zeros((3,))
+        yaw    = 0
+        yaw_dot = 0
+
+        if self.null:
+            # If there's only one waypoint
+            x    = self.points[0,:]
+        else:
+            # Find interval index i and time within interval t.
+            t = np.clip(t, self.t_keyframes[0], self.t_keyframes[-1])
+            for i in range(self.t_keyframes.size-1):
+                if self.t_keyframes[i] + self.delta_t[i] >= t:
+                    break
+            t = t - self.t_keyframes[i]
+
+            # Evaluate polynomial.
+            for j in range(3):
+                x[j]        = np.polyval(       self.x_poly[i,j,:], t)
+                x_dot[j]    = np.polyval(   self.x_dot_poly[i,j,:], t)
+                x_ddot[j]   = np.polyval(  self.x_ddot_poly[i,j,:], t)
+                x_dddot[j]  = np.polyval( self.x_dddot_poly[i,j,:], t)
+                x_ddddot[j] = np.polyval(self.x_ddddot_poly[i,j,:], t)
+
+            yaw = np.polyval(self.yaw_poly[i, 0, :], t)
+            yaw_dot = np.polyval(self.yaw_dot_poly[i,0,:], t)
+
+        flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
+                        'yaw':yaw, 'yaw_dot':yaw_dot}
+        return flat_output
+
+if __name__=="__main__":
+
+    import matplotlib.pyplot as plt
+    from matplotlib import cm
+
+    waypoints = np.array([[0,0,0],
+                          [1,0,0],
+                          [1,1,0],
+                          [0,1,0],
+                          [0,2,0],
+                          [2,2,0]
+                          ])
+    yaw_angles = np.array([0, np.pi/2, 0, np.pi/4, 3*np.pi/2, 0])
+
+    traj = MinSnap(waypoints, yaw_angles, v_avg=2)
+
+    N = 1000
+    time = np.linspace(0,5,N)
+    x = np.zeros((N, 3))
+    xdot = np.zeros((N,3))
+    xddot = np.zeros((N,3))
+    xdddot = np.zeros((N,3))
+    xddddot = np.zeros((N,3))
+    yaw = np.zeros((N,))
+    yaw_dot = np.zeros((N,))
+
+    for i in range(N):
+        flat = traj.update(time[i])
+
+        x[i,:] = flat['x']
+        xdot[i,:] = flat['x_dot']
+        xddot[i,:] = flat['x_ddot']
+        xdddot[i,:] = flat['x_dddot']
+        xddddot[i,:] = flat['x_ddddot']
+
+        yaw[i] = flat['yaw']
+        yaw_dot[i] = flat['yaw_dot']
+
+
+    (fig, axes) = plt.subplots(nrows=5, ncols=1, sharex=True, num="Translational Flat Outputs")
+    ax = axes[0]
+    ax.plot(time, x[:,0], 'r-', label="X")
+    ax.plot(time, x[:,1], 'g-', label="Y")
+    ax.plot(time, x[:,2], 'b-', label="Z")
+    ax.legend()
+    ax.set_ylabel("x")
+    ax = axes[1]
+    ax.plot(time, xdot[:,0], 'r-', label="X")
+    ax.plot(time, xdot[:,1], 'g-', label="Y")
+    ax.plot(time, xdot[:,2], 'b-', label="Z")
+    ax.set_ylabel("xdot")
+    ax = axes[2]
+    ax.plot(time, xddot[:,0], 'r-', label="X")
+    ax.plot(time, xddot[:,1], 'g-', label="Y")
+    ax.plot(time, xddot[:,2], 'b-', label="Z")
+    ax.set_ylabel("xddot")
+    ax = axes[3]
+    ax.plot(time, xdddot[:,0], 'r-', label="X")
+    ax.plot(time, xdddot[:,1], 'g-', label="Y")
+    ax.plot(time, xdddot[:,2], 'b-', label="Z")
+    ax.set_ylabel("xdddot")
+    ax = axes[4]
+    ax.plot(time, xddddot[:,0], 'r-', label="X")
+    ax.plot(time, xddddot[:,1], 'g-', label="Y")
+    ax.plot(time, xddddot[:,2], 'b-', label="Z")
+    ax.set_ylabel("xddddot")
+    ax.set_xlabel("Time, s")
+
+    (fig, axes) = plt.subplots(nrows=2, ncols=1, sharex=True, num="Yaw vs Time")
+    ax = axes[0]
+    ax.plot(time, yaw, 'k', label="yaw")
+    ax.set_ylabel("yaw")
+    ax = axes[1]
+    ax.plot(time, yaw_dot, 'k', label="yaw")
+    ax.set_ylabel("yaw dot")
+    ax.set_xlabel("Time, s")
+
+    speed = np.sqrt(xdot[:,0]**2 + xdot[:,1]**2)
+    (fig, axes) = plt.subplots(nrows=1, ncols=1, num="XY Trajectory")
+    ax = axes
+    ax.scatter(x[:,0], x[:,1], c=cm.winter(speed/speed.max()), s=4, label="Flat Output")
+    ax.plot(waypoints[:,0], waypoints[:,1], 'ko', markersize=10, label="Waypoints")
+    ax.grid()
+    ax.legend()
+
+    plt.show()

rotorpy/trajectories/polynomial_traj.py

@@ -0,0 +1,107 @@
+import numpy as np
+
+class Polynomial(object):
+    """
+
+    """
+    def __init__(self, points, v_avg=1.2):
+        """
+        Inputs:
+            points, (N, 3) array of N waypoint coordinates in 3D
+            v_avg, the average speed between segments
+        """
+
+        def get_poly(xi, xf, T):
+            """
+            Return fully constrained polynomial coefficients from xi to xf in
+            time interval [0,T]. Low derivatives are all zero at the endpoints.
+            """
+            A = np.array([[      0,       0,      0,    0, 0, 1],
+                          [   T**5,    T**4,   T**3, T**2, T, 1],
+                          [      0,       0,      0,    0, 1, 0],
+                          [ 5*T**4,  4*T**3, 3*T**2,  2*T, 1, 0],
+                          [      0,       0,      0,    2, 0, 0],
+                          [20*T**3, 12*T**2,    6*T,    2, 0, 0]])
+            b = np.array([xi, xf, 0, 0, 0, 0])
+            poly = np.linalg.solve(A,b)
+            return poly
+
+        self.v_avg = v_avg
+
+        # Remove any sequential duplicate points; always keep the first point.
+        seg_dist = np.linalg.norm(np.diff(points, axis=0), axis=1)
+        seg_mask = np.append(True, seg_dist > 1e-3)
+        points = points[seg_mask,:]
+
+        # If at least two waypoints remain, calculate segment polynomials.
+        if points.shape[0] >= 2:
+            N = points.shape[0]-1
+            self.T = seg_dist/self.v_avg # segment duration
+            self.x_poly = np.zeros((N, 3, 6))
+            for i in range(N):
+                for j in range(3):
+                    self.x_poly[i,j,:] = get_poly(points[i,j], points[i+1,j], self.T[i])
+        # Otherwise, hard code constant polynomial at initial waypoint position.
+        else:
+            N = 1
+            self.T = np.zeros((N,))
+            self.x_poly = np.zeros((N, 3, 6))
+            self.x_poly[0,:,-1] = points[0,:]
+
+        # Calculate global start time of each segment.
+        self.t_start = np.concatenate(([0], np.cumsum(self.T[:-1])))
+
+        # Calculate derivative polynomials.
+        self.x_dot_poly    = np.zeros((N, 3, 5))
+        self.x_ddot_poly   = np.zeros((N, 3, 4))
+        self.x_dddot_poly  = np.zeros((N, 3, 3))
+        self.x_ddddot_poly = np.zeros((N, 3, 2))
+        for i in range(N):
+            for j in range(3):
+                self.x_dot_poly[i,j,:]    = np.polyder(self.x_poly[i,j,:], m=1)
+                self.x_ddot_poly[i,j,:]   = np.polyder(self.x_poly[i,j,:], m=2)
+                self.x_dddot_poly[i,j,:]  = np.polyder(self.x_poly[i,j,:], m=3)
+                self.x_ddddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=4)
+
+    def update(self, t):
+        """
+        Given the present time, return the desired flat output and derivatives.
+
+        Inputs
+            t, time, s
+        Outputs
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+        """
+        x        = np.zeros((3,))
+        x_dot    = np.zeros((3,))
+        x_ddot   = np.zeros((3,))
+        x_dddot  = np.zeros((3,))
+        x_ddddot = np.zeros((3,))
+        yaw = 0
+        yaw_dot = 0
+
+        # Find interval index i and time within interval t.
+        t = np.clip(t, self.t_start[0], self.t_start[-1]+self.T[-1])
+        for i in range(self.t_start.size):
+            if self.t_start[i] + self.T[i] >= t:
+                break
+        t = t - self.t_start[i]
+
+        # Evaluate polynomial.
+        for j in range(3):
+            x[j]        = np.polyval(       self.x_poly[i,j,:], t)
+            x_dot[j]    = np.polyval(   self.x_dot_poly[i,j,:], t)
+            x_ddot[j]   = np.polyval(  self.x_ddot_poly[i,j,:], t)
+            x_dddot[j]  = np.polyval( self.x_dddot_poly[i,j,:], t)
+            x_ddddot[j] = np.polyval(self.x_ddddot_poly[i,j,:], t)
+
+        flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
+                        'yaw':yaw, 'yaw_dot':yaw_dot}
+        return flat_output

rotorpy/trajectories/speed_traj.py

@@ -0,0 +1,105 @@
+"""
+Imports
+"""
+import numpy as np
+
+class ConstantSpeed(object):
+    """
+    
+    """
+    def __init__(self, init_pos, dist=1, speed=1, axis=0, repeat=False):
+        """
+        Constant speed will command a step response in speed on a specified axis. The following inputs
+        try to ensure that the vehicle is not commanded to go beyond the boundaries of the environment. 
+
+        INPUTS
+            init_pos := the inital position for the trajectory to begin, should be the current quadrotor's position
+            dist := the length of the trajectory in meters. 
+            speed := the speed of the trajectory in meters. 
+            axis := the axis to travel (0 -> X, 1 -> Y, 2 -> Z)
+            repeat := determines if the trajectory should repeat, where the direction is switched from its current state.
+        """
+
+        self.pt1 = init_pos
+        self.dist = dist    # m
+        self.speed = speed  # m/s
+
+        # Based on the length of the trajectory (distance) and the desired speed, compute how long the desired speed should be commaned. 
+        self.t_width = self.dist/self.speed  # seconds
+        
+        # Create a vector where the "axis'th" element is 1 and the other elements are 0. This will multiplied by the flat outputs 
+        # so that the only "active" axis is the one specified by "axis"
+        self.active_axis = np.zeros((3,))
+        self.active_axis[axis] = 1
+
+        self.direction = 1
+
+        self.repeat = repeat
+        self.reverse_threshold = 0.01
+
+        self.pt2 = self.pt1 + self.speed*self.t_width*self.active_axis
+
+    def update(self, t):
+        """
+        Given the present time, return the desired flat output and derivatives.
+
+        Inputs
+            t, time, s
+        Outputs
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+        """
+
+        v = (self.speed)*self.active_axis*self.direction
+        if t <= self.t_width:
+            x       = self.pt1 + v*t
+            x_dot   = v
+        else:
+            x       = self.pt2
+            x_dot   = np.zeros((3,))
+
+        x_ddot = np.zeros((3,))
+        x_dddot = np.zeros((3,))
+        x_ddddot = np.zeros((3,))
+
+        yaw    = 0
+        yaw_dot = 0
+
+        flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
+                        'yaw':yaw, 'yaw_dot':yaw_dot}
+        return flat_output
+
+if __name__=="__main__":
+
+    import matplotlib.pyplot as plt
+
+    traj = ConstantSpeed(init_pos=np.array([0,0,0]))
+
+
+    t = np.linspace(0,10,500)
+    x = np.zeros((t.shape[0], 3))
+    
+    for i in range(t.shape[0]):
+        flat = traj.update(t[i])
+        x[i,:] = flat['x']
+
+    (fig, axes) = plt.subplots(nrows=3, ncols=1, num="Constant Speed Trajectory")
+    ax = axes[0]
+    ax.plot(t, x[:,0], 'r', linewidth=1.5)
+    ax.set_ylabel("X, m")
+    ax = axes[1]
+    ax.plot(t, x[:,1], 'g', linewidth=1.5)
+    ax.set_ylabel("Y, m")
+    ax = axes[2]
+    ax.plot(t, x[:,2], 'b', linewidth=1.5)
+    ax.set_ylabel("Z, m")
+    ax.set_xlabel("Time, s")
+    
+    plt.show()
+    

rotorpy/trajectories/traj_template.py

@@ -0,0 +1,46 @@
+"""
+Imports
+"""
+import numpy as np
+
+class TrajTemplate(object):
+    """
+    The trajectory is implemented as a class. There are two required methods for each trajectory class: __init__() and update().
+    The __init__() is required for instantiating the class with appropriate parameterizations. For example, if you are doing 
+    a circular trajectory you might want to specify the radius of the circle. 
+    The update() method is called at each iteration of the simulator. The only input to update is time t. The output of update()
+    should be the desired flat outputs in a dictionary, as specified below. 
+    """
+    def __init__(self):
+        """
+        This is the constructor for the Trajectory object. A fresh trajectory
+        object will be constructed before each mission.
+        """
+
+    def update(self, t):
+        """
+        Given the present time, return the desired flat output and derivatives.
+
+        Inputs
+            t, time, s
+        Outputs
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2
+                x_dddot,  jerk, m/s**3
+                x_ddddot, snap, m/s**4
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+        """
+        x    = np.zeros((3,))
+        x_dot = np.zeros((3,))
+        x_ddot = np.zeros((3,))
+        x_dddot = np.zeros((3,))
+        x_ddddot = np.zeros((3,))
+        yaw    = 0
+        yaw_dot = 0
+
+        flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
+                        'yaw':yaw, 'yaw_dot':yaw_dot}
+        return flat_output

rotorpy/utils/animate.py

@@ -0,0 +1,132 @@
+"""
+TODO: Set up figure for appropriate target video size (eg. 720p).
+TODO: Decide which additional user options should be available.
+"""
+
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+from matplotlib.animation import FuncAnimation
+import matplotlib.pyplot as plt
+from scipy.spatial.transform import Rotation
+
+from rotorpy.utils.axes3ds import Axes3Ds
+from rotorpy.utils.shapes import Quadrotor
+
+import os
+
+class ClosingFuncAnimation(FuncAnimation):
+    def __init__(self, fig, func, *args, **kwargs):
+        self._close_on_finish = kwargs.pop('close_on_finish')
+        FuncAnimation.__init__(self, fig, func, *args, **kwargs)
+
+    # def _stop(self, *args):
+    #     super()._stop(self, *args)
+    #     if self._close_on_finish:
+    #         plt.close(self._fig)
+
+    def _step(self, *args):
+        still_going = FuncAnimation._step(self, *args)
+        if self._close_on_finish and not still_going:
+            plt.close(self._fig)
+
+def _decimate_index(time, sample_time):
+    """
+    Given sorted lists of source times and sample times, return indices of
+    source time closest to each sample time.
+    """
+    index = np.arange(time.size)
+    sample_index = np.round(np.interp(sample_time, time, index)).astype(int)
+    return sample_index
+
+def animate(time, position, rotation, world, filename=None, blit=False, show_axes=True, close_on_finish=False):
+    """
+    Animate a completed simulation result based on the time, position, and
+    rotation history. The animation may be viewed live or saved to a .mp4 video
+    (slower, requires additional libraries).
+
+    For a live view, it is absolutely critical to retain a reference to the
+    returned object in order to prevent garbage collection before the animation
+    has completed displaying.
+
+    Parameters
+        time, (N,) with uniform intervals
+        position, (N,3)
+        rotation, (N,3,3)
+        world, a World object
+        filename, for saved video, or live view if None
+        blit, if True use blit for faster animation, default is False
+        show_axes, if True plot axes, default is True
+        close_on_finish, if True close figure at end of live animation or save, default is False
+    """
+
+    # Temporal style.
+    rtf = 1.0 # real time factor > 1.0 is faster than real time playback
+    render_fps = 30
+
+    # Decimate data to render interval; always include t=0.
+    if time[-1] != 0:
+        sample_time = np.arange(0, time[-1], 1/render_fps * rtf)
+    else:
+        sample_time = np.zeros((1,))
+    index = _decimate_index(time, sample_time)
+    time = time[index]
+    position = position[index,:]
+    rotation = rotation[index,:,:]
+
+    # Set up axes.
+    if filename is not None:
+        if isinstance(filename, Path):
+            fig = plt.figure(filename.name)
+        else:
+            fig = plt.figure(filename)
+    else:
+        fig = plt.figure('Animation')
+    fig.clear()
+    ax = Axes3Ds(fig)
+    if not show_axes:
+        ax.set_axis_off()
+    ax.set_xlim(-1,1)
+    ax.set_ylim(-1,1)
+    ax.set_zlim(-1,1)
+
+    quad = Quadrotor(ax)
+
+    world_artists = world.draw(ax)
+
+    title_artist = ax.set_title('t = {}'.format(time[0]))
+
+    def init():
+        ax.draw(fig.canvas.get_renderer())
+        return world_artists + list(quad.artists) + [title_artist]
+
+    def update(frame):
+        title_artist.set_text('t = {:.2f}'.format(time[frame]))
+        quad.transform(position=position[frame,:], rotation=rotation[frame,:,:])
+        [a.do_3d_projection(fig.canvas.get_renderer()) for a in quad.artists]
+        return world_artists + list(quad.artists) + [title_artist]
+
+    ani = ClosingFuncAnimation(fig=fig,
+                        func=update,
+                        frames=time.size,
+                        init_func=init,
+                        interval=1000.0/render_fps,
+                        repeat=False,
+                        blit=blit,
+                        close_on_finish=close_on_finish)
+
+    if filename is not None:
+        print('Saving Animation')
+        if not ".mp4" in filename:
+            filename = filename + ".mp4"
+        path = os.path.join(os.path.dirname(__file__),'..','data_out',filename)
+        ani.save(path,
+                 writer='ffmpeg',
+                 fps=render_fps,
+                 dpi=100)
+        if close_on_finish:
+            plt.close(fig)
+            ani = None
+
+    return ani

rotorpy/utils/axes3ds.py

@@ -0,0 +1,159 @@
+"""
+This module provides Axes3Ds ("Axes3D Spatial"), a drop-in replacement for
+Axes3D which incorporates the improvements proposed by eric-wieser in matplotlib
+issue #8896.
+
+The purpose is to reduce the distortion when projecting 3D scenes into the 2D
+image. For example, the projection of a sphere will be (closer to) a circle.
+"""
+
+"""
+License agreement for matplotlib versions 1.3.0 and later
+=========================================================
+
+1. This LICENSE AGREEMENT is between the Matplotlib Development Team
+("MDT"), and the Individual or Organization ("Licensee") accessing and
+otherwise using matplotlib software in source or binary form and its
+associated documentation.
+
+2. Subject to the terms and conditions of this License Agreement, MDT
+hereby grants Licensee a nonexclusive, royalty-free, world-wide license
+to reproduce, analyze, test, perform and/or display publicly, prepare
+derivative works, distribute, and otherwise use matplotlib
+alone or in any derivative version, provided, however, that MDT's
+License Agreement and MDT's notice of copyright, i.e., "Copyright (c)
+2012- Matplotlib Development Team; All Rights Reserved" are retained in
+matplotlib  alone or in any derivative version prepared by
+Licensee.
+
+3. In the event Licensee prepares a derivative work that is based on or
+incorporates matplotlib or any part thereof, and wants to
+make the derivative work available to others as provided herein, then
+Licensee hereby agrees to include in any such work a brief summary of
+the changes made to matplotlib .
+
+4. MDT is making matplotlib available to Licensee on an "AS
+IS" basis.  MDT MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR
+IMPLIED.  BY WAY OF EXAMPLE, BUT NOT LIMITATION, MDT MAKES NO AND
+DISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS
+FOR ANY PARTICULAR PURPOSE OR THAT THE USE OF MATPLOTLIB
+WILL NOT INFRINGE ANY THIRD PARTY RIGHTS.
+
+5. MDT SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF MATPLOTLIB
+ FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR
+LOSS AS A RESULT OF MODIFYING, DISTRIBUTING, OR OTHERWISE USING
+MATPLOTLIB , OR ANY DERIVATIVE THEREOF, EVEN IF ADVISED OF
+THE POSSIBILITY THEREOF.
+
+6. This License Agreement will automatically terminate upon a material
+breach of its terms and conditions.
+
+7. Nothing in this License Agreement shall be deemed to create any
+relationship of agency, partnership, or joint venture between MDT and
+Licensee.  This License Agreement does not grant permission to use MDT
+trademarks or trade name in a trademark sense to endorse or promote
+products or services of Licensee, or any third party.
+
+8. By copying, installing or otherwise using matplotlib ,
+Licensee agrees to be bound by the terms and conditions of this License
+Agreement.
+
+"""
+
+import numpy as np
+
+from mpl_toolkits.mplot3d import Axes3D
+from mpl_toolkits.mplot3d import proj3d
+
+# Patch note: An update of the original implementation in proj3d.py.
+def world_transformation(xmin, xmax,
+                         ymin, ymax,
+                         zmin, zmax, pb_aspect=None):
+    """
+    produce a matrix that scales homogenous coords in the specified ranges
+    to [0, 1], or [0, pb_aspect[i]] if the plotbox aspect ratio is specified
+    """
+    dx = xmax - xmin
+    dy = ymax - ymin
+    dz = zmax - zmin
+    if pb_aspect is not None:
+        ax, ay, az = pb_aspect
+        dx /= ax
+        dy /= ay
+        dz /= az
+
+    return np.array([[1/dx, 0,    0,    -xmin/dx],
+                     [0,    1/dy, 0,    -ymin/dy],
+                     [0,    0,    1/dz, -zmin/dz],
+                     [0,    0,    0,    1]])
+
+class Axes3Ds(Axes3D):
+    """
+    Class Axes3Ds ("Axes3D Spatial") is a drop-in replacement for Axes3D which
+    incorporates the improvements proposed by eric-wieser in matplotlib issue
+    #8896.
+    """
+
+    # Patch note: A new function.
+    def apply_aspect(self, position=None):
+        if position is None:
+            position = self.get_position(original=True)
+
+        # in the superclass, we would go through and actually deal with axis
+        # scales and box/datalim. Those are all irrelevant - all we need to do
+        # is make sure our coordinate system is square.
+        figW, figH = self.get_figure().get_size_inches()
+        fig_aspect = figH / figW
+        box_aspect = 1
+        pb = position.frozen()
+        pb1 = pb.shrunk_to_aspect(box_aspect, pb, fig_aspect)
+        self.set_position(pb1.anchored(self.get_anchor(), pb), 'active')
+
+    # Patch note: Overwritten to use the updated version of world_transformation
+    # and the new pb_aspect value.
+    def get_proj(self):
+        """
+        Create the projection matrix from the current viewing position.
+        elev stores the elevation angle in the z plane
+        azim stores the azimuth angle in the x,y plane
+        dist is the distance of the eye viewing point from the object
+        point.
+        """
+        # chosen for similarity with the initial view before gh-8896
+        pb_aspect = np.array([4, 4, 3]) / 3.5
+
+        relev, razim = np.pi * self.elev/180, np.pi * self.azim/180
+
+        xmin, xmax = self.get_xlim3d()
+        ymin, ymax = self.get_ylim3d()
+        zmin, zmax = self.get_zlim3d()
+
+        # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0
+        worldM = world_transformation(xmin, xmax,
+                                      ymin, ymax,
+                                      zmin, zmax, pb_aspect=pb_aspect)
+
+        # look into the middle of the new coordinates
+        R = pb_aspect / 2
+
+        xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist
+        yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist
+        zp = R[2] + np.sin(relev) * self.dist
+        E = np.array((xp, yp, zp))
+
+        self.eye = E
+        self.vvec = R - E
+        self.vvec = self.vvec / np.linalg.norm(self.vvec)
+
+        if abs(relev) > np.pi/2:
+            # upside down
+            V = np.array((0, 0, -1))
+        else:
+            V = np.array((0, 0, 1))
+        zfront, zback = -self.dist, self.dist
+
+        viewM = proj3d.view_transformation(E, R, V)
+        projM = self._projection(zfront, zback)
+        M0 = np.dot(viewM, worldM)
+        M = np.dot(projM, M0)
+        return M

rotorpy/utils/numpy_encoding.py

@@ -0,0 +1,110 @@
+import json
+import numpy as np
+
+def to_ndarray(obj, dtype=np.float64):
+    """
+    Greedily and recursively convert the given object to a dtype ndarray.
+    """
+    if isinstance(obj, dict):
+        for k in obj:
+            obj[k] = to_ndarray(obj[k])
+        return obj
+    elif isinstance(obj, list):
+        try:
+            return np.array(obj, dtype=dtype)
+        except:
+            return [to_ndarray(o) for o in obj]
+    else:
+        return obj
+
+class HelperNumpyJSONEncoder(json.JSONEncoder):
+    """
+    This encoder encodes Numpy arrays as lists.
+    """
+    def default(self, o):
+        if isinstance(o, np.ndarray):
+            return o.tolist()
+        return json.JSONEncoder.default(self, o)
+
+class NumpyJSONEncoder(json.JSONEncoder):
+    """
+    This encoder will print an entire collection onto a single line if it fits.
+    Otherwise the individual elements are printed on separate lines. Numpy
+    arrays are encoded as lists.
+
+    This class is derived from contributions by Tim Ludwinski and Jannis
+    Mainczyk to a stackoverflow discussion:
+    https://stackoverflow.com/questions/16264515/json-dumps-custom-formatting
+    """
+
+    MAX_WIDTH = 80 # Maximum length of a single line list.
+    MAX_ITEMS = 80 # Maximum number of items in a single line list.
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.indentation_level = 0
+
+    def encode(self, o):
+        # If o fits on a single line, do so.
+        line = json.dumps(o, cls=HelperNumpyJSONEncoder)
+        if len(line) <= self.MAX_WIDTH:
+            return line
+        # Otherwise, break o into pieces.
+        else:
+            # If a list, split each entry into a separate line.
+            if isinstance(o, (list, tuple)):
+                self.indentation_level += 1
+                output = [self.indent_str + self.encode(el) for el in o]
+                self.indentation_level -= 1
+                return "[\n" + ",\n".join(output) + "\n" + self.indent_str + "]"
+            # If a dict, each key/value pair into a separate line.
+            if isinstance(o, dict):
+                self.indentation_level += 1
+                output = [self.indent_str + f"{json.dumps(k)}: {self.encode(v)}" for k, v in o.items()]
+                self.indentation_level -= 1
+                return "{\n" + ",\n".join(output) + "\n" + self.indent_str + "}"
+            # Otherwise use default encoding.
+            return json.dumps(o)
+
+    @property
+    def indent_str(self) -> str:
+        if self.indent == None:
+            indent = 0
+        else:
+            indent = self.indent
+        return " " * self.indentation_level * indent
+
+if __name__ == '__main__':
+    import copy
+
+    # Example data.
+    data = {
+        'bounds': {'extents': [0, 5.0, 0, 2.0, 0, 13.0]},
+        'blocks': [
+            {'extents': [2, 3, 0.0, 2, 0.0, 10.0], 'color': [1, 0, 0]},
+            {'extents': [2, 3, 0.0, 2, 0.0, 10.0], 'color': [1, 0, 0]},
+            {'extents': [2, 3, 0.0, 2, 0.0, 10.0], 'color': [1, 0, 0]},
+            {'extents': [2, 3, 0.0, 2, 0.0, 10.0], 'color': [1, 0, 0]},
+            {'extents': [2, 3, 0.0, 2, 0.0, 10.0]},
+            {'extents': [2, 3, 0.0, 2, 0.0, 10.0]},
+            {'extents': [2, 3, 0.0, 2, 0.0, 10.0]}],
+        'start': np.array([0, 0, 1]),
+        'goal': np.array([4, 0, 2]),
+        'resolution': np.array([0.25, 0.25, 0.5]),
+        'margin': 0.1,
+        'expected_path_length': 20.52
+    }
+    data['more'] = copy.deepcopy(data)
+
+    # Print JSON string to terminal.
+    print(json.dumps(data, cls=NumpyJSONEncoder, indent=4))
+
+    # Using 'dump' not yet supported.
+    with open('example.json', 'w') as file:
+        file.write(json.dumps(data, cls=NumpyJSONEncoder, indent=4))
+
+    with open('example.json') as file:
+        data_out = json.load(file)
+    data_out = to_ndarray(data_out)
+
+    print(data_out)

rotorpy/utils/occupancy_map.py

@@ -0,0 +1,260 @@
+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 get_local_2d_occupancy_map(self, position, region_size=(10, 10)):
+        """
+        Return a new occupancy map that is centered at the position of the vehicle. 
+        Parameters:
+            position, a numpy array indicating the (x,y,z) position of the quadrotor in the world frame.
+            region_size, indicates the size of the rectangular XY region around the quadrotor. 
+        """
+
+        region_size = np.array(region_size)
+
+        # Get the index corresponding to the current position of the quadrotor. 
+        center = self.metric_to_index(position)
+
+        # World bounds
+        world_bounds = self.world.world['bounds']['extents']
+
+        # Get the extents of the XY region, clipping to the extents of the world.
+        xmin = max(position[0] - region_size[0], world_bounds[0])
+        xmax = min(position[0] + region_size[0], world_bounds[1])
+        ymin = max(position[1] - region_size[1], world_bounds[2])
+        ymax = min(position[1] + region_size[1], world_bounds[3])
+
+        # Now collect the portion of the map that is contained within the region.
+        bounds = [xmin, xmax, ymin, ymax, position[2], position[2]]
+        (inner_min_index, inner_max_index) = self._metric_block_to_index_range(bounds, outer_bound=True)
+
+        a, b = inner_min_index, inner_max_index
+        self.local_map = self.map[a[0]:(b[0]+1), a[1]:(b[1]+1), center[2]]
+        
+        return ((xmin, xmax), (ymin, ymax), self.local_map)
+
+    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 axes3ds import Axes3Ds
+    import matplotlib.pyplot as plt
+
+    import os
+
+    # Create a world object first
+    world = World.random_forest(world_dims=(5, 5, 5), tree_width=.1, tree_height=5, num_trees=10)
+    world_fname = 'pillar'
+    world = World.from_file(os.path.abspath(os.path.join(os.path.dirname(__file__),'..','worlds',world_fname+'.json')))
+
+    # Create a figure
+    fig = plt.figure()
+    ax = Axes3Ds(fig)
+    # Draw the world
+    world.draw(ax)
+
+    # Create an Occupancy map
+    oc = OccupancyMap(world, (.1, .1, .1), .01)
+    position = np.array([2.5, 2.5, 0])
+    region = (1, 1)
+    oc.get_local_2d_occupancy_map(position, region_size=region)
+    # 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()

rotorpy/utils/plotter.py

@@ -0,0 +1,247 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+import matplotlib.pyplot as plt
+
+from rotorpy.utils.axes3ds import Axes3Ds
+from rotorpy.utils.animate import animate
+
+import os
+
+"""
+Functions for showing the results from the simulator.
+
+"""
+
+class Plotter():
+
+    def __init__(self, results, world):
+
+        (self.time, self.x, self.x_des, self.v, 
+        self.v_des, self.q, self.q_des, self.w, 
+        self.s, self.s_des, self.M, self.T, self.wind,
+        self.accel, self.gyro, self.accel_gt,
+        self.x_mc, self.v_mc, self.q_mc, self.w_mc, 
+        self.filter_state, self.covariance, self.sd) = self.unpack_results(results)
+
+        self.R = Rotation.from_quat(self.q).as_matrix()
+        self.R_mc = Rotation.from_quat(self.q_mc).as_matrix() # Rotation as measured by motion capture.
+
+        self.world = world
+
+        return
+
+    def plot_results(self, plot_mocap, plot_estimator, plot_imu):
+        """
+        Plot the results
+
+        """
+
+        # 3D Paths
+        fig = plt.figure('3D Path')
+        ax = Axes3Ds(fig)
+        self.world.draw(ax)
+        ax.plot3D(self.x[:,0], self.x[:,1], self.x[:,2], 'b.')
+        ax.plot3D(self.x_des[:,0], self.x_des[:,1], self.x_des[:,2], 'k')
+
+        # Position and Velocity vs. Time
+        (fig, axes) = plt.subplots(nrows=2, ncols=1, sharex=True, num='Pos/Vel vs Time')
+        ax = axes[0]
+        ax.plot(self.time, self.x_des[:,0], 'r', self.time, self.x_des[:,1], 'g', self.time, self.x_des[:,2], 'b')
+        ax.plot(self.time, self.x[:,0], 'r.',    self.time, self.x[:,1], 'g.',    self.time, self.x[:,2], 'b.')
+        ax.legend(('x', 'y', 'z'))
+        ax.set_ylabel('position, m')
+        ax.grid('major')
+        ax.set_title('Position')
+        ax = axes[1]
+        ax.plot(self.time, self.v_des[:,0], 'r', self.time, self.v_des[:,1], 'g', self.time, self.v_des[:,2], 'b')
+        ax.plot(self.time, self.v[:,0], 'r.',    self.time, self.v[:,1], 'g.',    self.time, self.v[:,2], 'b.')
+        ax.legend(('x', 'y', 'z'))
+        ax.set_ylabel('velocity, m/s')
+        ax.set_xlabel('time, s')
+        ax.grid('major')
+
+        # Orientation and Angular Velocity vs. Time
+        (fig, axes) = plt.subplots(nrows=2, ncols=1, sharex=True, num='Attitude/Rate vs Time')
+        ax = axes[0]
+        ax.plot(self.time, self.q_des[:,0], 'r', self.time, self.q_des[:,1], 'g', self.time, self.q_des[:,2], 'b', self.time, self.q_des[:,3], 'm')
+        ax.plot(self.time, self.q[:,0], 'r.',    self.time, self.q[:,1], 'g.',    self.time, self.q[:,2], 'b.',    self.time, self.q[:,3],     'm.')
+        ax.legend(('i', 'j', 'k', 'w'))
+        ax.set_ylabel('quaternion')
+        ax.set_xlabel('time, s')
+        ax.grid('major')
+        ax = axes[1]
+        ax.plot(self.time, self.w[:,0], 'r.', self.time, self.w[:,1], 'g.', self.time, self.w[:,2], 'b.')
+        ax.legend(('x', 'y', 'z'))
+        ax.set_ylabel('angular velocity, rad/s')
+        ax.set_xlabel('time, s')
+        ax.grid('major')
+
+        if plot_mocap:  # if mocap should be plotted. 
+            # Motion capture position and velocity vs time
+            (fig, axes) = plt.subplots(nrows=2, ncols=1, sharex=True, num='Motion Capture Pos/Vel vs Time')
+            ax = axes[0]
+            ax.plot(self.time, self.x_mc[:,0], 'r.', self.time, self.x_mc[:,1], 'g.',    self.time, self.x_mc[:,2], 'b.')
+            ax.legend(('x', 'y', 'z'))
+            ax.set_ylabel('position, m')
+            ax.grid('major')
+            ax.set_title('MOTION CAPTURE Position/Velocity')
+            ax = axes[1]
+            ax.plot(self.time, self.v_mc[:,0], 'r.',    self.time, self.v_mc[:,1], 'g.',    self.time, self.v_mc[:,2], 'b.')
+            ax.legend(('x', 'y', 'z'))
+            ax.set_ylabel('velocity, m/s')
+            ax.set_xlabel('time, s')
+            ax.grid('major')
+            # Motion Capture Orientation and Angular Velocity vs. Time
+            (fig, axes) = plt.subplots(nrows=2, ncols=1, sharex=True, num='Motion Capture Attitude/Rate vs Time')
+            ax = axes[0]
+            ax.plot(self.time, self.q_mc[:,0], 'r.',    self.time, self.q_mc[:,1], 'g.',    self.time, self.q_mc[:,2], 'b.',    self.time, self.q_mc[:,3],     'm.')
+            ax.legend(('i', 'j', 'k', 'w'))
+            ax.set_ylabel('quaternion')
+            ax.set_xlabel('time, s')
+            ax.grid('major')
+            ax.set_title("MOTION CAPTURE Attitude/Rate")
+            ax = axes[1]
+            ax.plot(self.time, self.w_mc[:,0], 'r.', self.time, self.w_mc[:,1], 'g.', self.time, self.w_mc[:,2], 'b.')
+            ax.legend(('x', 'y', 'z'))
+            ax.set_ylabel('angular velocity, rad/s')
+            ax.set_xlabel('time, s')
+            ax.grid('major')
+
+        # Commands vs. Time
+        (fig, axes) = plt.subplots(nrows=3, ncols=1, sharex=True, num='Commands vs Time')
+        ax = axes[0]
+        ax.plot(self.time, self.s_des[:,0], 'r', self.time, self.s_des[:,1], 'g', self.time, self.s_des[:,2], 'b',  self.time, self.s_des[:,3], 'k')
+        ax.plot(self.time, self.s[:,0], 'r.',    self.time, self.s[:,1], 'g.',    self.time, self.s[:,2], 'b.',     self.time, self.s[:,3], 'k.')
+        ax.legend(('1', '2', '3', '4'))
+        ax.set_ylabel('motor speeds, rad/s')
+        ax.grid('major')
+        ax.set_title('Commands')
+        ax = axes[1]
+        ax.plot(self.time, self.M[:,0], 'r.', self.time, self.M[:,1], 'g.', self.time, self.M[:,2], 'b.')
+        ax.legend(('x', 'y', 'z'))
+        ax.set_ylabel('moment, N*m')
+        ax.grid('major')
+        ax = axes[2]
+        ax.plot(self.time, self.T, 'k.')
+        ax.set_ylabel('thrust, N')
+        ax.set_xlabel('time, s')
+        ax.grid('major')
+
+        # Winds
+        (fig, axes) = plt.subplots(nrows=3, ncols=1, sharex=True, num='Winds vs Time')
+        ax = axes[0]
+        ax.plot(self.time, self.wind[:,0], 'r')
+        ax.set_ylabel("wind X, m/s")
+        ax.grid('major')
+        ax.set_title('Winds')
+        ax = axes[1]
+        ax.plot(self.time, self.wind[:,1], 'g')
+        ax.set_ylabel("wind Y, m/s")
+        ax.grid('major')
+        ax = axes[2]
+        ax.plot(self.time, self.wind[:,2], 'b')
+        ax.set_ylabel("wind Z, m/s")
+        ax.set_xlabel("time, s")
+        ax.grid('major')
+
+        # IMU sensor
+        if plot_imu:
+            (fig, axes) = plt.subplots(nrows=2, ncols=1, sharex=True, num="IMU Measurements vs Time")
+            ax = axes[0]
+            ax.plot(self.time, self.accel[:,0], 'r.', self.time, self.accel[:,1], 'g.', self.time, self.accel[:,2], 'b.')
+            ax.plot(self.time, self.accel_gt[:,0], 'k', self.time, self.accel_gt[:,1], 'c', self.time, self.accel_gt[:,2], 'm')
+            ax.set_ylabel("linear acceleration, m/s/s")
+            ax.grid()
+            ax = axes[1]
+            ax.plot(self.time, self.gyro[:,0], 'r.', self.time, self.gyro[:,1], 'g.', self.time, self.gyro[:,2], 'b.')
+            ax.set_ylabel("angular velocity, rad/s")
+            ax.grid()
+            ax.legend(('x','y','z'))
+            ax.set_xlabel("Time, s")
+
+        if plot_estimator:
+            if self.estimator_exists:
+                N_filter = self.filter_state.shape[1]
+                (fig, axes) = plt.subplots(nrows=N_filter, ncols=1, sharex=True, num="Filter States vs Time")
+                fig.set_size_inches(11, 8.5)
+                for i in range(N_filter):
+                    ax = axes[i]
+                    ax.plot(self.time, self.filter_state[:,i], 'k', )
+                    ax.fill_between(self.time, self.filter_state[:,i]-self.sd[:,i], self.filter_state[:,i]+self.sd[:,i], alpha=0.3, color='k')
+                    ax.set_ylabel("x"+str(i))
+                ax.set_xlabel("Time, s")
+
+                (fig, axes) = plt.subplots(nrows=N_filter, ncols=1, sharex=True, num="Filter Covariance vs Time")
+                fig.set_size_inches(11, 8.5)
+                for i in range(N_filter):
+                    ax = axes[i]
+                    ax.plot(self.time, self.sd[:,i]**2, 'k', )
+                    ax.set_ylabel("cov(x"+str(i)+")")
+                ax.set_xlabel("Time, s")
+
+        plt.show()
+
+        return
+
+    def animate_results(self, fname=None):
+        """
+        Animate the results
+        
+        """
+
+        # Animation (Slow)
+        # Instead of viewing the animation live, you may provide a .mp4 filename to save.
+        ani = animate(self.time, self.x, self.R, world=self.world, filename=fname)
+        plt.show()
+
+        return
+
+    def unpack_results(self, result):
+
+        # Unpack the dictionary of results
+        time                = result['time']
+        state               = result['state']
+        control             = result['control']
+        flat                = result['flat']
+        imu_measurements    = result['imu_measurements']
+        imu_gt              = result['imu_gt']
+        mocap               = result['mocap_measurements']
+        state_estimate      = result['state_estimate']
+
+        # Unpack each result into NumPy arrays
+        x = state['x']
+        x_des = flat['x']
+        v = state['v']
+        v_des = flat['x_dot']
+
+        q = state['q']
+        q_des = control['cmd_q']
+        w = state['w']
+
+        s_des = control['cmd_motor_speeds']
+        s = state['rotor_speeds']
+        M = control['cmd_moment']
+        T = control['cmd_thrust']
+
+        wind = state['wind']
+
+        accel   = imu_measurements['accel']
+        gyro    = imu_measurements['gyro']
+
+        accel_gt = imu_gt['accel']
+
+        x_mc = mocap['x']
+        v_mc = mocap['v']
+        q_mc = mocap['q']
+        w_mc = mocap['w']
+
+        filter_state = state_estimate['filter_state']
+        covariance = state_estimate['covariance']
+        if filter_state.shape[1] > 0:
+            sd = 3*np.sqrt(np.diagonal(covariance, axis1=1, axis2=2))
+            self.estimator_exists = True
+        else:
+            sd = []
+            self.estimator_exists = False
+
+        return (time, x, x_des, v, v_des, q, q_des, w, s, s_des, M, T, wind, accel, gyro, accel_gt, x_mc, v_mc, q_mc, w_mc, filter_state, covariance, sd)

rotorpy/utils/postprocessing.py

@@ -0,0 +1,106 @@
+import pandas as pd
+import numpy as np
+from scipy.spatial.transform import Rotation
+import os
+
+
+def unpack_sim_data(result):
+    """
+    Unpacks the simulated result dictionary and converts it to a Pandas DataFrame
+
+    """
+    
+    headers = [ 'time',                                                                                                                                                         # Time
+            'x', 'y', 'z', 'xdot', 'ydot', 'zdot', 'qx', 'qy', 'qz', 'qw', 'wx', 'wy', 'wz', 'windx', 'windy', 'windz', 'r1', 'r2', 'r3', 'r4',                             # Vehicle state                                                                                                                                               # GT body velocity
+            'xdes', 'ydes', 'zdes', 'xdotdes', 'ydotdes', 'zdotdes', 'xddotdes', 'yddotdes', 'zddotdes', 'xdddotdes', 'ydddotdes', 'zdddotdes',                             # Flat outputs
+            'xddddotdes', 'yddddotdes', 'zddddotdes', 'yawdes', 'yawdotdes', 
+            'ax', 'ay', 'az', 'ax_gt', 'ay_gt', 'az_gt', 'gx', 'gy', 'gz',                                                                                                  # IMU measurements
+            'mocap_x', 'mocap_y', 'mocap_z', 'mocap_xdot', 'mocap_ydot', 'mocap_zdot', 'mocap_qx', 'mocap_qy', 'mocap_qz', 'mocap_qw', 'mocap_wx', 'mocap_wy', 'mocap_wz',  # Mocap measurements
+            'r1des', 'r2des', 'r3des', 'r4des', 'thrustdes', 'qxdes', 'qydes', 'qzdes', 'qwdes', 'mxdes', 'mydes', 'mzdes',                                                 # Controller
+    ]
+
+    # Extract data into numpy arrays
+    time                = result['time'].reshape(-1,1)
+    state               = result['state']
+    x               = state['x']
+    v               = state['v']
+    q               = state['q']
+    w               = state['w']
+    wind            = state['wind']
+    rotor_speeds    = state['rotor_speeds']
+    control             = result['control']
+    cmd_rotor   = control['cmd_motor_speeds']
+    cmd_thrust  = control['cmd_thrust'].reshape(-1,1)
+    cmd_q       = control['cmd_q']
+    cmd_moment  = control['cmd_moment']
+
+    flat                = result['flat']
+    x_des       = flat['x']
+    v_des       = flat['x_dot']
+    a_des       = flat['x_ddot']
+    j_des       = flat['x_dddot']
+    s_des       = flat['x_ddddot']
+    yaw_des     = flat['yaw'].reshape(-1,1)
+    yawdot_des  = flat['yaw_dot'].reshape(-1,1)
+
+    imu_measurements    = result['imu_measurements']
+    a_measured = imu_measurements['accel']
+    w_measured = imu_measurements['gyro']
+
+    mocap_measurements = result['mocap_measurements']
+    x_mc = mocap_measurements['x']
+    v_mc = mocap_measurements['v']
+    q_mc = mocap_measurements['q']
+    w_mc = mocap_measurements['w']
+
+    imu_actual          = result['imu_gt']
+    a_actual = imu_actual['accel']
+
+    state_estimate      = result['state_estimate']
+    filter_state = state_estimate['filter_state']
+    covariance = state_estimate['covariance']
+
+    if filter_state.shape[1] > 0:
+        # Computes the standard deviation of the filter covariance diagonal elements
+        sd = 3*np.sqrt(np.diagonal(covariance, axis1=1, axis2=2))
+        headers.extend(['xhat_'+str(i) for i in range(filter_state.shape[1])])
+        headers.extend(['sigma_'+str(i) for i in range(filter_state.shape[1])])
+
+        dataset = np.hstack((time,
+                             x, v, q, w, wind, rotor_speeds,
+                             x_des, v_des, a_des, j_des, s_des, yaw_des, yawdot_des, 
+                             a_measured, a_actual, w_measured, 
+                             x_mc, v_mc, q_mc, w_mc,
+                             cmd_rotor, cmd_thrust, cmd_q, cmd_moment, 
+                             filter_state, sd))
+    else:
+        sd = []
+
+        dataset = np.hstack((time,
+                            x, v, q, w, wind, rotor_speeds,
+                            x_des, v_des, a_des, j_des, s_des, yaw_des, yawdot_des, 
+                            a_measured, a_actual, w_measured, 
+                            x_mc, v_mc, q_mc, w_mc,
+                            cmd_rotor, cmd_thrust, cmd_q, cmd_moment))
+    df = pd.DataFrame(data=dataset,
+                        columns=headers)
+    
+    return df
+
+def save_sim_data(result, filename="output.csv"):
+    """
+    Saves the data in result as a csv file for post processing.
+    """
+
+    if not (".csv" in filename):
+        # Check whether or not filename contains .csv
+        filename = filename + ".csv"
+
+    # Get dataframe from simulation data. 
+    df = unpack_sim_data(result)
+
+    # Send it to csv file and save it in /rotorpy/data_out/ file. 
+    path = os.path.join(os.path.dirname(__file__), '..', 'data_out/'+filename)
+    df.to_csv(path)
+
+    return 

rotorpy/utils/shapes.py

@@ -0,0 +1,437 @@
+"""
+Parametric 3D shapes for spatial plots and animations. Shapes are drawn on an
+Axes3D axes, and then can be moved using .transform(). They can return a list of
+artists to support blitting in animations.
+
+TODO:
+  There is a fair amount of code duplication here; a superclass may be warranted.
+
+"""
+
+import itertools
+import numpy as np
+from mpl_toolkits.mplot3d import art3d
+import matplotlib.colors as mcolors
+from scipy.spatial.transform import Rotation
+
+class Face():
+
+    def __init__(self, ax, corners, *,
+        shade=True,
+        alpha=1.0,
+        facecolors=None,
+        edgecolors=None,
+        linewidth=0,
+        antialiased=True):
+        """
+        Parameters
+            ax, Axes3D to contain new shape
+            corners, shape=(N,3)
+            shade, shade faces using default lightsource, default is True
+            linewidth, width of lines, default is 0
+            alpha, transparency value in domain [0,1], default is 1.0
+            edgecolors, color of edges
+            facecolors, color of faces
+            antialiased, smoother edge lines, default is True
+        """
+        self.shade = shade
+        self.facecolors = facecolors
+
+        self.ax = ax
+
+        if self.facecolors is None:
+            self.facecolors = self.ax._get_lines.get_next_color()
+        self.facecolors = np.array(mcolors.to_rgba(self.facecolors))
+
+        # Precompute verticies and normal vectors in reference configuration.
+        self.verts = np.reshape(corners, (1, -1, 3))
+        self.normals = np.asarray(self.ax._generate_normals(self.verts))
+
+        # Instantiate and add collection.
+        self.polyc = art3d.Poly3DCollection(self.verts, linewidth=linewidth, antialiased=antialiased, alpha=alpha, edgecolors=edgecolors, facecolors=self.facecolors)
+        self.artists = (self.polyc,)
+        self.transform(np.zeros((3,)), np.identity(3))
+        self.ax.add_collection(self.polyc)
+
+    def transform(self, position, rotation=np.identity(3)):
+
+        position = np.array(position)
+        position.shape = (3,1)
+
+        # The verts array is indexed as (i_face, j_coordinate, k_point).
+        verts = np.swapaxes(self.verts, 1, 2)
+        new_verts = np.matmul(rotation, verts) + position
+        self.polyc.set_verts(np.swapaxes(new_verts, 1, 2))
+
+        if self.shade:
+            normals = np.matmul(rotation, self.normals.T).T
+            colset = self.ax._shade_colors(self.facecolors, normals)
+        else:
+            colset = self.facecolors
+        self.polyc.set_facecolors(colset)
+
+class Cuboid():
+
+    def __init__(self, ax, x_span, y_span, z_span, *,
+        shade=True,
+        alpha=1.0,
+        facecolors=None,
+        edgecolors=None,
+        linewidth=0,
+        antialiased=True):
+        """
+        Parameters
+            ax, Axes3D to contain new shape
+            x_span, width in x-direction
+            y_span, width in y-direction
+            z_span, width in z-direction
+            shade, shade faces using default lightsource, default is True
+            linewidth, width of lines, default is 0
+            alpha, transparency value in domain [0,1], default is 1.0
+            edgecolors, color of edges
+            facecolors, color of faces
+            antialiased, smoother edge lines, default is True
+        """
+        self.shade = shade
+        self.facecolors = facecolors
+
+        self.ax = ax
+
+        if self.facecolors is None:
+            self.facecolors = self.ax._get_lines.get_next_color()
+        self.facecolors = np.array(mcolors.to_rgba(self.facecolors))
+
+        # Precompute verticies and normal vectors in reference configuration.
+        self.verts = self.build_verts(x_span, y_span, z_span)
+        self.normals = np.asarray(self.ax._generate_normals(self.verts))
+
+        # Instantiate and add collection.
+        self.polyc = art3d.Poly3DCollection(self.verts, linewidth=linewidth, antialiased=antialiased, alpha=alpha, edgecolors=edgecolors, facecolors=self.facecolors)
+        self.artists = (self.polyc,)
+        self.transform(np.zeros((3,)), np.identity(3))
+        self.ax.add_collection(self.polyc)
+
+    def transform(self, position, rotation=np.identity(3)):
+
+        position = np.array(position)
+        position.shape = (3,1)
+
+        # The verts array is indexed as (i_face, j_coordinate, k_point).
+        verts = np.swapaxes(self.verts, 1, 2)
+        new_verts = np.matmul(rotation, verts) + position
+        self.polyc.set_verts(np.swapaxes(new_verts, 1, 2))
+
+        if self.shade:
+            normals = np.matmul(rotation, self.normals.T).T
+            colset = self.ax._shade_colors(self.facecolors, normals)
+        else:
+            colset = self.facecolors
+        self.polyc.set_facecolors(colset)
+
+    def build_verts(self, x_span, y_span, z_span):
+        """
+        Input
+            x_span, width in x-direction
+            y_span, width in y-direction
+            z_span, width in z-direction
+        Returns
+            verts, shape=(6_faces, 4_points, 3_coordinates)
+        """
+
+        # Coordinates of each point.
+        (x, y, z) = (x_span, y_span, z_span)
+        bot_pts = np.array([
+            [0, 0, 0],
+            [x, 0, 0],
+            [x, y, 0],
+            [0, y, 0]])
+        top_pts = np.array([
+            [0, 0, z],
+            [x, 0, z],
+            [x, y, z],
+            [0, y, z]])
+        pts = np.concatenate((bot_pts, top_pts), axis=0)
+
+        # Indices of points for each face.
+        side_faces = [(i, (i+1)%4, 4+((i+1)%4), 4+i) for i in range(4)]
+        side_faces = np.array(side_faces, dtype=int)
+        bot_faces = np.arange(4, dtype=int)
+        bot_faces.shape = (1,4)
+        top_faces = 4 + bot_faces
+        all_faces = np.concatenate((side_faces, bot_faces, top_faces), axis=0)
+
+        # Vertex list.
+        xt = pts[:,0][all_faces]
+        yt = pts[:,1][all_faces]
+        zt = pts[:,2][all_faces]
+        verts = np.stack((xt, yt, zt), axis=-1)
+
+        return verts
+
+class Cylinder():
+
+    def __init__(self, ax, radius, height, n_pts=8, shade=True, color=None):
+        self.shade = shade
+
+        self.ax = ax
+
+        if color is None:
+            color = self.ax._get_lines.get_next_color()
+        self.color = np.array(mcolors.to_rgba(color))
+
+        # Precompute verticies and normal vectors in reference configuration.
+        self.verts = self.build_verts(radius, height, n_pts)
+        self.normals = np.asarray(self.ax._generate_normals(self.verts))
+
+        # Instantiate and add collection.
+        self.polyc = art3d.Poly3DCollection(self.verts, color='b', linewidth=0, antialiased=False)
+        self.artists = (self.polyc,)
+        self.transform(np.zeros((3,)), np.identity(3))
+        self.ax.add_collection(self.polyc)
+
+    def transform(self, position, rotation):
+
+        position.shape = (3,1)
+
+        # The verts array is indexed as (i_triangle, j_coordinate, k_point).
+        verts = np.swapaxes(self.verts, 1, 2)
+        new_verts = np.matmul(rotation, verts) + position
+        self.polyc.set_verts(np.swapaxes(new_verts, 1, 2))
+
+        if self.shade:
+            normals = np.matmul(rotation, self.normals.T).T
+            colset = self.ax._shade_colors(self.color, normals)
+        else:
+            colset = self.color
+        self.polyc.set_facecolors(colset)
+
+    def build_verts(self, radius, height, n_pts):
+        """
+        Input
+            radius, radius of cylinder
+            height, height of cylinder
+            n_pts, number of points used to describe rim of cylinder
+        Returns
+            verts, [n_triangles, 3_points, 3_coordinates]
+        """
+
+        theta = np.linspace(0, 2*np.pi, n_pts, endpoint=False)
+        delta_theta = (theta[1]-theta[0])/2
+
+        # Points around the bottom rim, top rim, bottom center, and top center.
+        bot_pts = np.zeros((3, n_pts))
+        bot_pts[0,:] = radius * np.cos(theta)
+        bot_pts[1,:] = radius * np.sin(theta)
+        bot_pts[2,:] = np.full(n_pts, -height/2)
+        top_pts = np.zeros((3, n_pts))
+        top_pts[0,:] = radius * np.cos(theta + delta_theta)
+        top_pts[1,:] = radius * np.sin(theta + delta_theta)
+        top_pts[2,:] = np.full(n_pts, height/2)
+        bot_center = np.array([[0], [0], [-height/2]])
+        top_center = np.array([[0], [0],  [height/2]])
+        pts = np.concatenate((bot_pts, top_pts, bot_center, top_center), axis=1)
+
+        # Triangle indices for the shell.
+        up_triangles = np.stack((
+            np.arange(0, n_pts, dtype=int),
+            np.arange(1, n_pts+1, dtype=int),
+            np.arange(n_pts+0, n_pts+n_pts, dtype=int)))
+        up_triangles[1,-1] = 0
+        down_triangles = np.stack((
+            np.arange(0, n_pts, dtype=int),
+            np.arange(n_pts, n_pts+n_pts, dtype=int),
+            np.arange(n_pts-1, n_pts+n_pts-1, dtype=int)))
+        down_triangles[2,0] = n_pts+n_pts-1
+        shell_triangles = np.concatenate((up_triangles, down_triangles), axis=1)
+
+        # Triangle indices for the bottom.
+        bot_triangles = np.stack((
+            np.arange(0, n_pts, dtype=int),
+            np.arange(1, n_pts+1, dtype=int),
+            np.full(n_pts, 2*n_pts, dtype=int)))
+        bot_triangles[1,-1] = 0
+        top_triangles = np.stack((
+            np.arange(n_pts+0, n_pts+n_pts, dtype=int),
+            np.arange(n_pts+1, n_pts+n_pts+1, dtype=int),
+            np.full(n_pts, 2*n_pts+1, dtype=int)))
+        top_triangles[1,-1] = n_pts
+
+        all_triangles = np.concatenate((shell_triangles, bot_triangles, top_triangles), axis=1)
+
+        xt = pts[0,:][all_triangles.T]
+        yt = pts[1,:][all_triangles.T]
+        zt = pts[2,:][all_triangles.T]
+        verts = np.stack((xt, yt, zt), axis=-1)
+
+        return verts
+
+class Quadrotor():
+
+    def __init__(self, ax,
+        arm_length=0.125, rotor_radius=0.08, n_rotors=4,
+        shade=True, color=None):
+
+        self.ax = ax
+
+        # Apply same color to all rotor objects.
+        if color is None:
+            color = self.ax._get_lines.get_next_color()
+        self.color = np.array(mcolors.to_rgba(color))
+
+        # Precompute positions and rotations in the reference configuration.
+        theta = np.linspace(0, 2*np.pi, n_rotors, endpoint=False)
+        theta = theta + np.mean(theta[:2])
+        self.rotor_position = np.zeros((3, n_rotors))
+        self.rotor_position[0,:] = arm_length*np.cos(theta)
+        self.rotor_position[1,:] = arm_length*np.sin(theta)
+
+        # Instantiate.
+        self.rotors = [Cylinder(ax,
+                                rotor_radius,
+                                0.1*rotor_radius,
+                                shade=shade,
+                                color=color) for _ in range(n_rotors)]
+        self.artists = tuple(itertools.chain.from_iterable(r.artists for r in self.rotors))
+        self.transform(np.zeros((3,)), np.identity(3))
+
+    def transform(self, position, rotation):
+        position.shape = (3,1)
+        for (r, pos) in zip(self.rotors, self.rotor_position.T):
+            pos.shape = (3,1)
+            r.transform(np.matmul(rotation,pos)+position, rotation)
+
+if __name__ == '__main__':
+    from axes3ds import Axes3Ds
+    import matplotlib.pyplot as plt
+
+    # Test Face
+    fig = plt.figure(num=4, clear=True)
+    ax = Axes3Ds(fig)
+    corners = np.array([(1,1,1), (-1,1,1), (-1,-1,1), (1,-1,1)])
+    z_plus_face = Face(ax, corners=corners, facecolors='b')
+    x_plus_face = Face(ax, corners=corners, facecolors='r')
+    x_plus_face.transform(
+        position=(0,0,0),
+        rotation=Rotation.from_rotvec(np.pi/2 * np.array([0, 1, 0])).as_matrix())
+    y_plus_face = Face(ax, corners=corners, facecolors='g')
+    y_plus_face.transform(
+        position=(0,0,0),
+        rotation=Rotation.from_rotvec(np.pi/2 * np.array([-1, 0, 0])).as_matrix())
+    ax.set_xlim(-2,2)
+    ax.set_ylim(-2,2)
+    ax.set_zlim(-2,2)
+    ax.set_xlabel('x')
+    ax.set_ylabel('y')
+    ax.set_zlabel('z')
+
+    # Test Cuboid
+    fig = plt.figure(num=0, clear=True)
+    ax = Axes3Ds(fig)
+    cuboid = Cuboid(ax, x_span=1, y_span=1, z_span=1)
+    cuboid.transform(position=np.array([[0, 0, 0]]), rotation=np.identity(3))
+    rotation = Rotation.from_rotvec(np.pi/4 * np.array([1, 0, 0])).as_matrix()
+    cuboid = Cuboid(ax, x_span=1, y_span=2, z_span=3)
+    cuboid.transform(position=np.array([[2.0, 1, 1]]), rotation=rotation)
+    ax.set_xlim(-1,3)
+    ax.set_ylim(-1,3)
+    ax.set_zlim(-1,3)
+
+    # Test Cylinder
+    fig = plt.figure(num=1, clear=True)
+    ax = Axes3Ds(fig)
+    cylinder = Cylinder(ax, radius=0.2, height=0.2)
+    cylinder.transform(position=np.array([[0, 0, 0]]), rotation=np.identity(3))
+    rotation = Rotation.from_rotvec(np.pi/4 * np.array([1, 0, 0])).as_matrix()
+    cylinder = Cylinder(ax, radius=0.2, height=0.2)
+    cylinder.transform(position=np.array([[1.0, 0, 0]]), rotation=rotation)
+    ax.set_xlim(-1,1)
+    ax.set_ylim(-1,1)
+    ax.set_zlim(-1,1)
+
+    # Test Quadrotor
+    fig = plt.figure(num=2, clear=True)
+    ax = Axes3Ds(fig)
+    quad = Quadrotor(ax)
+    quad.transform(position=np.array([[0.5, 0.5, 0.5]]), rotation=np.identity(3))
+    quad = Quadrotor(ax)
+    rotation = Rotation.from_rotvec(np.pi/4 * np.array([1, 0, 0])).as_matrix()
+    quad.transform(position=np.array([[0.8, 0.8, 0.8]]), rotation=rotation)
+    ax.set_xlim(-1,1)
+    ax.set_ylim(-1,1)
+    ax.set_zlim(-1,1)
+
+    # Test Cuboid coloring.
+    fig = plt.figure(num=3, clear=True)
+    ax = Axes3Ds(fig)
+    ax.set_xlim(-3.25,3.25)
+    ax.set_ylim(-3.25,3.25)
+    ax.set_zlim(-3.25,3.25)
+    ax.set_xlabel('x')
+    ax.set_ylabel('y')
+    ax.set_zlabel('z')
+
+    # No shading.
+    x = (-1, 0, 1)
+    z = -3.25
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, shade=False)
+    cuboid.transform(position=(x[0], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, shade=False)
+    cuboid.transform(position=(x[1], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, shade=False, facecolors='b')
+    cuboid.transform(position=(x[2], 0, z))
+
+    # Shading.
+    z = z + 1
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5)
+    cuboid.transform(position=(x[0], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5)
+    cuboid.transform(position=(x[1], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, facecolors='b')
+    cuboid.transform(position=(x[2], 0, z))
+
+    # Transparency.
+    z = z + 1
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, alpha=0.5)
+    cuboid.transform(position=(x[0], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, alpha=0.5)
+    cuboid.transform(position=(x[1], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, alpha=0.5, facecolors='b')
+    cuboid.transform(position=(x[2], 0, z))
+
+    # No shading, edge lines.
+    z = z + 1
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, shade=False, linewidth=1)
+    cuboid.transform(position=(x[0], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, shade=False, linewidth=1, edgecolors='k')
+    cuboid.transform(position=(x[1], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, shade=False, linewidth=1, facecolors='b', edgecolors='k')
+    cuboid.transform(position=(x[2], 0, z))
+
+    # Shading, edge lines.
+    z = z + 1
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, linewidth=1)
+    cuboid.transform(position=(x[0], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, linewidth=1, edgecolors='k')
+    cuboid.transform(position=(x[1], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, linewidth=1, facecolors='b', edgecolors='k')
+    cuboid.transform(position=(x[2], 0, z))
+
+    # Transparency, edge lines.
+    z = z + 1
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, alpha=0.5, linewidth=1)
+    cuboid.transform(position=(x[0], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, alpha=0.5, linewidth=1, edgecolors='k')
+    cuboid.transform(position=(x[1], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, alpha=0.5, linewidth=1, facecolors='b', edgecolors='k')
+    cuboid.transform(position=(x[2], 0, z))
+
+    # Transparent edges.
+    z = z + 1
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, alpha=0, linewidth=1, edgecolors='k', antialiased=False)
+    cuboid.transform(position=(x[1], 0, z))
+    cuboid = Cuboid(ax, x_span=0.5, y_span=0.5, z_span=0.5, alpha=0, linewidth=1, edgecolors='k', antialiased=True)
+    cuboid.transform(position=(x[2], 0, z))
+
+
+    # Draw all figures.
+    plt.show()

rotorpy/utils/system_info.py

@@ -0,0 +1,23 @@
+print('The below information will help debug installation problems.\n')
+
+import sys
+print('Python Version:')
+print(sys.version)
+print()
+
+import platform
+print('Operating system:')
+print(platform.platform())
+print()
+
+import matplotlib
+print('matplotlib:', matplotlib.__version__)
+
+import numpy
+print('numpy:     ', numpy.__version__)
+
+import pip
+print('pip:       ', pip.__version__)
+
+import scipy
+print('scipy:     ', scipy.__version__)

rotorpy/vehicles/README.md

@@ -0,0 +1,7 @@
+# Vehicle Module
+
+Vehicle classes are where the continuous-time dynamics are stored. These dynamics are integrated using the `step` method. The IMU requires a `statedot` method which outputs the acceleration and body rate to be transformed by the sensor. 
+
+Besides that, integration must also be handled in `step`, which is done via `scipy.integrate.solve_ivp` [(reference)](https://docs.scipy.org/doc/scipy/reference/generated/scipy.integrate.solve_ivp.html#r179348322575-1). This is an RK45 method with adaptive step. It is recommended that other vehicles use this method for integration as well, since it preserves the integration accuracy for larger timesteps compared to forward/backward Euler. 
+
+Currently `Multirotor` is the only vehicle supported with automatic plotting and saving of the states and controller outputs.

rotorpy/vehicles/crazyflie_params.py

@@ -0,0 +1,65 @@
+"""
+Physical parameters for a quadrotor. Values parameterize the inertia, motor dynamics, 
+rotor aerodynamics, parasitic drag, and rotor placement. 
+Additional sources:
+    https://bitcraze.io/2015/02/measuring-propeller-rpm-part-3
+    https://wiki.bitcraze.io/misc:investigations:thrust
+    https://commons.erau.edu/cgi/viewcontent.cgi?article=2057&context=publication
+Notes:
+    k_thrust is inferred from 14.5 g thrust at 2500 rad/s
+    k_drag is mostly made up
+"""
+import numpy as np
+
+d = 0.043
+
+quad_params = {
+
+    # Inertial properties
+    'mass': 0.03,       # kg
+    'Ixx':  1.43e-5,    # kg*m^2
+    'Iyy':  1.43e-5,    # kg*m^2
+    'Izz':  2.89e-5,    # kg*m^2
+    'Ixy':  0.0,        # kg*m^2
+    'Iyz':  0.0,        # kg*m^2 
+    'Ixz':  0.0,        # kg*m^2
+
+    # Geometric properties, all vectors are relative to the center of mass.
+    'num_rotors': 4,                        # for looping over each actuator
+    # 'rotor_pos': {  
+    #                 'r1': np.array([0.046,  0,      0]),    # Location of Rotor 1, meters
+    #                 'r2': np.array([0,      0.046,  0]),    # Location of Rotor 2, meters
+    #                 'r3': np.array([-0.046, 0,      0]),    # Location of Rotor 3, meters
+    #                 'r4': np.array([0,      -0.046, 0]),    # Location of Rotor 4, meters
+    #              },
+    'rotor_pos': {  
+                    'r1': d*np.array([ 0.70710678118, 0.70710678118, 0]),    # Location of Rotor 1, meters
+                    'r2': d*np.array([ 0.70710678118,-0.70710678118, 0]),    # Location of Rotor 2, meters
+                    'r3': d*np.array([-0.70710678118,-0.70710678118, 0]),    # Location of Rotor 3, meters
+                    'r4': d*np.array([-0.70710678118, 0.70710678118, 0]),    # Location of Rotor 4, meters
+                 },
+
+    'rotor_directions': np.array([1,-1,1,-1]),  # This dictates the direction of the torque for each motor. 
+
+    'rI': np.array([0,0,0]), # location of the IMU sensor, meters
+
+    # Frame aerodynamic properties
+    'c_Dx': 0.5e-2,  # parasitic drag in body x axis, N/(m/s)**2
+    'c_Dy': 0.5e-2,  # parasitic drag in body y axis, N/(m/s)**2
+    'c_Dz': 1e-2,  # parasitic drag in body z axis, N/(m/s)**2
+
+    # Rotor properties
+    # See "System Identification of the Crazyflie 2.0 Nano Quadrocopter", Forster 2015.
+    'k_eta': 2.3e-08,           # thrust coefficient N/(rad/s)**2
+    'k_m':   7.8e-10,           # yaw moment coefficient Nm/(rad/s)**2
+    'k_d':   10.2506e-07,       # rotor drag coefficient N/(rad*m/s**2) = kg/rad
+    'k_z':   7.553e-07,         # induced inflow coefficient N/(rad*m/s**2) = kg/rad
+    'k_flap': 0.0,              # Flapping moment coefficient Nm/(rad*m/s**2) = kg*m/rad
+
+    # Motor properties
+    'tau_m': 0.005,           # motor response time, seconds
+    'rotor_speed_min': 0,       # rad/s
+    'rotor_speed_max': 2500,    # rad/s
+    'motor_noise_std': 100,     # rad/s
+
+}

rotorpy/vehicles/hummingbird_params.py

@@ -0,0 +1,58 @@
+"""
+Physical parameters for the AscTec Hummingbird. Values parameterize the inertia, motor dynamics, 
+rotor aerodynamics, parasitic drag, and rotor placement. 
+Additional sources:
+    https://digitalrepository.unm.edu/cgi/viewcontent.cgi?article=1189&context=ece_etds
+    https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=7991501
+Notes:
+    The k_d and k_z terms are an order of magnitude smaller because 10^-3 was too large. 
+"""
+import numpy as np
+
+d = 0.17
+
+quad_params = {
+
+    # Inertial properties
+    'mass': 0.500,       # kg
+    'Ixx':  3.65e-3,    # kg*m^2
+    'Iyy':  3.68e-3,    # kg*m^2
+    'Izz':  7.03e-3,    # kg*m^2
+    'Ixy':  0.0,        # kg*m^2
+    'Iyz':  0.0,        # kg*m^2 
+    'Ixz':  0.0,        # kg*m^2
+
+    # Geometric properties, all vectors are relative to the center of mass.
+    'num_rotors': 4,                        # for looping over each actuator
+    'rotor_radius': 0.10,                   # rotor radius, in meters
+    'rotor_pos': {  
+                    'r1': d*np.array([ 0.70710678118, 0.70710678118, 0]),    # Location of Rotor 1, meters
+                    'r2': d*np.array([ 0.70710678118,-0.70710678118, 0]),    # Location of Rotor 2, meters
+                    'r3': d*np.array([-0.70710678118,-0.70710678118, 0]),    # Location of Rotor 3, meters
+                    'r4': d*np.array([-0.70710678118, 0.70710678118, 0]),    # Location of Rotor 4, meters
+                 },
+
+    'rotor_directions': np.array([1,-1,1,-1]),  # This dictates the direction of the torque for each motor. 
+
+    'rI': np.array([0,0,0]), # location of the IMU sensor, meters
+
+    # Frame aerodynamic properties
+    'c_Dx': 0.5e-2,  # parasitic drag in body x axis, N/(m/s)**2
+    'c_Dy': 0.5e-2,  # parasitic drag in body y axis, N/(m/s)**2
+    'c_Dz': 1e-2,  # parasitic drag in body z axis, N/(m/s)**2
+
+    # Rotor properties
+    # See "System Identification of the Crazyflie 2.0 Nano Quadrocopter", Forster 2015.
+    'k_eta': 5.57e-06,          # thrust coefficient N/(rad/s)**2
+    'k_m':   1.36e-07,          # yaw moment coefficient Nm/(rad/s)**2
+    'k_d':   1.19e-04,          # rotor drag coefficient N/(rad*m/s**2) = kg/rad
+    'k_z':   2.32e-04,         # induced inflow coefficient N/(rad*m/s**2) = kg/rad
+    'k_flap': 0.0,              # Flapping moment coefficient Nm/(rad*m/s**2) = kg*m/rad
+
+    # Motor properties
+    'tau_m': 0.005,           # motor response time, seconds
+    'rotor_speed_min': 0,       # rad/s
+    'rotor_speed_max': 1500,    # rad/s
+    'motor_noise_std': 50,     # rad/s
+
+}

rotorpy/vehicles/multirotor.py

@@ -0,0 +1,313 @@
+import numpy as np
+from numpy.linalg import inv, norm
+import scipy.integrate
+from scipy.spatial.transform import Rotation
+from rotorpy.vehicles.hummingbird_params import quad_params
+
+"""
+Multirotor models
+"""
+
+def quat_dot(quat, omega):
+    """
+    Parameters:
+        quat, [i,j,k,w]
+        omega, angular velocity of body in body axes
+
+    Returns
+        duat_dot, [i,j,k,w]
+
+    """
+    # Adapted from "Quaternions And Dynamics" by Basile Graf.
+    (q0, q1, q2, q3) = (quat[0], quat[1], quat[2], quat[3])
+    G = np.array([[ q3,  q2, -q1, -q0],
+                  [-q2,  q3,  q0, -q1],
+                  [ q1, -q0,  q3, -q2]])
+    quat_dot = 0.5 * G.T @ omega
+    # Augment to maintain unit quaternion.
+    quat_err = np.sum(quat**2) - 1
+    quat_err_grad = 2 * quat
+    quat_dot = quat_dot - quat_err * quat_err_grad
+    return quat_dot
+
+class Multirotor(object):
+    """
+    Quadrotor forward dynamics model.
+    """
+    def __init__(self, quad_params, 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])},
+                ):
+        """
+        Initialize quadrotor physical parameters.
+        """
+
+        # Inertial parameters
+        self.mass            = quad_params['mass'] # kg
+        self.Ixx             = quad_params['Ixx']  # kg*m^2
+        self.Iyy             = quad_params['Iyy']  # kg*m^2
+        self.Izz             = quad_params['Izz']  # kg*m^2
+        self.Ixy             = quad_params['Ixy']  # kg*m^2
+        self.Ixz             = quad_params['Ixz']  # kg*m^2
+        self.Iyz             = quad_params['Iyz']  # kg*m^2
+
+        # Frame parameters
+        self.c_Dx            = quad_params['c_Dx']  # drag coeff, N/(m/s)**2
+        self.c_Dy            = quad_params['c_Dy']  # drag coeff, N/(m/s)**2
+        self.c_Dz            = quad_params['c_Dz']  # drag coeff, N/(m/s)**2
+
+        self.num_rotors      = quad_params['num_rotors']
+        self.rotor_pos       = quad_params['rotor_pos']
+
+        self.rotor_dir       = quad_params['rotor_directions']
+
+        self.extract_geometry()
+
+        # Rotor parameters    
+        self.rotor_speed_min = quad_params['rotor_speed_min'] # rad/s
+        self.rotor_speed_max = quad_params['rotor_speed_max'] # rad/s
+
+        self.k_eta           = quad_params['k_eta']     # thrust coeff, N/(rad/s)**2
+        self.k_m             = quad_params['k_m']       # yaw moment coeff, Nm/(rad/s)**2
+        self.k_d             = quad_params['k_d']       # rotor drag coeff, N/(m/s)
+        self.k_z             = quad_params['k_z']       # induced inflow coeff N/(m/s)
+        self.k_flap          = quad_params['k_flap']    # Flapping moment coefficient Nm/(m/s)
+
+        # Motor parameters
+        self.tau_m           = quad_params['tau_m']     # motor reponse time, seconds
+        self.motor_noise     = quad_params['motor_noise_std'] # noise added to the actual motor speed, rad/s / sqrt(Hz)
+
+        # Additional constants.
+        self.inertia = np.array([[self.Ixx, self.Ixy, self.Ixz],
+                                 [self.Ixy, self.Iyy, self.Iyz],
+                                 [self.Ixz, self.Iyz, self.Izz]])
+        self.rotor_drag_matrix = np.array([[self.k_d,   0,                 0],
+                                           [0,          self.k_d,          0],
+                                           [0,          0,          self.k_z]])
+        self.drag_matrix = np.array([[self.c_Dx,    0,          0],
+                                     [0,            self.c_Dy,  0],
+                                     [0,            0,          self.c_Dz]])
+        self.g = 9.81 # m/s^2
+
+        self.inv_inertia = inv(self.inertia)
+        self.weight = np.array([0, 0, -self.mass*self.g])
+
+        # Set the initial state
+        self.initial_state = initial_state
+
+    def extract_geometry(self):
+        """
+        Extracts the geometry in self.rotors for efficient use later on in the computation of 
+        wrenches acting on the rigid body.
+        The rotor_geometry is an array of length (n,3), where n is the number of rotors. 
+        Each row corresponds to the position vector of the rotor relative to the CoM. 
+        """
+
+        self.rotor_geometry = np.array([]).reshape(0,3)
+        for rotor in self.rotor_pos:
+            r = self.rotor_pos[rotor]
+            self.rotor_geometry = np.vstack([self.rotor_geometry, r])
+
+        return
+
+    def statedot(self, state, cmd_rotor_speeds, t_step):
+        """
+        Integrate dynamics forward from state given constant cmd_rotor_speeds for time t_step.
+        """
+
+        # The true motor speeds can not fall below min and max speeds.
+        cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max)
+
+        # Form autonomous ODE for constant inputs and integrate one time step.
+        def s_dot_fn(t, s):
+            return self._s_dot_fn(t, s, cmd_rotor_speeds)
+        s = Multirotor._pack_state(state)
+        
+        s_dot = s_dot_fn(0, s)
+        v_dot = s_dot[3:6]
+        w_dot = s_dot[10:13]
+
+        state_dot = {'vdot': v_dot,'wdot': w_dot}
+        return state_dot 
+
+
+    def step(self, state, cmd_rotor_speeds, t_step):
+        """
+        Integrate dynamics forward from state given constant cmd_rotor_speeds for time t_step.
+        """
+
+        # The true motor speeds can not fall below min and max speeds.
+        cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max)
+
+        # Form autonomous ODE for constant inputs and integrate one time step.
+        def s_dot_fn(t, s):
+            return self._s_dot_fn(t, s, cmd_rotor_speeds)
+        s = Multirotor._pack_state(state)
+
+        # Option 1 - RK45 integration
+        sol = scipy.integrate.solve_ivp(s_dot_fn, (0, t_step), s, first_step=t_step)
+        s = sol['y'][:,-1]
+        # Option 2 - Euler integration
+        # s = s + s_dot_fn(0, s) * t_step  # first argument doesn't matter. It's time invariant model
+
+        state = Multirotor._unpack_state(s)
+
+        # Re-normalize unit quaternion.
+        state['q'] = state['q'] / norm(state['q'])
+
+        # Add noise to the motor speed measurement
+        state['rotor_speeds'] += np.random.normal(scale=np.abs(self.motor_noise), size=(self.num_rotors,))
+
+        return state
+
+    def _s_dot_fn(self, t, s, cmd_rotor_speeds):
+        """
+        Compute derivative of state for quadrotor given fixed control inputs as
+        an autonomous ODE.
+        """
+
+        state = Multirotor._unpack_state(s)
+
+        rotor_speeds = state['rotor_speeds']
+        inertial_velocity = state['v']
+        wind_velocity = state['wind']
+
+        R = Rotation.from_quat(state['q']).as_matrix()
+
+        # Compute airspeed vector in the body frame
+        body_airspeed_vector = R.T@(inertial_velocity - wind_velocity)
+
+        # Rotor speed derivative
+        rotor_accel = (1/self.tau_m)*(cmd_rotor_speeds - rotor_speeds)
+
+        # Position derivative.
+        x_dot = state['v']
+
+        # Orientation derivative.
+        q_dot = quat_dot(state['q'], state['w'])
+
+        # Compute total wrench in the body frame based on the current rotor speeds and their location w.r.t. CoM
+        (FtotB, Mtot) = self.compute_body_wrench(state['w'], rotor_speeds, body_airspeed_vector)
+
+        # Rotate the force from the body frame to the inertial frame
+        Ftot = R@FtotB
+
+        # Velocity derivative.
+        v_dot = (self.weight + Ftot) / self.mass
+
+        # Angular velocity derivative.
+        w = state['w']
+        w_hat = Multirotor.hat_map(w)
+        w_dot = self.inv_inertia @ (Mtot - w_hat @ (self.inertia @ w))
+
+        # NOTE: the wind dynamics are currently handled in the wind_profile object. 
+        # The line below doesn't do anything, as the wind state is assigned elsewhere. 
+        wind_dot = np.zeros(3,)
+
+        # Pack into vector of derivatives.
+        s_dot = np.zeros((16+self.num_rotors,))
+        s_dot[0:3]   = x_dot
+        s_dot[3:6]   = v_dot
+        s_dot[6:10]  = q_dot
+        s_dot[10:13] = w_dot
+        s_dot[13:16] = wind_dot
+        s_dot[16:]   = rotor_accel
+
+        return s_dot
+
+    def compute_body_wrench(self, body_rates, rotor_speeds, body_airspeed_vector):
+        """
+        Computes the wrench acting on the rigid body based on the rotor speeds for thrust and airspeed 
+        for aerodynamic forces. 
+        The airspeed is represented in the body frame.
+        The net force Ftot is represented in the body frame. 
+        The net moment Mtot is represented in the body frame. 
+        """
+
+        FtotB = np.zeros((3,))
+        MtotB = np.zeros((3,))
+
+        for i in range(self.num_rotors):
+            # Loop through each rotor, compute the forces 
+
+            r = self.rotor_geometry[i,:] # the position of rotor i relative to the CoM, in body coordinates
+            
+            # Compute the local airspeed by adding on the rotational component to the airspeed.
+            local_airspeed_vector = body_airspeed_vector + Multirotor.hat_map(body_rates)@r
+
+            T = np.array([0, 0, self.k_eta*rotor_speeds[i]**2])             # thrust vector in body frame
+            H = -rotor_speeds[i]*self.rotor_drag_matrix@local_airspeed_vector     # rotor drag force
+
+            # Compute the moments
+            M_force = Multirotor.hat_map(r)@(T+H)
+            M_yaw = self.rotor_dir[i]*np.array([0, 0, self.k_m*rotor_speeds[i]**2]) 
+            M_flap = -rotor_speeds[i]*self.k_flap*Multirotor.hat_map(local_airspeed_vector)@np.array([0,0,1])
+
+            FtotB += (T+H)
+            MtotB += (M_force + M_yaw + M_flap)
+
+        # Compute the drag force acting on the frame
+        D = -Multirotor._norm(body_airspeed_vector)*self.drag_matrix@body_airspeed_vector
+
+        FtotB += D
+
+        return (FtotB, MtotB)
+
+    @classmethod
+    def rotate_k(cls, q):
+        """
+        Rotate the unit vector k by quaternion q. This is the third column of
+        the rotation matrix associated with a rotation by q.
+        """
+        return np.array([  2*(q[0]*q[2]+q[1]*q[3]),
+                           2*(q[1]*q[2]-q[0]*q[3]),
+                         1-2*(q[0]**2  +q[1]**2)    ])
+
+    @classmethod
+    def hat_map(cls, s):
+        """
+        Given vector s in R^3, return associate skew symmetric matrix S in R^3x3
+        """
+        return np.array([[    0, -s[2],  s[1]],
+                         [ s[2],     0, -s[0]],
+                         [-s[1],  s[0],     0]])
+
+    @classmethod
+    def _pack_state(cls, state):
+        """
+        Convert a state dict to Quadrotor's private internal vector representation.
+        """
+        s = np.zeros((20,))   # FIXME: this shouldn't be hardcoded. Should vary with the number of rotors. 
+        s[0:3]   = state['x']       # inertial position
+        s[3:6]   = state['v']       # inertial velocity
+        s[6:10]  = state['q']       # orientation
+        s[10:13] = state['w']       # body rates
+        s[13:16] = state['wind']    # wind vector
+        s[16:]   = state['rotor_speeds']     # rotor speeds
+
+        return s
+
+    @classmethod
+    def _norm(cls, v):
+        """
+        Given a vector v in R^3, return the 2 norm (length) of the vector
+        """
+        norm = (v[0]**2 + v[1]**2 + v[2]**2)**0.5
+        return norm
+
+    @classmethod
+    def _unpack_state(cls, s):
+        """
+        Convert Quadrotor's private internal vector representation to a state dict.
+        x = inertial position
+        v = inertial velocity
+        q = orientation
+        w = body rates
+        wind = wind vector
+        rotor_speeds = rotor speeds
+        """
+        state = {'x':s[0:3], 'v':s[3:6], 'q':s[6:10], 'w':s[10:13], 'wind':s[13:16], 'rotor_speeds':s[16:]}
+        return state

rotorpy/wind/README.md

@@ -0,0 +1,11 @@
+# Wind Module
+
+Currently winds are implemented as independent objects that take the current time and the vehicle's location and computes a wind vector at that location. Winds can be spatially varying, temporally varying, or both! 
+
+In `default_winds.py` we have a collection of common gust models, including sinusoid wind signals, constant wind, and discrete step changes in wind. 
+
+`spatial_winds.py` includes an example of a static spatial wind field, in which a column of air resembling a steady state wind tunnel can be produced. 
+
+`dryden_winds.py` contain a collection of models that uses the [Dryden Wind Turbulence Model](https://en.wikipedia.org/wiki/Dryden_Wind_Turbulence_Model) to produce stochastic wind patterns. The model is implemented using code from the external repository: `wind-dynamics` [(link)](https://github.com/goromal/wind-dynamics).
+
+If you would like to create your own wind module, you can do so by using the template found in `wind_template.py`. 

rotorpy/wind/default_winds.py

@@ -0,0 +1,182 @@
+import numpy as np
+import sys
+
+"""
+Below are some default wind objects that might be useful inputs to the system.  
+"""
+
+class NoWind(object):
+    """
+    This wind profile is the trivial case of no wind. It will output 
+    zero wind speed on all axes for all time. 
+    Alternatively, you can use ConstantWind with wx=wy=wz=0. 
+    """
+
+    def __init__(self):
+        """
+        Inputs: 
+            Nothing
+        """
+
+    def update(self, t, position):
+        """
+        Given the present time and position of the multirotor, return the
+        current wind speed on all three axes. 
+        
+        The wind should be expressed in the world coordinates. 
+        """
+        return np.array([0,0,0])
+
+class ConstantWind(object):
+    """
+    This wind profile is constant both spatially and temporally. 
+    Wind speed is specified on each axis. 
+    """
+
+    def __init__(self, wx, wy, wz):
+        """
+        """
+
+        self.wind = np.array([wx, wy, wz])
+
+        
+    def update(self, t, position):
+        """
+        Given the present time and position of the multirotor, return the
+        current wind speed on all three axes. 
+        
+        The wind should be expressed in the world coordinates. 
+        """
+
+        return self.wind
+
+class SinusoidWind(object):
+    """
+    Wind will vary subject to a sine function with appropriate amplitude, frequency, and phase offset.
+    """
+
+    def __init__(self, amplitudes=np.array([1,1,1]), frequencies=np.array([1,1,1]), phase=np.array([0,0,0])):
+        """
+        Inputs: 
+            amplitudes := array of amplitudes on each axis
+            frequencies := array of frequencies for the wind pattern on each axis
+            phase := relative phase offset on each axis in seconds
+        """
+        self.Ax, self.Ay, self.Az = amplitudes[0], amplitudes[1], amplitudes[2]
+        self.fx, self.fy, self.fz = frequencies[0], frequencies[1], frequencies[2]
+        self.px, self.py, self.pz = phase[0], phase[1], phase[2]
+
+    def update(self, t, position):
+        """
+        Given the present time and position of the multirotor, return the
+        current wind speed on all three axes. 
+        
+        The wind should be expressed in the world coordinates. 
+        """
+
+        wind = np.array([self.Ax*np.sin(2*np.pi*self.fx*(t+self.px)),
+                         self.Ay*np.sin(2*np.pi*self.fy*(t+self.py)),
+                         self.Az*np.sin(2*np.pi*self.fz*(t+self.pz))])
+
+        return wind
+
+class LadderWind(object):
+    """
+    The wind will step up and down between a minimum and maximum speed for a specified duration. 
+    Visualized below...
+
+                     | | <- duration
+     max ->          ---         ---
+                  ---         ---
+               ---         ---
+     min -> ---         ---
+            --------------------------> t
+    
+    ** Normally the wind will start at min and increase sequentially, but if the random flag is set true, 
+       the wind will step to a random sublevel after each duration is up. 
+
+    """
+
+    def __init__(self, min=np.array([-1,-1,-1]), max=np.array([1,1,1]), duration=np.array([1,1,1]), Nstep=np.array([5,5,5]), random_flag=False):
+        """
+        Inputs:
+            min := array of minimum wind speeds across each axis
+            max := array of maximum wind speeds across each axis
+            duration := array of durations for each step
+            Nstep := array for the integer number of discretized steps between min and max across each axis
+            start_step := 
+        """
+
+        # Check the inputs for consistency, quit and raise a flag if the inputs aren't physically realizable
+        if np.any(Nstep <= 0):
+            print("LadderWind Error: The number of steps must be greater than or equal to 1")
+            sys.exit(1)
+
+        if np.any(max - min < 0):
+            print("LadderWind Error: The max value must be greater than the min value.")
+            sys.exit(1)
+
+        self.random_flag = random_flag
+        self.durx, self.dury, self.durz = duration[0], duration[1], duration[2]
+        self.nx, self.ny, self.nz = Nstep[0], Nstep[1], Nstep[2]
+
+        # Compute arrays of intermediate wind speeds for each axis
+        self.wx_arr = np.linspace(min[0], max[0], self.nx)
+        self.wy_arr = np.linspace(min[1], max[1], self.ny)
+        self.wz_arr = np.linspace(min[2], max[2], self.nz)
+
+        # Initialize the amplitude id.. these numbers are used to index the arrays above to get the appropriate wind speed on each axis
+        if self.random_flag:
+            self.xid = np.random.choice(self.nx)
+            self.yid = np.random.choice(self.ny)
+            self.zid = np.random.choice(self.nz)
+        else:
+            self.xid, self.yid, self.zid = 0, 0, 0
+
+        # Initialize the timers... since we don't yet know the starting time, we'll set them in the first call
+        self.timerx = None
+        self.timery = None
+        self.timerz = None
+
+        # Initialize the winds
+        self.wx, self.wy, self.wz = self.wx_arr[self.xid], self.wy_arr[self.yid], self.wz_arr[self.zid]
+
+    def update(self, t, position):
+        """
+        Given the present time and position of the multirotor, return the
+        current wind speed on all three axes. 
+        
+        The wind should be expressed in the world coordinates. 
+        """
+        if self.timerx is None:
+            self.timerx, self.timery, self.timerz = t, t, t
+        
+        if (t - self.timerx) >= self.durx:
+            if self.random_flag:
+                self.xid = np.random.choice(self.nx)
+            else:
+                self.xid = (self.xid + 1) % self.nx
+            self.wx = self.wx_arr[self.xid]
+            self.timerx = t
+
+        if (t - self.timery) >= self.dury:
+            if self.random_flag:
+                self.yid = np.random.choice(self.ny)
+            else:
+                self.yid = (self.yid + 1) % self.ny
+            self.wy = self.wy_arr[self.yid]
+            self.timery = t
+
+        if (t - self.timerz) >= self.durz:
+            if self.random_flag:
+                self.zid = np.random.choice(self.nz)
+            else:
+                self.zid = (self.zid + 1) % self.nz
+            self.wz = self.wz_arr[self.zid]
+            self.timerz = t
+
+        return np.array([self.wx, self.wy, self.wz])
+    
+if __name__=="__main__":
+    wind = ConstantWind(wx=1,wy=1,wz=1)
+    print(wind.update(0,np.array([0,0,0])))

rotorpy/wind/dryden_utils.py

@@ -0,0 +1,151 @@
+"""
+Utility scripts for the Dryden wind model
+The Dryden Gust model is implemented using this package: 
+https://github.com/goromal/wind-dynamics
+
+"""
+import numpy as np
+
+class GustModelBase():
+    """
+    """
+
+    def __init__(self, V, L, sigma, dt=0.05):
+        """
+        Inputs: 
+            V, average velocity through medium, m/s
+            L, characteristic length scale, m
+            sigma, turbulence intensity
+        """
+
+        self.dt = dt
+        b = 2*np.sqrt(3)*L/V
+        c = 2*L/V
+
+        self.alpha = sigma*np.sqrt(2*L/np.pi/V)
+        self.beta = self.alpha*b
+        self.delta = 2*c
+        self.gamma = c*c
+
+        self.u_km1 = 0
+        self.u_km2 = 0
+        self.y_km1 = 0
+        self.y_km2 = 0
+
+        self.initialized = True
+        return 
+    
+    def run(self, dt):
+        """
+        """
+
+        C1 = 1.0 + 2*self.delta/dt + 4*self.gamma/dt/dt
+        C2 = 2.0 - 8*self.gamma/dt/dt
+        C3 = 1.0 - 2*self.delta/dt + 4*self.gamma/dt/dt
+        C4 = self.alpha + 2*self.beta/dt
+        C5 = 2*self.alpha
+        C6 = self.alpha - 2*self.beta/dt
+
+        u_k = np.random.uniform(-1,1)
+        y_k = (C4*u_k + C5*self.u_km1 + C6*self.u_km2 - C2*self.y_km1 - C3*self.y_km2)/C1
+
+        self.u_km2 = self.u_km1
+        self.u_km1 = u_k
+        self.y_km2 = self.y_km1
+        self.y_km1 = y_k
+
+        return y_k
+    
+    def integrate(self, dt):
+        """
+        """
+
+        if (dt > self.dt):
+            t = 0
+            y = 0
+            while (t < dt):
+                t_inc = min(self.dt, dt - t)
+                y = self.run(t_inc)
+                t += t_inc
+            return y
+        else:
+            return self.run(dt)
+        
+class DrydenWind():
+    """
+    """
+
+    def __init__(self, wx_nominal, wy_nominal, wz_nominal, 
+                       wx_sigma, wy_sigma, wz_sigma, 
+                       altitude=2.0):
+        
+        self.wx_nominal, self.wy_nominal, self.wz_nominal = wx_nominal, wy_nominal, wz_nominal
+        self.altitude = altitude
+
+        Lz_ft = 3.281 * altitude
+        Lx_ft = Lz_ft / ((0.177 + 0.000823 * Lz_ft)**(1.2))
+        Ly_ft = Lx_ft
+
+        self.wx_gust = GustModelBase(1.0, Lx_ft/3.281, wx_sigma)
+        self.wy_gust = GustModelBase(1.0, Ly_ft/3.281, wy_sigma)
+        self.wz_gust = GustModelBase(1.0, Lz_ft/3.281, wz_sigma)
+
+        self.initialized = True
+
+        return
+    
+    def getWind(self, dt):
+        """
+        """
+
+        if self.initialized:
+            wind_vector = np.array([self.wx_nominal, self.wy_nominal, self.wz_nominal]) + np.array([self.wx_gust.integrate(dt), self.wy_gust.integrate(dt), self.wz_gust.integrate(dt)])
+        else:
+            wind_vector = np.array([0,0,0])
+
+        return wind_vector
+    
+
+if __name__=="__main__":
+
+    import matplotlib.pyplot as plt
+    import numpy as np
+
+    t = 100.0
+    dt = 0.05
+    x_mean = 0.0
+    y_mean = 1.0
+    z_mean = 0.0
+    x_sigma = 10.0
+    y_sigma = 0.0
+    z_sigma = 70.0
+
+    plt.rcParams.update({
+        "text.usetex": True,
+        "font.family": "serif",
+        "font.serif": ["Palatino"],
+    })
+
+    wind = DrydenWind(x_mean, y_mean, z_mean, x_sigma, y_sigma, z_sigma)
+
+    n = int(np.floor(t / dt)) + 1
+    x = np.linspace(0, t, n)
+    y = np.zeros((3,x.size))
+
+    for i in range(n):
+        y[:,i] = wind.getWind(dt)
+
+    fig, axs = plt.subplots(3)
+    fig.suptitle('Dryden Gust Velocities\n$\sigma_x=%f$, $\sigma_y=%f$, $\sigma_z=%f$' % (x_sigma, y_sigma, z_sigma))
+    axs[0].plot([0, t],[x_mean, x_mean])
+    axs[0].plot(x, y[0,:])
+    axs[0].set_ylabel('X Velocity')
+    axs[1].plot([0, t],[y_mean, y_mean])
+    axs[1].plot(x, y[1,:])
+    axs[1].set_ylabel('Y Velocity')
+    axs[2].plot([0, t],[z_mean, z_mean])
+    axs[2].plot(x, y[2,:])
+    axs[2].set_ylabel('Z Velocity')
+    axs[2].set_xlabel('Time (s)')
+
+    plt.show()

rotorpy/wind/dryden_winds.py

@@ -0,0 +1,85 @@
+import numpy as np
+import os
+import sys
+
+# The Dryden Gust model is implemented using this package: 
+# https://github.com/goromal/wind-dynamics
+# If using the package directly, make sure the package is installed and the file ./wind-dynamics/python/wind_dynamics.so 
+# exists. It may be named slightly differently depending on your system. 
+# wind_path = os.path.join(os.path.dirname(__file__),'wind-dynamics/python')
+# sys.path.insert(1, wind_path) # if your script is not in the wind-dynamics/python directory
+# from wind_dynamics import DrydenWind
+
+from rotorpy.wind.dryden_utils import *
+
+class DrydenGust(object):
+    """
+    The Dryden Wind Turbulence model is governed by a pink noise process that is parameterized by 
+    an average (mean) windspeed and standard deviation. This class is a wrapper on an 
+    existing external package: https://github.com/goromal/wind-dynamics 
+    The Dryden model is accepted by the DoD in design, characterization, and testing of aircraft
+    in simulation. https://en.wikipedia.org/wiki/Dryden_Wind_Turbulence_Model
+    """
+
+    def __init__(self, dt=1/500, 
+                    avg_wind=np.array([0,0,0]), sig_wind=np.array([1,1,1]),
+                    altitude=2.0):
+        """
+        Inputs:
+            dt := time discretization, s, should match the simulator
+            avg_wind := mean windspeed on each axis, m/s
+            sig_wind := turbulence intensity in windspeed on each axis, m/s
+            altitude := expected operating altitude
+        """
+        self.dt = dt
+
+        self.wind = DrydenWind(avg_wind[0], avg_wind[1], avg_wind[2], sig_wind[0], sig_wind[1], sig_wind[2], altitude)
+
+    def update(self, t, position):
+        """
+        Given the present time and position of the multirotor, return the
+        current wind speed on all three axes. 
+        
+        The wind should be expressed in the world coordinates. 
+        """
+        return self.wind.getWind(self.dt)
+
+class DrydenGustLP(object):
+    """
+    This is another wrapper on an existing external package: https://github.com/goromal/wind-dynamics 
+    The Dryden model is accepted by the DoD in design, characterization, and testing of aircraft
+    in simulation. https://en.wikipedia.org/wiki/Dryden_Wind_Turbulence_Model
+
+    The difference between this model and DrydenGust is that we add an additional low pass filter governed by 
+    a cutoff frequency, 1/tau in order to generate smoother varying winds. 
+    """
+
+    def __init__(self, dt=1/500, 
+                    avg_wind=np.array([0,0,0]), sig_wind=np.array([1,1,1]),
+                    altitude=2.0,
+                    tau=0.1):
+        """
+        Inputs:
+            dt := time discretization, s, should match the simulator
+            avg_wind := mean windspeed on each axis, m/s
+            sig_wind := turbulence intensity (denoted sigma) in windspeed on each axis, m/s
+            altitude := expected operating altitude
+            tau      := cutoff frequency of the low pass filter (s)
+        """
+        self.dt = dt
+
+        self.tau = tau
+
+        self.wind = DrydenWind(avg_wind[0], avg_wind[1], avg_wind[2], sig_wind[0], sig_wind[1], sig_wind[2], altitude)
+        self.prev_wind = self.wind.getWind(self.dt)
+
+    def update(self, t, position):
+        """
+        Given the present time and position of the multirotor, return the
+        current wind speed on all three axes. 
+        
+        The wind should be expressed in the world coordinates. 
+        """
+        wind = (1-self.dt/self.tau)*self.prev_wind + self.dt/self.tau*self.wind.getWind(self.dt)
+        self.prev_wind = wind
+        return wind

rotorpy/wind/spatial_winds.py

@@ -0,0 +1,44 @@
+"""
+Here is an example of a static spatial wind field. 
+"""
+import numpy as np
+
+class WindTunnel(object):
+    """
+    This profile is a cylindrical column of air pointing at (0,0,0). Outside of this column the wind is zero. 
+    """
+
+    def __init__(self, mag=1, dir=np.array([1,0,0]), radius=1):
+        """
+        Inputs: 
+            magnitude, a scalar representing the strength of the wind column
+            direction, a vector describing the direction of the wind column
+                e.g., [1,0,0] means the column will be directed in the positive x axis
+            radius, the size of the column (the radius of the cylinder) in meters. 
+            
+        """
+
+        self.mag = mag
+        if np.linalg.norm(dir) > 1: # Check if this is a unit vector. If not, normalize it. 
+            self.dir = dir/np.linalg.norm(dir)
+        else:
+            self.dir = dir
+        self.radius = radius
+
+    def update(self, t, position):
+        """
+        Given the present time and position of the vehicle in the world frame, return the
+        current wind speed on all three axes. 
+        """
+
+        # We can get the perpendicular distance of the vector xi+yj+zk from the line self.dir 
+        # using the cross product. This assumes that ||self.dir|| = 1 and that the column
+        # passes through the origin (0,0,0). 
+
+        if np.linalg.norm(np.cross(position, self.dir)) <= self.radius:
+            # If this condition passes, we're within the column. 
+            wind = self.mag*self.dir
+        else:
+            wind = np.array([0,0,0])
+
+        return wind

rotorpy/wind/wind_template.py

@@ -0,0 +1,35 @@
+"""
+Wind profiles should be implemented here to be added as an argument in the 
+simulate method found in simulate.py
+
+The main structure of each object is the only thing that must remain consistent.
+When creating a new wind profile, the object must have the following:
+
+- An __init__ method. The arguments vary depending on the wind profile. 
+- An update method. The arguments *must* include time and position. 
+
+"""
+import numpy as np
+
+class WindTemplate(object):
+    """
+    Winds are implemented with two required methods: __init__() and update(). 
+    With __init__() you can specify any parameters or constants used to specify the wind. 
+    The update() method is called at each time step of the simulator. The position and time 
+    of the vehicle are provided. 
+    """
+
+    def __init__(self):
+        """
+        Inputs: 
+            Nothing
+        """
+
+    def update(self, t, position):
+        """
+        Given the present time and position of the vehicle in the world frame, return the
+        current wind speed on all three axes. 
+        
+        Be careful whether or not the wind is specified in the body or world coordinates!. 
+        """
+        return np.array([0,0,0])

rotorpy/world.py

@@ -0,0 +1,305 @@
+import json
+import numpy as np
+
+from rotorpy.utils.shapes import Cuboid
+from rotorpy.utils.numpy_encoding import NumpyJSONEncoder, to_ndarray
+
+def interp_path(path, res):
+    if path.size == 3:
+        # There's only one datapoint. Return the point. 
+        return path.reshape(1,-1)
+    else:
+        cumdist = np.cumsum(np.linalg.norm(np.diff(path, axis=0),axis=1))
+        if cumdist[-1] > 0:
+            t = np.insert(cumdist,0,0)
+            ts = np.arange(0, cumdist[-1], res)
+            pts = np.empty((ts.size, 3), dtype=np.float64)
+            for k in range(3):
+                pts[:,k] = np.interp(ts, t, path[:,k])
+        else:
+            pts = path[[0],:]
+        return pts
+
+class World(object):
+
+    def __init__(self, world_data):
+        """
+        Construct World object from data. Instead of using this constructor
+        directly, see also class methods 'World.from_file()' for building a
+        world from a saved .json file or 'World.grid_forest()' for building a
+        world object of a parameterized style.
+
+        Parameters:
+            world_data, dict containing keys 'bounds' and 'blocks'
+                bounds, dict containing key 'extents'
+                    extents, list of [xmin, xmax, ymin, ymax, zmin, zmax]
+                blocks, list of dicts containing keys 'extents' and 'color'
+                    extents, list of [xmin, xmax, ymin, ymax, zmin, zmax]
+                    color, color specification
+        """
+        self.world = world_data
+
+    @classmethod
+    def from_file(cls, filename):
+        """
+        Read world definition from a .json text file and return World object.
+
+        Parameters:
+            filename
+
+        Returns:
+            world, World object
+
+        Example use:
+            my_world = World.from_file('my_filename.json')
+        """
+        with open(filename) as file:
+            return cls(to_ndarray(json.load(file)))
+
+    def to_file(self, filename):
+        """
+        Write world definition to a .json text file.
+
+        Parameters:
+            filename
+
+        Example use:
+            my_word.to_file('my_filename.json')
+        """
+        with open(filename, 'w') as file:  # TODO check for directory to exist
+            file.write(json.dumps(self.world, cls=NumpyJSONEncoder, indent=4))
+
+    def closest_points(self, points):
+        """
+        For each point, return the closest occupied point in the world and the
+        distance to that point. This is appropriate for computing sphere-vs-world
+        collisions.
+
+        Input
+            points, (N,3)
+        Returns
+            closest_points, (N,3)
+            closest_distances, (N,)
+        """
+
+        closest_points = np.empty_like(points)
+        closest_distances = np.full(points.shape[0], np.inf)
+        p = np.empty_like(points)
+        for block in self.world.get('blocks', []):
+            # Computation takes advantage of axes-aligned blocks. Note that
+            # scipy.spatial.Rectangle can compute this distance, but wouldn't
+            # return the point itself.
+            r = block['extents']
+            for i in range(3):
+                p[:, i] = np.clip(points[:, i], r[2*i], r[2*i+1])
+            d = np.linalg.norm(points-p, axis=1)
+            mask = d < closest_distances
+            closest_points[mask, :] = p[mask, :]
+            closest_distances[mask] = d[mask]
+        return (closest_points, closest_distances)
+
+    def min_dist_boundary(self, points):
+        """
+        For each point, calculate the minimum distance to the boundary checking, x,y,z. A negative distance means the
+        point is outside the boundary
+        Input
+            points, (N,3)
+        Returns
+            closest_distances, (N,)
+        """
+
+        # Bounds with upper limits negated [xmin, -xmax, ymin, -ymax, ...]
+        test_bounds = np.array(self.world['bounds']['extents'])
+        test_bounds[1::2] = -test_bounds[1::2]
+
+        # Repeated coordinates with second entry negated [x, -x, y, -y, ...]
+        test_points = np.repeat(points, 2, 1)
+        test_points[:,1::2] = -test_points[:,::2]
+
+        # Compute [x-xmin, xmax-x, y-ymin, ymax-y, z-zmin, zmax-z].
+        # Minimum distance is the minimum for each point to all walls.
+        distances = test_points - test_bounds
+        min_distances = np.amin(distances, 1)
+
+        return min_distances
+
+    def path_collisions(self, path, margin):
+        """
+        Densely sample the path and check for collisions. Return a boolean mask
+        over the samples and the sample points themselves.
+        """
+        pts = interp_path(path, res=0.001)
+        (closest_pts, closest_dist) = self.closest_points(pts)
+        collisions_blocks = closest_dist < margin
+        collisions_points = self.min_dist_boundary(pts) < 0
+        collisions = np.logical_or(collisions_points, collisions_blocks)
+        return pts[collisions]
+
+    def draw_empty_world(self, ax):
+        """
+        Draw just the world without any obstacles yet. The boundary is represented with a black line.
+        Parameters:
+            ax, Axes3D object
+        """
+        (xmin, xmax, ymin, ymax, zmin, zmax) = self.world['bounds']['extents']
+
+        # Set axes limits all equal to approximate 'axis equal' display.
+        x_width = xmax-xmin
+        y_width = ymax-ymin
+        z_width = zmax-zmin
+        width = np.max((x_width, y_width, z_width))
+        ax.set_xlim((xmin, xmin+width))
+        ax.set_ylim((ymin, ymin+width))
+        ax.set_zlim((zmin, zmin+width))
+        ax.set_xlabel('x')
+        ax.set_ylabel('y')
+        ax.set_zlabel('z')
+        c = Cuboid(ax, xmax - xmin, ymax - ymin, zmax - zmin, alpha=0.01, linewidth=1, edgecolors='k')
+        c.transform(position=(xmin, ymin, zmin))
+        return list(c.artists)
+
+    def draw(self, ax):
+        """
+        Draw world onto existing Axes3D axes and return artists corresponding to the
+        blocks.
+
+        Parameters:
+            ax, Axes3D object
+
+        Returns:
+            block_artists, list of Artists associated with blocks
+
+        Example use:
+            my_world.draw(ax)
+        """
+        bounds_artists = self.draw_empty_world(ax)
+
+        block_artists = []
+        for b in self.world.get('blocks', []):
+            (xmin, xmax, ymin, ymax, zmin, zmax) = b['extents']
+            c = Cuboid(ax, xmax-xmin, ymax-ymin, zmax-zmin, alpha=0.6, linewidth=1, edgecolors='k', facecolors=b.get('color', None))
+            c.transform(position=(xmin, ymin, zmin))
+            block_artists.extend(c.artists)
+        return bounds_artists + block_artists
+
+    def draw_line(self, ax, points, color=None, linewidth=2):
+        path_length = np.sum(np.linalg.norm(np.diff(points, axis=0),axis=1))
+        pts = interp_path(points, res=path_length/1000)
+        # The scatter object is assigned a single z-order value. Split for better occlusion rendering.
+        for p in np.array_split(pts, 20):
+            ax.scatter(p[:,0], p[:,1], p[:,2], s=linewidth**2, c=color, edgecolors='none', depthshade=False)
+
+    def draw_points(self, ax, points, color=None, markersize=4):
+        # The scatter object is assigned a single z-order value. Split for better occlusion rendering.
+        for p in np.array_split(points, 20):
+            ax.scatter(p[:,0], p[:,1], p[:,2], s=markersize**2, c=color, edgecolors='none', depthshade=False)
+
+    # The follow class methods are convenience functions for building different
+    # kinds of parametric worlds.
+
+    @classmethod
+    def empty(cls, extents):
+        """
+        Return World object for bounded empty space.
+
+        Parameters:
+            extents, tuple of (xmin, xmax, ymin, ymax, zmin, zmax)
+
+        Returns:
+            world, World object
+
+        Example use:
+            my_world = World.empty((xmin, xmax, ymin, ymax, zmin, zmax))
+        """
+        bounds = {'extents': extents}
+        blocks = []
+        world_data = {'bounds': bounds, 'blocks': blocks}
+        return cls(world_data)
+
+    @classmethod
+    def grid_forest(cls, n_rows, n_cols, width, height, spacing):
+        """
+        Return World object describing a grid forest world parameterized by
+        arguments. The boundary extents fit tightly to the included trees.
+
+        Parameters:
+            n_rows, rows of trees stacked in the y-direction
+            n_cols, columns of trees stacked in the x-direction
+            width, weight of square cross section trees
+            height, height of trees
+            spacing, spacing between centers of rows and columns
+
+        Returns:
+            world, World object
+
+        Example use:
+            my_world = World.grid_forest(n_rows=4, n_cols=3, width=0.5, height=3.0, spacing=2.0)
+        """
+
+        # Bounds are outer boundary for world, which are an implicit obstacle.
+        x_max = (n_cols-1)*spacing + width
+        y_max = (n_rows-1)*spacing + width
+        bounds = {'extents': [0, x_max, 0, y_max, 0, height]}
+
+        # Blocks are obstacles in the environment.
+        x_root = spacing * np.arange(n_cols)
+        y_root = spacing * np.arange(n_rows)
+        blocks = []
+        for x in x_root:
+            for y in y_root:
+                blocks.append({'extents': [x, x+width, y, y+width, 0, height], 'color': [1, 0, 0]})
+
+        world_data = {'bounds': bounds, 'blocks': blocks}
+        return cls(world_data)
+
+    @classmethod
+    def random_forest(cls, world_dims, tree_width, tree_height, num_trees):
+        """
+        Return World object describing a random forest world parameterized by
+        arguments.
+
+        Parameters:
+            world_dims, a tuple of (xmax, ymax, zmax). xmin,ymin, and zmin are set to 0.
+            tree_width, weight of square cross section trees
+            tree_height, height of trees
+            num_trees, number of trees
+
+        Returns:
+            world, World object
+        """
+
+        # Bounds are outer boundary for world, which are an implicit obstacle.
+        bounds = {'extents': [0, world_dims[0], 0, world_dims[1], 0, world_dims[2]]}
+
+        # Blocks are obstacles in the environment.
+        xs = np.random.uniform(0, world_dims[0], num_trees)
+        ys = np.random.uniform(0, world_dims[1], num_trees)
+        pts = np.stack((xs, ys), axis=-1) # min corner location of trees
+        w, h = tree_width, tree_height
+        blocks = []
+        for pt in pts:
+            extents = list(np.round([pt[0], pt[0]+w, pt[1], pt[1]+w, 0, h], 2))
+            blocks.append({'extents': extents, 'color': [1, 0, 0]})
+
+        world_data = {'bounds': bounds, 'blocks': blocks}
+        return cls(world_data)
+
+
+if __name__ == '__main__':
+    import argparse
+    from pathlib import Path
+    import matplotlib.pyplot as plt
+    from rotorpy.axes3ds import Axes3Ds
+
+    parser = argparse.ArgumentParser(description='Display a map file in a Matplotlib window.')
+    parser.add_argument('filename', help="Filename for map file json.")
+    p = parser.parse_args()
+
+    file = Path(p.filename)
+    world = World.from_file(file)
+
+    fig = plt.figure(f"{file.name}")
+    ax = Axes3Ds(fig)
+    world.draw(ax)
+
+    plt.show()

rotorpy/worlds/2d_vortex_shedding.json

@@ -0,0 +1,10 @@
+{
+    "bounds": {"extents": [20, 50, 5, 35, -0.5, 10]},
+    "blocks": [
+        {"extents": [20, 21, 15, 25, -0.5, 10], "color": [0, 1, 0]},
+        {"extents": [21, 22, 15.25, 24.75, -0.5, 10], "color": [0, 1, 0]},
+        {"extents": [22, 23, 15.50, 24.50, -0.5, 10], "color": [0, 1, 0]},
+        {"extents": [23, 24, 16, 24, -0.5, 10], "color": [0, 1, 0]},
+        {"extents": [24, 25, 17, 23, -0.5, 10], "color": [0, 1, 0]}
+    ]
+}

rotorpy/worlds/README.md

@@ -0,0 +1,23 @@
+# World Module
+
+Worlds are used to represent obstacles in the map. Maps are implemented as `.json` files using the following structure: 
+
+The world bounds are defined by the "bounds" key: 
+```
+{
+    "bounds": {"extents": [xmin, xmax, ymin, ymax, zmin, zmax]},
+
+```
+Obstacles are defined using cuboids. The cuboids are defined as follows: 
+```
+    "blocks": [
+        {"extents": [xmin, xmax, ymin, ymax, zmin, zmax], "color": [R, G, B]},
+        ...
+    ]
+}
+```
+where the "color" value is a specified by RGB values between 0 and 1. As an example: 
+```
+{"extents": [0, 1, -3, 2, 0, 10], "color": [0, 1, 0]}
+```
+will create a box with corners (0,-3,0), (0, 2,0), (0,-3,10), (0,2,10), (1,-3,0), (1,2,0), (1,-3,10), (1,2,10). In other words, a box that has LWH dimensions (1m)x(5m)x(10m). The `[0,1,0]` label indicates it will be rendered in green. 

rotorpy/worlds/custom_pillars.json

@@ -0,0 +1,8 @@
+{
+    "bounds": {"extents": [-7.5, 7.5, -10, 10, -0.5, 3]},
+    "blocks": [
+        {"extents": [-1.25, -1, -0.125, 0.125, -0.5, 3.0], "color": [0, 1, 0]},
+        {"extents": [1, 1.25, -0.125, 0.125, -0.5, 3.0], "color": [1, 0, 0]},
+        {"extents": [-0.5, 0.5, -1.5, 1.5, -0.5, 3.0], "color": [0, 0, 1]}
+    ]
+}

rotorpy/worlds/double_pillar.json

@@ -0,0 +1,7 @@
+{
+    "bounds": {"extents": [-3.5, 3.5, -5, 5, -0.5, 3]},
+    "blocks": [
+        {"extents": [-1.25, -1, -0.125, 0.125, -0.5, 3.0], "color": [0, 1, 0]},
+        {"extents": [1, 1.25, -0.125, 0.125, -0.5, 3.0], "color": [1, 0, 0]}
+    ]
+}

rotorpy/worlds/grid_forest.json

@@ -0,0 +1,17 @@
+{
+    "bounds": {"extents": [0, 4.5, 0, 6.5, 0, 3.0]},
+    "blocks": [
+        {"extents": [0.0, 0.5, 0.0, 0.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [0.0, 0.5, 2.0, 2.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [0.0, 0.5, 4.0, 4.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [0.0, 0.5, 6.0, 6.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [2.0, 2.5, 0.0, 0.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [2.0, 2.5, 2.0, 2.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [2.0, 2.5, 4.0, 4.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [2.0, 2.5, 6.0, 6.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [4.0, 4.5, 0.0, 0.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [4.0, 4.5, 2.0, 2.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [4.0, 4.5, 4.0, 4.5, 0, 3.0], "color": [1, 0, 0]},
+        {"extents": [4.0, 4.5, 6.0, 6.5, 0, 3.0], "color": [1, 0, 0]}
+    ]
+}

rotorpy/worlds/pillar.json

@@ -0,0 +1,6 @@
+{
+    "bounds": {"extents": [-10, 10, -10, 10, -0.5, 3]},
+    "blocks": [
+        {"extents": [-1, -0.75, -0.125, 0.125, -0.5, 3.0], "color": [0, 1, 0]}
+    ]
+}

setup.py

@@ -0,0 +1,21 @@
+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', 'rotorpy.wind-dynamics']
+packages = list(filter(isdir, all_packages))
+
+setup(
+    name='rotorpy',
+    packages=packages,
+    version='0.1',
+    install_requires=[
+            'cvxopt',
+            'matplotlib == 3.2.2',
+            'filterpy == 1.4.5',
+            'numpy',
+            'scipy',
+            'pandas',
+            'ndsplines',
+            'timeout_decorator'])