ocp/powerkite.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 
34 #include <acado_optimal_control.hpp>
35 #include <acado_gnuplot.hpp>
36 
37 
38 
39 int main( ){
40 
41 
42  USING_NAMESPACE_ACADO
43 
44 
45 // DIFFERENTIAL STATES :
46 // -------------------------
47  DifferentialState r; // the length r of the cable
48  DifferentialState phi; // the angle phi
49  DifferentialState theta; // the angle theta
50 // ------------------------- // -------------------------------------------
51  DifferentialState dr; // first derivative of r0 with respect to t
52  DifferentialState dphi; // first derivative of phi with respect to t
53  DifferentialState dtheta; // first derivative of theta with respect to t
54 // ------------------------- // -------------------------------------------
55  DifferentialState n; // winding number
56 // ------------------------- // -------------------------------------------
57  DifferentialState Psi; // the roll angle Psi
58  DifferentialState CL; // the aerodynamic lift coefficient
59 // ------------------------- // -------------------------------------------
60  DifferentialState W; // integral over the power at the generator
61  // ( = ENERGY )
62 
63 
64 // CONTROL :
65 // -------------------------
66  Control ddr0; // second derivative of r0 with respect to t
67  Control dPsi; // first derivative of Psi with respect to t
68  Control dCL; // first derivative of CL with respect to t
69 
70 
71 // PARAMETERS :
72 // ------------------------
73  // PARAMETERS OF THE KITE :
74  // -----------------------------
75  double mk = 850.00; // mass of the kite // [ kg ]
76  double A = 500.00; // effective area // [ m^2 ]
77  double V = 720.00; // volume // [ m^3 ]
78  double cd0 = 0.04; // aerodynamic drag coefficient // [ ]
79  // ( cd0: without downwash )
80  double K = 0.04; // induced drag constant // [ ]
81 
82 
83  // PHYSICAL CONSTANTS :
84  // -----------------------------
85  double g = 9.81; // gravitational constant // [ m /s^2]
86  double rho = 1.23; // density of the air // [ kg/m^3]
87 
88  // PARAMETERS OF THE CABLE :
89  // -----------------------------
90  double rhoc = 1450.00; // density of the cable // [ kg/m^3]
91  double cc = 1.00; // frictional constant // [ ]
92  double dc = 0.05614; // diameter // [ m ]
93 
94 
95  // PARAMETERS OF THE WIND :
96  // -----------------------------
97  double w0 = 10.00; // wind velocity at altitude h0 // [ m/s ]
98  double h0 = 100.00; // the altitude h0 // [ m ]
99  double hr = 0.10; // roughness length // [ m ]
100 
101 
102 
103 // OTHER VARIABLES :
104 // ------------------------
105 
106  double AQ ; // cross sectional area
107 
108  IntermediateState mc; // mass of the cable
109  IntermediateState m ; // effective inertial mass
110  IntermediateState m_; // effective gravitational mass
111 
112  IntermediateState Cg;
113 
114  IntermediateState dm; // first derivative of m with respect to t
115 
116 
117 // DEFINITION OF PI :
118 // ------------------------
119 
120  double PI = 3.1415926535897932;
121 
122 
123 // ORIENTATION AND FORCES :
124 // ------------------------
125 
126  IntermediateState h ; // altitude of the kite
127  IntermediateState w ; // the wind at altitude h
128  IntermediateState we [ 3]; // effective wind vector
129  IntermediateState nwe ; // norm of the effective wind vector
130  IntermediateState nwep ; // -
131  IntermediateState ewep [ 3]; // projection of ewe
132  IntermediateState eta ; // angle eta: cf. [2]
133  IntermediateState et [ 3]; // unit vector from the left to the right wing tip
134  IntermediateState Caer ;
135  IntermediateState Cf ; // simple constants
136  IntermediateState CD ; // the aerodynamic drag coefficient
137  IntermediateState Fg [ 3]; // the gravitational force
138  IntermediateState Faer [ 3]; // the aerodynamic force
139  IntermediateState Ff [ 3]; // the frictional force
140  IntermediateState F [ 3]; // the total force
141 
142 
143 // TERMS ON RIGHT-HAND-SIDE
144 // OF THE DIFFERENTIAL
145 // EQUATIONS :
146 // ------------------------
147 
148  IntermediateState a_pseudo [ 3]; // the pseudo accelaration
149  IntermediateState dn ; // the time derivate of the kite's winding number
150  IntermediateState ddr ; // second derivative of s with respect to t
151  IntermediateState ddphi ; // second derivative of phi with respect to t
152  IntermediateState ddtheta ; // second derivative of theta with respect to t
153  IntermediateState power ; // the power at the generator
154 // ------------------------ // ----------------------------------------------
155  IntermediateState regularisation ; // regularisation of controls
156 
157 
158 
159 // MODEL EQUATIONS :
160 // ===============================================================
161 
162 
163 
164 // CROSS AREA OF THE CABLE :
165 // ---------------------------------------------------------------
166 
167  AQ = PI*dc*dc/4.0 ;
168 
169 // THE EFECTIVE MASS' :
170 // ---------------------------------------------------------------
171 
172  mc = rhoc*AQ*r ; // mass of the cable
173  m = mk + mc / 3.0; // effective inertial mass
174  m_ = mk + mc / 2.0; // effective gravitational mass
175 // ----------------------------- // ----------------------------
176  dm = (rhoc*AQ/ 3.0)*dr; // time derivative of the mass
177 
178 
179 // WIND SHEAR MODEL :
180 // ---------------------------------------------------------------
181 
182  h = r*cos(theta) ;
183  w = log(h/hr) / log(h0/hr) *w0 ;
184 
185 
186 // EFFECTIVE WIND IN THE KITE`S SYSTEM :
187 // ---------------------------------------------------------------
188 
189  we[0] = w*sin(theta)*cos(phi) - dr ;
190  we[1] = -w*sin(phi) - r*sin(theta)*dphi ;
191  we[2] = -w*cos(theta)*cos(phi) + r *dtheta;
192 
193 
194 // CALCULATION OF THE KITE`S TRANSVERSAL AXIS :
195 // ---------------------------------------------------------------
196 
197  nwep = pow( we[1]*we[1] + we[2]*we[2], 0.5 );
198  nwe = pow( we[0]*we[0] + we[1]*we[1] + we[2]*we[2], 0.5 );
199  eta = asin( we[0]*tan(Psi)/ nwep ) ;
200 
201 // ---------------------------------------------------------------
202 
203 // ewep[0] = 0.0 ;
204  ewep[1] = we[1] / nwep ;
205  ewep[2] = we[2] / nwep ;
206 
207 // ---------------------------------------------------------------
208 
209  et [0] = sin(Psi) ;
210  et [1] = (-cos(Psi)*sin(eta))*ewep[1] - (cos(Psi)*cos(eta))*ewep[2];
211  et [2] = (-cos(Psi)*sin(eta))*ewep[2] + (cos(Psi)*cos(eta))*ewep[1];
212 
213 
214 
215 
216 // CONSTANTS FOR SEVERAL FORCES :
217 // ---------------------------------------------------------------
218 
219  Cg = (V*rho-m_)*g ;
220  Caer = (rho*A/2.0 )*nwe ;
221  Cf = (rho*dc/8.0)*r*nwe ;
222 
223 
224 // THE DRAG-COEFFICIENT :
225 // ---------------------------------------------------------------
226 
227  CD = cd0 + K*CL*CL ;
228 
229 
230 
231 // SUM OF GRAVITATIONAL AND LIFTING FORCE :
232 // ---------------------------------------------------------------
233 
234  Fg [0] = Cg * cos(theta) ;
235 // Fg [1] = Cg * 0.0 ;
236  Fg [2] = Cg * sin(theta) ;
237 
238 
239 // SUM OF THE AERODYNAMIC FORCES :
240 // ---------------------------------------------------------------
241 
242  Faer[0] = Caer*( CL*(we[1]*et[2]-we[2]*et[1]) + CD*we[0] ) ;
243  Faer[1] = Caer*( CL*(we[2]*et[0]-we[0]*et[2]) + CD*we[1] ) ;
244  Faer[2] = Caer*( CL*(we[0]*et[1]-we[1]*et[0]) + CD*we[2] ) ;
245 
246 
247 // THE FRICTION OF THE CABLE :
248 // ---------------------------------------------------------------
249 
250 // Ff [0] = Cf * cc* we[0] ;
251  Ff [1] = Cf * cc* we[1] ;
252  Ff [2] = Cf * cc* we[2] ;
253 
254 
255 
256 // THE TOTAL FORCE :
257 // ---------------------------------------------------------------
258 
259  F [0] = Fg[0] + Faer[0] ;
260  F [1] = Faer[1] + Ff[1] ;
261  F [2] = Fg[2] + Faer[2] + Ff[2] ;
262 
263 
264 
265 // THE PSEUDO ACCELERATION:
266 // ---------------------------------------------------------------
267 
268  a_pseudo [0] = - ddr0
269  + r*( dtheta*dtheta
270  + sin(theta)*sin(theta)*dphi *dphi )
271  - dm/m*dr ;
272 
273  a_pseudo [1] = - 2.0*cos(theta)/sin(theta)*dphi*dtheta
274  - 2.0*dr/r*dphi
275  - dm/m*dphi ;
276 
277  a_pseudo [2] = cos(theta)*sin(theta)*dphi*dphi
278  - 2.0*dr/r*dtheta
279  - dm/m*dtheta ;
280 
281 
282 
283 
284 // THE EQUATIONS OF MOTION:
285 // ---------------------------------------------------------------
286 
287  ddr = F[0]/m + a_pseudo[0] ;
288  ddphi = F[1]/(m*r*sin(theta)) + a_pseudo[1] ;
289  ddtheta = -F[2]/(m*r ) + a_pseudo[2] ;
290 
291 
292 
293 
294 
295 // THE DERIVATIVE OF THE WINDING NUMBER :
296 // ---------------------------------------------------------------
297 
298  dn = ( dphi*ddtheta - dtheta*ddphi ) /
299  (2.0*PI*(dphi*dphi + dtheta*dtheta) ) ;
300 
301 
302 
303 // THE POWER AT THE GENERATOR :
304 // ---------------------------------------------------------------
305 
306  power = m*ddr*dr ;
307 
308 
309 
310 // REGULARISATION TERMS :
311 // ---------------------------------------------------------------
312 
313 
314  regularisation = 5.0e2 * ddr0 * ddr0
315  + 1.0e8 * dPsi * dPsi
316  + 1.0e5 * dCL * dCL
317  + 2.5e5 * dn * dn
318  + 2.5e7 * ddphi * ddphi;
319  + 2.5e7 * ddtheta * ddtheta;
320  + 2.5e6 * dtheta * dtheta;
321 // ---------------------------
322 
323 
324 
325 // THE "RIGHT-HAND-SIDE" OF THE ODE:
326 // ---------------------------------------------------------------
327  DifferentialEquation f;
328 
329  f << dot(r) == dr ;
330  f << dot(phi) == dphi ;
331  f << dot(theta) == dtheta ;
332  f << dot(dr) == ddr0 ;
333  f << dot(dphi) == ddphi ;
334  f << dot(dtheta) == ddtheta ;
335  f << dot(n) == dn ;
336  f << dot(Psi) == dPsi ;
337  f << dot(CL) == dCL ;
338  f << dot(W) == (-power + regularisation)*1.0e-6;
339 
340 
341  // DEFINE AN OPTIMAL CONTROL PROBLEM:
342  // ----------------------------------
343  OCP ocp( 0.0, 18.0, 18 );
344  ocp.minimizeMayerTerm( W );
345  ocp.subjectTo( f );
346 
347 
348  // INITIAL VALUE CONSTRAINTS:
349  // ---------------------------------
350  ocp.subjectTo( AT_START, n == 0.0 );
351  ocp.subjectTo( AT_START, W == 0.0 );
352 
353 
354  // PERIODIC BOUNDARY CONSTRAINTS:
355  // ----------------------------------------
356  ocp.subjectTo( 0.0, r , -r , 0.0 );
357  ocp.subjectTo( 0.0, phi , -phi , 0.0 );
358  ocp.subjectTo( 0.0, theta , -theta , 0.0 );
359  ocp.subjectTo( 0.0, dr , -dr , 0.0 );
360  ocp.subjectTo( 0.0, dphi , -dphi , 0.0 );
361  ocp.subjectTo( 0.0, dtheta, -dtheta, 0.0 );
362  ocp.subjectTo( 0.0, Psi , -Psi , 0.0 );
363  ocp.subjectTo( 0.0, CL , -CL , 0.0 );
364 
365  ocp.subjectTo( -0.34 <= phi <= 0.34 );
366  ocp.subjectTo( 0.85 <= theta <= 1.45 );
367  ocp.subjectTo( -40.0 <= dr <= 10.0 );
368  ocp.subjectTo( -0.29 <= Psi <= 0.29 );
369  ocp.subjectTo( 0.1 <= CL <= 1.50 );
370  ocp.subjectTo( -0.7 <= n <= 0.90 );
371  ocp.subjectTo( -25.0 <= ddr0 <= 25.0 );
372  ocp.subjectTo( -0.065 <= dPsi <= 0.065 );
373  ocp.subjectTo( -3.5 <= dCL <= 3.5 );
374 
375 
376  // CREATE A PLOT WINDOW AND VISUALIZE THE RESULT:
377  // ----------------------------------------------
378 
379  GnuplotWindow window;
380  window.addSubplot( r, "CABLE LENGTH r [m]" );
381  window.addSubplot( phi, "POSITION ANGLE phi [rad]" );
382  window.addSubplot( theta,"POSITION ANGLE theta [rad]" );
383  window.addSubplot( Psi, "ROLL ANGLE psi [rad]" );
384  window.addSubplot( CL, "LIFT COEFFICIENT CL" );
385  window.addSubplot( W, "ENERGY W [MJ]" );
386  window.addSubplot( F[0], "FORCE IN CABLE [N]" );
387  window.addSubplot( phi,theta, "Kite Orbit","theta [rad]","phi [rad]" );
388 
389  // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
390  // ---------------------------------------------------
391 
392  OptimizationAlgorithm algorithm(ocp);
393 
394  algorithm.initializeDifferentialStates("powerkite_states.txt" );
395  algorithm.initializeControls ("powerkite_controls.txt" );
396  algorithm.set ( MAX_NUM_ITERATIONS, 100 );
397  algorithm.set ( KKT_TOLERANCE , 1e-2 );
398 
399  algorithm << window;
400 
401  algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );
402 
403  algorithm.solve();
404  return 0;
405 }
406 
407 
408 
main
int main()
Definition: ocp/powerkite.cpp:39
sin
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
Definition: acado_syntax.cpp:43
acado_optimal_control.hpp
OptimizationAlgorithmBase::initializeControls
returnValue initializeControls(const char *fileName)
Definition: optimization_algorithm_base.cpp:142
OptimizationAlgorithm
User-interface to formulate and solve optimal control problems and static NLPs.
Definition: optimization_algorithm.hpp:54
USING_NAMESPACE_ACADO
#define USING_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:59
asin
IntermediateState asin(const Expression &arg)
Definition: acado_syntax.cpp:46
pow
IntermediateState pow(const Expression &arg1, const Expression &arg2)
Definition: acado_syntax.cpp:54
PRINT_SCP_METHOD_PROFILE
Definition: acado_types.hpp:418
tan
IntermediateState tan(const Expression &arg)
Definition: acado_syntax.cpp:45
OCP::subjectTo
returnValue subjectTo(const DifferentialEquation &differentialEquation_)
Definition: ocp.cpp:153
OCP::minimizeMayerTerm
returnValue minimizeMayerTerm(const Expression &arg)
Definition: ocp.cpp:238
PlotWindow::addSubplot
returnValue addSubplot(PlotWindowSubplot &_subplot)
Definition: plot_window.cpp:276
cos
IntermediateState cos(const Expression &arg)
Definition: acado_syntax.cpp:44
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
YES
#define YES
Definition: acado_types.hpp:51
V
#define V
Definition: rocket_with_templates_ocp.hpp:42
OptimizationAlgorithmBase::initializeDifferentialStates
returnValue initializeDifferentialStates(const char *fileName, BooleanType autoinit=BT_FALSE)
Definition: optimization_algorithm_base.cpp:106
OCP
Data class for defining optimal control problems.
Definition: ocp.hpp:89
dot
Expression dot(const Expression &arg)
Definition: acado_syntax.cpp:98
KKT_TOLERANCE
Definition: acado_types.hpp:371
MAX_NUM_ITERATIONS
Definition: acado_types.hpp:370
GnuplotWindow
Provides an interface to Gnuplot for plotting algorithmic outputs.
Definition: gnuplot_window.hpp:58
AT_START
Definition: acado_types.hpp:732
OptimizationAlgorithm::solve
virtual returnValue solve()
Definition: optimization_algorithm.cpp:100
acado_gnuplot.hpp
DifferentialEquation
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
Definition: differential_equation.hpp:55
log
IntermediateState log(const Expression &arg)
Definition: acado_syntax.cpp:52

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