kite_carousel.cpp
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 
00032 #include <acado_code_generation.hpp>
00033 
00034 int main()
00035 {
00036         USING_NAMESPACE_ACADO
00037 
00038         // Variables:
00039         DifferentialState phi;
00040         DifferentialState theta;
00041         DifferentialState dphi;
00042         DifferentialState dtheta;
00043 
00044         Control u1;
00045         Control u2;
00046 
00047         IntermediateState c;// c := rho * |we|^2 / (2*m)
00048         DifferentialEquation f;// the right-hand side of the model
00049 
00050         // Parameters:
00051         const double R = 1.00;// radius of the carousel arm
00052         const double Omega = 1.00;// constant rotational velocity of the carousel arm
00053         const double m = 0.80;// the mass of the plane
00054         const double r = 1.00;// length of the cable between arm and plane
00055         const double A = 0.15;// wing area of the plane
00056 
00057         const double rho = 1.20;// density of the air
00058         const double CL = 1.00;// nominal lift coefficient
00059         const double CD = 0.15;// nominal drag coefficient
00060         const double b = 15.00;// roll stabilization coefficient
00061         const double g = 9.81;// gravitational constant
00062 
00063         // COMPUTE THE CONSTANT c:
00064         // -------------------------
00065         c = ( (R*R*Omega*Omega)
00066                         + (r*r)*( (Omega+dphi)*(Omega+dphi) + dtheta*dtheta )
00067                         + (2.0*r*R*Omega)*( (Omega+dphi)*sin(theta)*cos(phi)
00068                                         + dtheta*cos(theta)*sin(phi) ) ) * ( A*rho/( 2.0*m ) );
00069 
00070         f << dot( phi ) == dphi;
00071         f << dot( theta ) == dtheta;
00072 
00073         f << dot( dphi ) == ( 2.0*r*(Omega+dphi)*dtheta*cos(theta)
00074                         + (R*Omega*Omega)*sin(phi)
00075                         + c*(CD*(1.0+u2)+CL*(1.0+0.5*u2)*phi) ) / (-r*sin(theta));
00076 
00077         f << dot( dtheta ) == ( (R*Omega*Omega)*cos(theta)*cos(phi)
00078                         + r*(Omega+dphi)*sin(theta)*cos(theta)
00079                         + g*sin(theta) - c*( CL*u1 + b*dtheta ) ) / r;
00080 
00081         // Reference functions and weighting matrices:
00082         Function h, hN;
00083         h << phi << theta << dphi << dtheta << u1 << u2;
00084         hN << phi << theta << dphi << dtheta;
00085 
00086         DMatrix W = eye<double>( h.getDim() );
00087         DMatrix WN = eye<double>( hN.getDim() );
00088 
00089         W(0,0) = 5.000;
00090         W(1,1) = 1.000;
00091         W(2,2) = 10.000;
00092         W(3,3) = 10.000;
00093         W(4,4) = 0.1;
00094         W(5,5) = 0.1;
00095 
00096         WN(0,0) = 1.0584373059248833e+01;
00097         WN(0,1) = 1.2682415889087276e-01;
00098         WN(0,2) = 1.2922232012424681e+00;
00099         WN(0,3) = 3.7982078044271374e-02;
00100         WN(1,0) = 1.2682415889087265e-01;
00101         WN(1,1) = 3.2598407653299275e+00;
00102         WN(1,2) = -1.1779732282636639e-01;
00103         WN(1,3) = 9.8830655348904548e-02;
00104         WN(2,0) = 1.2922232012424684e+00;
00105         WN(2,1) = -1.1779732282636640e-01;
00106         WN(2,2) = 4.3662024133354898e+00;
00107         WN(2,3) = -5.9269992411260908e-02;
00108         WN(3,0) = 3.7982078044271367e-02;
00109         WN(3,1) = 9.8830655348904534e-02;
00110         WN(3,2) = -5.9269992411260901e-02;
00111         WN(3,3) = 3.6495564367004318e-01;
00112 
00113         //
00114         // Optimal Control Problem
00115         //
00116 
00117 //      OCP ocp( 0.0, 12.0, 15 );
00118         OCP ocp( 0.0, 2.0 * M_PI, 10 );
00119 
00120         ocp.subjectTo( f );
00121 
00122         ocp.minimizeLSQ(W, h);
00123         ocp.minimizeLSQEndTerm(WN, hN);
00124 
00125         ocp.subjectTo( 18.0 <= u1 <= 22.0 );
00126         ocp.subjectTo( -0.2 <= u2 <= 0.2 );
00127 
00128         // Export the code:
00129         OCPexport mpc(ocp);
00130 
00131         mpc.set( INTEGRATOR_TYPE , INT_RK4 );
00132         mpc.set( NUM_INTEGRATOR_STEPS , 30 );
00133         mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
00134         mpc.set( HOTSTART_QP, YES );
00135         mpc.set( GENERATE_TEST_FILE, NO );
00136 
00137         if (mpc.exportCode("kite_carousel_export") != SUCCESSFUL_RETURN)
00138                 exit( EXIT_FAILURE );
00139 
00140         mpc.printDimensionsQP( );
00141 
00142 //      DifferentialState Gx(4,4), Gu(4,2);
00143 // 
00144 //      Expression rhs;
00145 //      f.getExpression( rhs );
00146 // 
00147 //      Function Df;
00148 //      Df << rhs;
00149 //      Df << forwardDerivative( rhs, x ) * Gx;
00150 //      Df << forwardDerivative( rhs, x ) * Gu + forwardDerivative( rhs, u );
00151 // 
00152 // 
00153 //      EvaluationPoint z(Df);
00154 //      
00155 //      DVector xx(28);
00156 //      xx.setZero();
00157 // 
00158 //     xx(0) = -4.2155955213988627e-02;
00159 //      xx(1) =  1.8015724412870739e+00;
00160 //      xx(2) =  0.0;
00161 //      xx(3) =  0.0;
00162 // 
00163 //      DVector uu(2);
00164 //     uu(0) = 20.5;
00165 //      uu(1) = -0.1;
00166 //      
00167 //      z.setX( xx );
00168 //     z.setU( uu );
00169 // 
00170 //     DVector result = Df.evaluate( z );
00171 //      result.print( "x" );
00172 
00173         return EXIT_SUCCESS;
00174 }
00175 
00176 


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:37:44