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