quadcopter.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 #include <acado_code_generation.hpp>
27 
28 using namespace std;
29 USING_NAMESPACE_ACADO
30 
31 int main( )
32 {
33  // Define variables, functions and constants:
34  // ----------------------------------------------------------
35  DifferentialState dT1;
36  DifferentialState dT2;
37  DifferentialState dT3;
38  DifferentialState dT4;
39 
40  DifferentialState T1;
41  DifferentialState T2;
42  DifferentialState T3;
43  DifferentialState T4;
44 
45  DifferentialState W1;
46  DifferentialState W2;
47  DifferentialState W3;
48  DifferentialState W4;
49 
50  DifferentialState q1;
51  DifferentialState q2;
52  DifferentialState q3;
53  DifferentialState q4;
54 
55  DifferentialState Omega1;
56  DifferentialState Omega2;
57  DifferentialState Omega3;
58 
59  DifferentialState V1;
60  DifferentialState V2;
61  DifferentialState V3;
62 
63  DifferentialState P1; // x
64  DifferentialState P2; // y
65  DifferentialState P3; // z
66 
67  DifferentialState IP1;
68  DifferentialState IP2;
69  DifferentialState IP3;
70 
71  Control U1;
72  Control U2;
73  Control U3;
74  Control U4;
75 
76  DifferentialEquation f1, f2;
77 
78  const double rho = 1.23;
79  const double A = 0.1;
80  const double Cl = 0.25;
81  const double Cd = 0.3*Cl;
82  const double m = 10;
83  const double g = 9.81;
84  const double L = 0.5;
85  const double Jp = 1e-2;
86  const double xi = 1e-2;
87  const double J1 = 0.25;
88  const double J2 = 0.25;
89  const double J3 = 1;
90  const double gain = 1e-4;
91 
92  const double alpha = 0.0;
93 
94 
95  // Define the quadcopter ODE model in fully nonlinear form:
96  // ----------------------------------------------------------
97  f1 << dot(dT1) == U1*gain;
98  f1 << dot(dT2) == U2*gain;
99  f1 << dot(dT3) == U3*gain;
100  f1 << dot(dT4) == U4*gain;
101  f1 << dot(T1) == dT1;
102  f1 << dot(T2) == dT2;
103  f1 << dot(T3) == dT3;
104  f1 << dot(T4) == dT4;
105  f1 << dot(W1) == (T1 - W1*xi)/Jp;
106  f1 << dot(W2) == (T2 - W2*xi)/Jp;
107  f1 << dot(W3) == (T3 - W3*xi)/Jp;
108  f1 << dot(W4) == (T4 - W4*xi)/Jp;
109  f1 << dot(q1) == - (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);
110  f1 << dot(q2) == (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);
111  f1 << dot(q3) == (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);
112  f1 << dot(q4) == (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);
113  f1 << dot(Omega1) == (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1;
114  f1 << dot(Omega2) == -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2;
115  f1 << dot(Omega3) == (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3;
116  f1 << dot(V1) == (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
117  f1 << dot(V2) == -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
118  f1 << dot(V3) == (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g;
119  f1 << dot(P1) == V1;
120  f1 << dot(P2) == V2;
121  f1 << dot(P3) == V3;
122  f1 << dot(IP1) == P1;
123  f1 << dot(IP2) == P2;
124  f1 << dot(IP3) == P3;
125 
126  // Define the quadcopter ODE model in 3-stage format:
127  // ----------------------------------------------------------
128 
129  // LINEAR INPUT SYSTEM (STAGE 1):
130  DMatrix M1, A1, B1;
131  M1 = eye<double>(12);
132  A1 = zeros<double>(12,12);
133  B1 = zeros<double>(12,4);
134 
135  A1(4,0) = 1.0;
136  A1(5,1) = 1.0;
137  A1(6,2) = 1.0;
138  A1(7,3) = 1.0;
139  A1(8,4) = 1.0/Jp; A1(8,8) = -xi/Jp;
140  A1(9,5) = 1.0/Jp; A1(9,9) = -xi/Jp;
141  A1(10,6) = 1.0/Jp; A1(10,10) = -xi/Jp;
142  A1(11,7) = 1.0/Jp; A1(11,11) = -xi/Jp;
143 
144  B1(0,0) = gain;
145  B1(1,1) = gain;
146  B1(2,2) = gain;
147  B1(3,3) = gain;
148 
149  // NONLINEAR SYSTEM (STAGE 2):
150  f2 << dot(q1) == - (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);
151  f2 << dot(q2) == (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);
152  f2 << dot(q3) == (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);
153  f2 << dot(q4) == (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);
154  f2 << dot(Omega1) == (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1;
155  f2 << dot(Omega2) == -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2;
156  f2 << dot(Omega3) == (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3;
157  f2 << dot(V1) == (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
158  f2 << dot(V2) == -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
159  f2 << dot(V3) == (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g;
160 
161  // LINEAR OUTPUT SYSTEM (STAGE 3):
162  DMatrix M3, A3;
163  M3 = eye<double>(6);
164  A3 = zeros<double>(6,6);
165 
166  A3(3,0) = 1.0;
167  A3(4,1) = 1.0;
168  A3(5,2) = 1.0;
169 
170  OutputFcn f3;
171 
172  f3 << V1;
173  f3 << V2;
174  f3 << V3;
175  f3 << 0.0;
176  f3 << 0.0;
177  f3 << 0.0;
178 
179 
180  // ----------------------------------------------------------
181  // ----------------------------------------------------------
182  SIMexport sim1( 10, 1.0 );
183 
184  sim1.setModel( f1 );
185  sim1.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
186 
187  sim1.set( NUM_INTEGRATOR_STEPS, 50 );
188  sim1.setTimingSteps( 10000 );
189 
190  cout << "-----------------------------------------------------------\n Using a QuadCopter ODE model in fully nonlinear form:\n-----------------------------------------------------------\n";
191  sim1.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
192 
193 
194  // ----------------------------------------------------------
195  // ----------------------------------------------------------
196  SIMexport sim2( 10, 1.0 );
197 
198  sim2.setLinearInput( M1, A1, B1 );
199  sim2.setModel( f2 );
200  sim2.setLinearOutput( M3, A3, f3 );
201  sim2.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
202 
203  sim2.set( NUM_INTEGRATOR_STEPS, 50 );
204  sim2.setTimingSteps( 10000 );
205 
206  cout << "-----------------------------------------------------------\n Using a QuadCopter ODE model in 3-stage format:\n-----------------------------------------------------------\n";
207  sim2.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
208 
209  return 0;
210 }
211 
ModelContainer::setLinearInput
returnValue setLinearInput(const DMatrix &A1_, const DMatrix &B1_)
Definition: model_container.cpp:92
B1
#define B1
Definition: hydroscal_model.hpp:85
OutputFcn
Allows to setup and evaluate output functions based on SymbolicExpressions.
Definition: output_fcn.hpp:55
ModelContainer::setLinearOutput
returnValue setLinearOutput(const DMatrix &A3_, const OutputFcn &rhs_)
Definition: model_container.cpp:105
main
USING_NAMESPACE_ACADO int main()
Definition: quadcopter.cpp:31
USING_NAMESPACE_ACADO
#define USING_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:59
SIMexport::setTimingSteps
virtual returnValue setTimingSteps(uint _timingSteps)
Definition: sim_export.cpp:917
A1
#define A1
Definition: hydroscal_model.hpp:84
ModelContainer::setModel
returnValue setModel(const DifferentialEquation &_f)
Definition: model_container.cpp:71
Options::set
returnValue set(OptionsName name, int value)
Definition: options.cpp:126
rho
const double rho
Definition: examples/code_generation/mpc_mhe/cstr.cpp:43
SIMexport
User-interface to automatically generate simulation algorithms for fast optimal control.
Definition: sim_export.hpp:60
INT_IRK_GL4
Definition: integrator_export_types.hpp:47
NUM_INTEGRATOR_STEPS
Definition: acado_types.hpp:354
SIMexport::exportAndRun
virtual returnValue exportAndRun(const std::string &dirName, const std::string &initStates, const std::string &controls, const std::string &results=std::string("results.txt"), const std::string &ref=std::string("ref.txt"))
Definition: sim_export.cpp:746
dot
Expression dot(const Expression &arg)
Definition: acado_syntax.cpp:98
L
#define L
acado_code_generation.hpp
INTEGRATOR_TYPE
Definition: acado_types.hpp:397
DifferentialEquation
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
Definition: differential_equation.hpp:55

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:35:02