multirotor - Quickstart¶

This tutorial is a quick overview of functionality in multiotor library. multirotor is a simulation package for drones which provides accurate, modular, and extensible interface to model flight. It is able to model electrical and physical characteristics of flight. This tutorial illustrates the following features:

Describing and creating a Multirotor class
Visualization
Using optimized PID control for flight
Trajectory class model multiple waypoints
BaseMultirotorEnv class for gym-like interface

This document can be run as a Jupyter notebook using jupytext extension. Link to raw markdown file is here.

%matplotlib widget
%reload_ext autoreload
%autoreload 2
from pprint import pprint
import numpy as np
np.set_printoptions(precision=3)
import matplotlib.pyplot as plt
plt.rcParams.update({'font.size': 8})

Creating a vehicle¶

A vehicle is represented by a multirotor.simulation.Multirotor class, which is responsible for the physics calculations. The Multirotor is parametrized by VehicleParams and SimulationParams.

VehicleParams itself is parametrized by PropellerParams for each propeller. A propeller may optionally have MotorParams characterizing the electrical properties. If not, any changes to propeller speeds are instant. Let’s create a simple vehicle, ignoring motor dynamics:

from multirotor.vehicle import VehicleParams, PropellerParams, SimulationParams
pp = PropellerParams(
    k_thrust=0.01,
    k_drag=1e-2,
    moment_of_inertia=1e-3
)
sp = SimulationParams(
    dt=1e-2 # simulation time-step in seconds
)

The vertical thrust in Newtons \(F_z\) is governed by the thrust coefficient. For propeller velocity (radians per second) \(\omega\),

\[ F_z = k_{thrust} \cdot \omega^2 \]

Similarly, the torque produced about the vertical axis of the multirotor due to propeller drag, \(T_z\) is governed by the drag coefficient,

\[ T_z = k_{drag} \cdot \omega^2 \]

multirotor has a convenience function for creating a vehicle from simple geometries:

from multirotor.helpers import vehicle_params_factory
from multirotor.simulation import Multirotor
vp = vp = vehicle_params_factory(
    n=4,          # number of propellers, equally spaced
    m_prop=1e-2,  # mass of propeller / kg
    d_prop=0.3,   # distance of propeller from center of mass /m
    params=pp,    # each propeller's parameters
    m_body=2      # mass of body
)
vehicle = Multirotor(params=vp, simulation=sp)

The Multirotor provides a host of methods to manipulate the vehicle. The two main methods are:

step_dynamics. It takes a 6D vector of forces and torques in the body frame of the vehicle, and returns the state.
step_speeds. It takes a nD vector of propeller speeds, and returns the state.

Note: The data type of the vectors should match vehicle.dtype.

The vehicle is described by its .state attribute. It is a 12-dimensional array containing the position, velocity, orientation, and angular velocity. Individual state components can be accessed by their relevant attributes:

# Step using forces/torques
state = vehicle.step_dynamics(np.asarray([1,2,vehicle.weight,0,0,1], vehicle.dtype))
# Step using speeds (rad/s)
state = vehicle.step_speeds(np.asarray([100, 100, 100, 100], dtype=vehicle.dtype))

print('State:\n', state)
print('\nConsists of:')
print(f'{"Position": >32}', vehicle.position)
print(f'{"Velocity (body-frame)": >32}', vehicle.velocity)
print(f'{"Orientation": >32}', vehicle.orientation)
print(f'{"Angular Rate": >32}', vehicle.angular_rate)

print('\nAdditional properties:')
print(f'{"Velocity (inertial-frame)": >32}', vehicle.inertial_velocity)
print(f'{"Acceleration": >32}', vehicle.acceleration)
print(f'{"Euler rate": >32}', vehicle.euler_rate)

State:
 [1.000e-04 1.000e-04 9.300e-03 5.000e-03 9.800e-03 1.863e+00 0.000e+00
 0.000e+00 1.290e-02 0.000e+00 0.000e+00 8.621e-01]

Consists of:
                        Position [0.    0.    0.009]
           Velocity (body-frame) [0.005 0.01  1.863]
                     Orientation [0.    0.    0.013]
                    Angular Rate [0.    0.    0.862]

Additional properties:

       Velocity (inertial-frame) [0.005 0.01  1.863]
                    Acceleration [ 8.000e-03 -4.000e-03  1.863e+02]

                      Euler rate [0.    0.    0.862]

Visualizing¶

multirotor.visualize.VehicleDrawing class is a wrapper around matplotlib. It can interactively visualize the vehicle in 3D.

from multirotor.visualize import VehicleDrawing
vehicle.reset() # resets time/position back to 0
drawing = VehicleDrawing(vehicle, trace=True)

for j in range(1000):
    vehicle.step_dynamics(np.asarray(
        [0.4, 2 * np.sin(j*np.pi/100), vehicle.params.mass*sp.g +  np.cos(j*2*np.pi/1000),
         0,0,0],
        dtype=vehicle.dtype))
    drawing.axis.set_title(f'pos:{vehicle.position}')
    drawing.update()

Additionally, the multirotor.visualize.plot_datalog function can visualize timeseries measurements from the vehicle:

from multirotor.helpers import DataLog
from multirotor.visualize import plot_datalog
log = DataLog(vehicle)
vehicle.reset()
for j in range(1000):
    vehicle.step_dynamics(np.asarray(
        [0.4, 2 * np.sin(j*np.pi/100), vehicle.params.mass*sp.g +  np.cos(j*2*np.pi/1000),
         0,0,0],
        dtype=vehicle.dtype))
    log.log()
log.done_logging() # converts to numpy arrays
plot_datalog(log, figsize=(6,4));

Controlling a vehicle¶

So far, so good. However, controlling a vehicle is another challenge.multirotor provides a multirotor.controller.Controller class, which uses PID control to navigate.

from multirotor.controller import Controller

ctrl = Controller.make_for(vehicle)

A controller has a step() method, which takes the reference position and yaw, and outputs the dynamics needed to achieve that.

\[ F_z,\tau_x,\tau_y,\tau_z = \texttt{step(}x,y,z,\psi\texttt{)} \]

Let’s say we want the vehicle to go up to \(z=10\) and \(x=20\)

dynamics = ctrl.step(reference=[20,0,10,0], persist=False)
print('F_z=%.3f, T_x==%.3f, T_y=%.3f, T_z=%.3f' % (dynamics[0], dynamics[1], dynamics[2], dynamics[3]))

F_z=30.212, T_x==0.002, T_y=0.002, T_z=0.000

Now that we have the prescribed dynamics, we must convert them into prescribed speeds (radians / s) for the vehicle propellers. That’s where control allocation comes in:

speeds = vehicle.allocate_control(dynamics[0], dynamics[1:4])
print(speeds)

[27.483 27.49  27.483 27.475]

And finally, the speeds can be applied to the vehicle:

state = vehicle.step_speeds(speeds)
print(state)

[ 1.005e+01  3.118e+00  1.420e-02  2.000e+00 -1.000e-04  5.010e-02
  0.000e+00  0.000e+00  0.000e+00  1.700e-03  1.700e-03  0.000e+00]

This can then be looped over and over again:

vehicle.reset()
ctrl.reset()
drawing = VehicleDrawing(
    vehicle, trace=True, body_axes=True,
    make_fig_kwargs={'xlim':(-1,10), 'ylim':(-3,3), 'zlim':(-1,2)}
)
log = DataLog(vehicle, ctrl)
for i in range(500):
    dynamics = ctrl.step(reference=[20,0,1,0], persist=True)
    speeds = vehicle.allocate_control(dynamics[0], dynamics[1:4])
    state = vehicle.step_speeds(speeds)
    drawing.update()
    log.log()
log.done_logging()
drawing.axis.view_init(elev=30, azim=-100)

plot_datalog(log, figsize=(6,4));

Optimizing control¶

The PID controller has many tunable parameters. Searching for an adequate parametrization for a vehicle is a complex search problem.

pprint(ctrl.get_params())

{'ctrl_a': {'k_d': array([0., 0., 0.]),
            'k_i': array([0., 0., 0.]),
            'k_p': array([1., 1., 1.]),
            'max_acceleration': 0.2,
            'max_err_i': array(1.),
            'max_jerk': 100.0,
            'square_root_scaling': False},
 'ctrl_p': {'k_d': array([0., 0.]),
            'k_i': array([0., 0.]),
            'k_p': array([16.667, 16.667]),
            'leashing': True,
            'max_acceleration': 3.0,
            'max_err_i': array(1.),
            'max_jerk': 100.0,
            'max_velocity': 7.0,
            'square_root_scaling': True},
 'ctrl_r': {'k_d': array([0., 0., 0.]),
            'k_i': array([0., 0., 0.]),
            'k_p': array([1., 1., 1.]),
            'max_acceleration': 0.2,
            'max_err_i': array(1.)},
 'ctrl_v': {'k_d': array([0., 0.]),
            'k_i': array([0., 0.]),
            'k_p': array([1., 1.]),
            'max_err_i': array(1.),
            'max_tilt': 0.17453292519943295},
 'ctrl_vz': {'k_d': 0, 'k_i': 0, 'k_p': 1.0, 'max_err_i': array(1.)},
 'ctrl_z': {'k_d': array([0.]),
            'k_i': array([0.]),
            'k_p': array(1.),
            'max_err_i': array(1.),
            'max_velocity': 5},
 'feedforward_weight': 0.0}

multirotor.optimize provides a convenience function called optimize() to search for the best parameters. Parameter search is done using the optuna library, which is installed as a dependency.

from multirotor.optimize import optimize, run_sim, apply_params
study = optimize(vp, sp, ctrl, ntrials=100)

from optuna.visualization import plot_parallel_coordinate # requires plotly
# from optuna.visualization.matplotlib import plot_parallel_coordinate # requires matplotlib
plot_parallel_coordinate(study)

Then, the best parameters can be applied to the controller:

apply_params(ctrl, study.best_params);

Trajectories¶

multirotor.trajectories defines the Trajectory class. It can take a list of waypoints and break them into smaller segments for the controller.

from multirotor.trajectories import Trajectory
from multirotor.env import DynamicsMultirotorEnv
vehicle.reset()
ctrl = Controller.make_for(vehicle)
drawing = VehicleDrawing(vehicle, trace=True)
traj = Trajectory(vehicle, points=[[5,0,0], [5,0,5], [5,5,2]],  proximity=0.1, resolution=0.3)
for i, (ref, _) in enumerate(traj):
    dynamics = ctrl.step(reference=[*ref, 0]) # yaw=0 in this case
    speeds = vehicle.allocate_control(dynamics[0], dynamics[1:4])
    vehicle.step_speeds(speeds)
    drawing.update()
    if i==1000:
        break

Environments¶

multirotor.env defines gym-compatible environments for reinforcement learning experiments. The environments, called DynamicsMultirotorEnv and SpeedsMultirotorEnv take either the dynamics or propeller speeds as inputs. By default, rewards are for navigating to the origin. env.reset() method initializes to a random position inside some bounding box. The BaseMultirotorEnv class can be extended for a variety of objectives.

from multirotor.env import DynamicsMultirotorEnv

env = DynamicsMultirotorEnv(vehicle, allocate=True)
state = env.reset()
print('Initial position')
print(env.vehicle.position)

forces_torques = np.asarray([1,1,10,0,0,0.1])
state, reward, done, info = env.step(forces_torques)

Initial position
[-0.783 -6.05  -2.583]

multirotor

Navigation

Related Topics