In this code example, we'll apply LQR feedback control to stabilize a 3D quadrotor at a point.
%pylab inline
from pylab import *
import control
import scipy.integrate
import path_utils
path_utils.add_relative_to_current_source_file_path_to_sys_path("../../lib")
import flashlight.ipython_display_utils as ipython_display_utils
import flashlight.quadrotor_3d as quadrotor_3d
Populating the interactive namespace from numpy and matplotlib Initializing flashlight v0.0.1 flashlight.quadrotor_3d: Constructing sympy symbols... flashlight.quadrotor_3d: Finished constructing sympy symbols (0.008 seconds). flashlight.quadrotor_3d: Loading sympy modules... flashlight.quadrotor_3d: Finished loading sympy modules (0.026 seconds).
/Users/mike/Code/GitHub/flashlight/code/lib/flashlight/transformations.py:1888: UserWarning: failed to import module _transformations warnings.warn("failed to import module %s" % name)
As in our previous 2D LQR example, we linearize the quadrotor dynamics about hovering-at-the-origin, and compute the optimal control matrix K
. Next, we set up our simulation callback function to compute the appropriate feedback control forces and compute the time derivative of our quadrotor state. Again, we should include a bit of extra logic here to handle the fact that the angular dimensions of our state space wrap around. But we omit this detail for simplicity. Finally, we specify the initial state of our simulation to have a large initial velocity, to simulate a large unexpected disturbance (e.g., a gust of wind) occuring at the beginning of the simulation.
Note that the flashlight conventions for ordering the entries in our state and control vectors are as follows.
x = matrix( [ p_z, p_y, p_x, theta, psi, phi, p_z_dot, p_y_dot, p_x_dot, theta_dot, psi_dot, phi_dot ] ).T
u = matrix( [ u_front_right, u_rear_right, u_rear_left, u_front_left ] ).T
Roughly speaking, p_z
, p_y
, and p_x
specify the quadrotor's position; theta
is the quadrotor's pitch angle (i.e., the rotation about the z
axis); psi
is the quadrotor's yaw angle (i.e., the rotation about the y
axis); and phi
is the quadrotor's roll angle (i.e., the rotation about the x
axis). The remaining values in our state vector are the time derivatives. Note also that the quadrotor's positive x
, y
, and z
axes are shown as the red, green, and blue lines extending out from the quadrotor in the animation below. We apply the Euler angle rotations to the rotating frame, applying yaw (psi
) first, then pitch (theta
), then roll (phi
). For more details on the Euler angle conventions and notation we adopt throughout flashlight, see [1].
[1] Niels Joubert, Mike Roberts, Anh Truong, Floraine Berthouzoz, Pat Hanrahan. An Interactive Tool for Designing Quadrotor Camera Shots. ACM Transactions on Graphics 34(6) (SIGGRAPH Asia 2015).
m = quadrotor_3d.m
g = quadrotor_3d.g
x_star = matrix([0,0,0,0,0,0,0,0,0,0,0,0]).T
u_star = matrix([m*g/4.0,m*g/4.0,m*g/4.0,m*g/4.0]).T
Q = diag([1,1,1,1,1,1,1,1,1,1,1,1])
R = diag([1,1,1,1])
A, B = quadrotor_3d.compute_df_dx_and_df_du(x_star, u_star)
K, S, E = control.lqr(A, B, Q, R)
x_disturbance = matrix([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, 5.0, 5.0, -2.0*pi, 0.0, 0.0]).T
x_0 = (x_star + x_disturbance).A1
def compute_x_dot(x_t, t):
x_t = matrix(x_t).T
x_bar_t = x_t - x_star
u_bar_t = -K*x_bar_t
u_t = u_bar_t + u_star
x_dot_t = quadrotor_3d.compute_x_dot(x_t, u_t).A1
return x_dot_t
num_timesteps_sim = 200
t_begin = 0.0
t_end = 10.0
t_sim = linspace(t_begin, t_end, num_timesteps_sim)
x_sim = scipy.integrate.odeint(compute_x_dot, x_0, t_sim)
quadrotor_3d.draw(t_sim, x_sim)
# quadrotor_3d.draw(t_sim, x_sim, savefig=True, out_dir="data/09", out_file="00.mp4")
ipython_display_utils.get_inline_video("data/09/00.mp4")
/Users/mike/Library/Enthought/Canopy_64bit/User/lib/python2.7/site-packages/traits/has_traits.py:1536: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future. setattr( self, name, value )