Updated axis limit computation and added other common trajectories.

.gitignore

@@ -0,0 +1,6 @@
+*.pyc
+model/*.pyc
+test/*.pyc
+utils/*.pyc
+
+

controller.py

@@ -26,6 +26,8 @@
 k_p_psi = 80
 k_d_psi = 5
 
+# LQR Gains
+
 def run(quad, des_state):
     x, y, z = quad.position()
     x_dot, y_dot, z_dot = quad.velocity()
@@ -54,4 +56,88 @@ def run(quad, des_state):
                    k_p_theta * (des_theta - theta) + k_d_theta * (q_des - q),
                    k_p_psi * (des_psi - psi) + k_d_psi * (r_des - r)]]).T
 
+    print(F)
+    print(M)
     return F, M
+
+def run_LQR(quad, des_state):
+
+    # get drone state
+    x, y, z = quad.position()
+    x_dot, y_dot, z_dot = quad.velocity()
+    phi, theta, psi = quad.attitude()
+    p, q, r = quad.omega()
+
+    # get desired state
+    des_x, des_y, des_z = des_state.pos
+    des_x_dot, des_y_dot, des_z_dot = des_state.vel
+    des_x_ddot, des_y_ddot, des_z_ddot = des_state.acc
+    des_psi = des_state.yaw
+    des_psi_dot = des_state.yawdot    
+
+
+    # For the LQR controller, the input to the controller are the states -x-,
+    # and reference values -r-, and the output -u- (inputs to the drone) 
+    # ar the Thrust and desired moments, (F,M) that should be applied to 
+    # the drone
+    #
+    # In general    u = Nu*r - K(x -Nx*r) = -K*x + (Nu + K*Nx)*r
+    # where K are the LQR gains, Nu and Nx are reference input matrices.
+    # See more at p.493 "Feedback Control of Dynamic Systems, Franklink G,
+    # Powell, J. Emami-Naeini, Abbas"
+
+    # This LQR controller was designed assuming a linearized model of the drone.
+
+    # Z dynamics LQR Gains and input matrices
+    K_z = np.matrix([10.0, 2.04709])
+    Nu_z = np.matrix([0.0])
+    Nx_z = np.matrix([[1.0],[0.0]])
+
+    # For Z dynamics, the only coupled variables are the z and z_dot
+    # so make an state vector for this dynamics
+    state = np.matrix([[z],[z_dot]])
+    # Calculating thrust, note it is identical to above equation for u
+    F = -K_z*state +(Nu_z + K_z*Nx_z)*des_z
+
+    # X dynamics LQR Gains and input matrices
+    K_x = np.matrix([10.0, 10.6217, 14.762, 1.042])
+    Nu_x = np.matrix([0.0])
+    Nx_x = np.matrix([[1.0],[0.0],[0.0],[0.0]])
+
+    # For X  dynamics, the only coupled variables are the x, x_dot, theta and theta_dot
+    # so make an state vector for this dynamics
+    state = np.matrix([[x],[x_dot],[theta],[q]])
+    # Calculating torque, note it is identical to above equation for u
+    L = -K_x*state +(Nu_x + K_x*Nx_x)*des_x
+
+
+    # Y dynamics LQR Gains and input matrices
+    K_y = np.matrix([-10.0, -10.6208, 14.7408, 1.039])
+    Nu_y = np.matrix([0.0])
+    Nx_y = np.matrix([[1.0],[0.0],[0.0],[0.0]])
+
+    # For X  dynamics, the only coupled variables are the x, x_dot, theta and theta_dot
+    # so make an state vector for this dynamics
+    state = np.matrix([[y],[y_dot],[phi],[p]])
+    # Calculating torque, note it is identical to above equation for u
+    O = -K_y*state +(Nu_y + K_y*Nx_y)*des_y
+
+
+    # Yaw dynamics LQR Gains and input matrices
+    K_yaw = np.matrix([10.0, 1.14])
+    Nu_yaw = np.matrix([0.0])
+    Nx_yaw = np.matrix([[1.0],[0.0]])
+
+    # For Yaw dynamics, the only coupled variables are the z and z_dot
+    # so make an state vector for this dynamics
+    state = np.matrix([[psi],[r]])
+    # Calculating thrust, note it is identical to above equation for u
+    N = -K_yaw*state +(Nu_yaw + K_yaw*Nx_yaw)*des_psi      
+
+    # Now create the torque vector
+    M = np.array([[O.item(0)], [L.item(0)], [N.item(0)]])
+
+    print(F.item(0))
+    print(M)
+
+    return F.item(0), M

model/params.py

@@ -15,7 +15,7 @@
 
 invI = np.linalg.inv(I)
 arm_length = 0.086 # meter
-height = 0.05
+height = 0.00
 minF = 0.0
 maxF = 2.0 * mass * g
 L = arm_length

quadPlot.py

@@ -15,15 +15,18 @@
 history = np.zeros((500,3))
 count = 0
 
-def plot_quad_3d(waypoints, args=()):
+def plot_quad_3d(waypoints, init_pos, args=()):
     fig = plt.figure()
     ax = fig.add_axes([0, 0, 1, 1], projection='3d')
     ax.plot([], [], [], '-', c='cyan')[0]
     ax.plot([], [], [], '-', c='red')[0]
     ax.plot([], [], [], '-', c='blue', marker='o', markevery=2)[0]
     ax.plot([], [], [], '.', c='red', markersize=4)[0]
     ax.plot([], [], [], '.', c='blue', markersize=2)[0]
-    set_limit((-0.5,0.5), (-0.5,0.5), (-0.5,8))
+    #set_limit((-0.5,0.5), (-0.5,0.5), (-0.5,8))
+    set_limit2(waypoints, init_pos)
+
+    set_ax_names("x","y","z")
     plot_waypoints(waypoints)
     an = animation.FuncAnimation(fig, _callback, fargs = args, init_func=None,
             frames=400, interval=10, blit=False)
@@ -45,6 +48,32 @@ def set_limit(x, y, z):
     ax.set_ylim(y)
     ax.set_zlim(z)
 
+def set_limit2(waypoints, pos):
+    """
+        Set the drawing limits based on the waypoints 
+        the quadrotor should fly and start point.
+
+    """
+    ax = plt.gca()
+    x_min = min(pos[0], min(waypoints[:,0]))
+    x_max = max(pos[0], max(waypoints[:,0]))
+    ax.set_xlim(x_min,x_max)
+
+    y_min = min(pos[1], min(waypoints[:,1]))
+    y_max = max(pos[1], max(waypoints[:,1]))
+    ax.set_ylim(y_min,y_max)
+
+    z_min = min(pos[2], min(waypoints[:,2]))
+    z_max = max(pos[2], max(waypoints[:,2]))
+    ax.set_zlim(z_min,z_max)
+
+def set_ax_names(x, y, z):
+    ax = plt.gca()
+    ax.set_xlabel(x)
+    ax.set_ylabel(y)
+    ax.set_zlabel(z)
+
+
 def set_frame(frame):
     # convert 3x6 world_frame matrix into three line_data objects which is 3x2 (row:point index, column:x,y,z)
     lines_data = [frame[:,[0,2]], frame[:,[1,3]], frame[:,[4,5]]]

runsim.py

@@ -24,26 +24,34 @@ def render(quad):
 
 def attitudeControl(quad, time, waypoints, coeff_x, coeff_y, coeff_z):
     desired_state = trajGen3D.generate_trajectory(time[0], 1.2, waypoints, coeff_x, coeff_y, coeff_z)
-    F, M = controller.run(quad, desired_state)
+    F, M = controller.run_LQR(quad, desired_state)
     quad.update(dt, F, M)
     time[0] += dt
 
 def main():
+    # define initial conditions
     pos = (0.5,0,0)
     attitude = (0,0,0)
+    # create new quadrotor
     quadcopter = Quadcopter(pos, attitude)
+
     sched = scheduler.Scheduler()
-    waypoints = trajGen3D.get_helix_waypoints(0, 9)
+    # Generate a trajectory with 9 waypoints
+    waypoints = trajGen3D.get_poly_waypoints(0, 9)
+    # Get coefficients of 8 degree polinomial that represents the 
+    # continuos trajectory that drone will follow in space
     (coeff_x, coeff_y, coeff_z) = trajGen3D.get_MST_coefficients(waypoints)
+    # 
     sched.add_task(attitudeControl, dt, (quadcopter,time,waypoints,coeff_x,coeff_y,coeff_z))
     kEvent_Render = sched.add_event(render, (quadcopter,))
 
     try:
-        plt.plot_quad_3d(waypoints, (sched, kEvent_Render))
+        plt.plot_quad_3d(waypoints, pos, (sched, kEvent_Render))
     except KeyboardInterrupt:
         print ("attempting to close threads.")
         sched.stop()
         print ("terminated.")
 
 if __name__ == "__main__":
     main()
+

trajGen3D.py

@@ -15,14 +15,56 @@
 current_heading = np.zeros(2)
 
 def get_helix_waypoints(t, n):
-    """ The function generate n helix waypoints from the given time t
-        output waypoints shape is [n, 3]
+    """ The function generate n helix waypoints from the given time t.
+        Output waypoints shape is [n, 3]
     """
     waypoints_t = np.linspace(t, t + 2*np.pi, n)
     x = 0.5*np.cos(waypoints_t)
     y = 0.5*np.sin(waypoints_t)
     z = waypoints_t
 
+
+    return np.stack((x, y, z), axis=-1)
+
+def get_poly_waypoints(t,n):
+    """ The function generate n waypoints from a polynomial at given t.
+        The polynomial is 
+            x = k1*t^2
+            y = k1*t^3
+            z = k2*t
+        Output waypoints shape is [n, 3]
+    """
+    waypoints_t = np.linspace(t, t + 2*np.pi, n)
+    k1 = 0.1
+    k2 = 0.5
+    x = (k1*waypoints_t)**2
+    y = (k1*waypoints_t)**3
+    z = k2*waypoints_t
+    #x = waypoints_t
+    #y = waypoints_t
+    #z = waypoints_t
+
+
+    return np.stack((x, y, z), axis=-1)
+
+def get_leminiscata_waypoints(t,n):
+    """ The function generate n waypoints from a leminiscata at given t.
+        The leminiscata is 
+            x = k1 * cos(wt/2)
+            y = k1 * sin(wt)
+            z = k2 * t
+        Output waypoints shape is [n, 3]
+    """
+    waypoints_t = np.linspace(t, t + 2*np.pi, n)
+
+    k1 = 0.5
+    k2 = 0.5
+    w = 0.1
+
+    x = k1*np.cos(w*waypoints_t/2.0)
+    y = k1*np.sin(w*waypoints_t)
+    z = k2*waypoints_t
+
     return np.stack((x, y, z), axis=-1)
 
 def get_MST_coefficients(waypoints):