acado: quadcopter.cpp Source File

Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 #include <acado_code_generation.hpp>
00027 
00028 using namespace std;
00029 USING_NAMESPACE_ACADO
00030 
00031 int main( )
00032 {
00033         // Define variables, functions and constants:
00034         // ----------------------------------------------------------
00035     DifferentialState   dT1;
00036     DifferentialState   dT2;
00037     DifferentialState   dT3;
00038     DifferentialState   dT4;
00039     
00040     DifferentialState   T1;
00041     DifferentialState   T2;
00042     DifferentialState   T3;
00043     DifferentialState   T4;
00044     
00045     DifferentialState   W1;
00046     DifferentialState   W2;
00047     DifferentialState   W3;
00048     DifferentialState   W4;
00049     
00050     DifferentialState   q1;
00051     DifferentialState   q2;
00052     DifferentialState   q3;
00053     DifferentialState   q4;
00054     
00055     DifferentialState   Omega1;
00056     DifferentialState   Omega2;
00057     DifferentialState   Omega3;
00058     
00059     DifferentialState   V1;
00060     DifferentialState   V2;
00061     DifferentialState   V3;
00062     
00063     DifferentialState   P1;             // x
00064     DifferentialState   P2;             // y
00065     DifferentialState   P3;             // z
00066     
00067     DifferentialState   IP1;
00068     DifferentialState   IP2;
00069     DifferentialState   IP3;
00070 
00071     Control             U1;
00072     Control             U2;
00073     Control             U3;
00074     Control             U4;
00075 
00076     DifferentialEquation   f1, f2;   
00077         
00078     const double rho = 1.23;
00079     const double A = 0.1;
00080     const double Cl = 0.25;
00081     const double Cd = 0.3*Cl;
00082     const double m = 10;
00083     const double g = 9.81;
00084     const double L  = 0.5;
00085     const double Jp = 1e-2;
00086     const double xi = 1e-2;
00087     const double J1 = 0.25;
00088     const double J2 = 0.25;
00089     const double J3 = 1;
00090     const double gain = 1e-4;
00091 
00092     const double alpha = 0.0;
00093 
00094 
00095         // Define the quadcopter ODE model in fully nonlinear form:
00096         // ----------------------------------------------------------
00097         f1 << U1*gain; 
00098         f1 << U2*gain; 
00099         f1 << U3*gain; 
00100         f1 << U4*gain; 
00101         f1 << dT1; 
00102         f1 << dT2; 
00103         f1 << dT3; 
00104         f1 << dT4; 
00105         f1 << (T1 - W1*xi)/Jp; 
00106         f1 << (T2 - W2*xi)/Jp; 
00107         f1 << (T3 - W3*xi)/Jp; 
00108         f1 << (T4 - W4*xi)/Jp; 
00109         f1 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
00110         f1 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
00111         f1 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
00112         f1 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
00113         f1 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1; 
00114         f1 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2; 
00115         f1 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3; 
00116         f1 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m); 
00117         f1 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m); 
00118         f1 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g; 
00119         f1 << V1; 
00120         f1 << V2; 
00121         f1 << V3; 
00122         f1 << P1; 
00123         f1 << P2; 
00124         f1 << P3; 
00125 
00126 
00127         // Define the quadcopter ODE model in 3-stage format:
00128         // ----------------------------------------------------------
00129         
00130         // LINEAR INPUT SYSTEM (STAGE 1):
00131         DMatrix M1, A1, B1;
00132         M1 = eye<double>(12);
00133         A1 = zeros<double>(12,12);
00134         B1 = zeros<double>(12,4);
00135         
00136         A1(4,0) = 1.0;
00137         A1(5,1) = 1.0;
00138         A1(6,2) = 1.0;
00139         A1(7,3) = 1.0;
00140         A1(8,4) = 1.0/Jp;       A1(8,8) = -xi/Jp;
00141         A1(9,5) = 1.0/Jp;       A1(9,9) = -xi/Jp;
00142         A1(10,6) = 1.0/Jp;      A1(10,10) = -xi/Jp;
00143         A1(11,7) = 1.0/Jp;      A1(11,11) = -xi/Jp;
00144         
00145         B1(0,0) = gain;
00146         B1(1,1) = gain;
00147         B1(2,2) = gain;
00148         B1(3,3) = gain;
00149         
00150         // NONLINEAR SYSTEM (STAGE 2):
00151         f2 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
00152         f2 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
00153         f2 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
00154         f2 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
00155         f2 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1; 
00156         f2 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2; 
00157         f2 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3; 
00158         f2 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m); 
00159         f2 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m); 
00160         f2 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g; 
00161         
00162         // LINEAR OUTPUT SYSTEM (STAGE 3):
00163         DMatrix M3, A3;
00164         M3 = eye<double>(6);
00165         A3 = zeros<double>(6,6);
00166         
00167         A3(3,0) = 1.0;
00168         A3(4,1) = 1.0;
00169         A3(5,2) = 1.0;
00170     
00171     OutputFcn f3;
00172         
00173         f3 << V1;
00174         f3 << V2;
00175         f3 << V3;
00176         f3 << 0.0;
00177         f3 << 0.0;
00178         f3 << 0.0;
00179 
00180 
00181         // ----------------------------------------------------------
00182         // ----------------------------------------------------------
00183         SIMexport sim1( 10, 1.0 );
00184         
00185         sim1.setModel( f1 );
00186         sim1.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
00187         
00188         sim1.set( NUM_INTEGRATOR_STEPS, 50 );
00189         sim1.setTimingSteps( 10000 );
00190         
00191         cout << "-----------------------------------------------------------\n  Using a QuadCopter ODE model in fully nonlinear form:\n-----------------------------------------------------------\n";
00192         sim1.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
00193 
00194 
00195         // ----------------------------------------------------------
00196         // ----------------------------------------------------------
00197         SIMexport sim2( 10, 1.0 );
00198         
00199         sim2.setLinearInput( M1, A1, B1 );
00200         sim2.setModel( f2 );
00201         sim2.setLinearOutput( M3, A3, f3 );
00202         sim2.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
00203         
00204         sim2.set( NUM_INTEGRATOR_STEPS, 50 );
00205         sim2.setTimingSteps( 10000 );
00206         
00207         cout << "-----------------------------------------------------------\n  Using a QuadCopter ODE model in 3-stage format:\n-----------------------------------------------------------\n";
00208         sim2.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
00209 
00210         return 0;
00211 }
00212