コード例 #1
0
ファイル: getting_started.cpp プロジェクト: drewm1980/acado
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    Parameter a, b;

    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    NLP nlp;
    nlp.minimize (          a*a + b*b       );
    nlp.subjectTo(  0.08 <= a               );
    nlp.subjectTo(  0.1  <= a + b + 0.3*a*a );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE NLP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(nlp);
    algorithm.solve();


    // PRINT OPTIMAL SOLUTION:
    // -----------------------
	Vector results;
	algorithm.getParameters( results );
	results.print( "optimal solution" );

    return 0;
}
コード例 #2
0
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    Parameter x, y;

    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    NLP nlp;
    nlp.minimize ( 100.0*( y - x*x )*( y - x*x ) + (1.0-x)*(1.0-x) );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE NLP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(nlp);

    algorithm.set( KKT_TOLERANCE, 1e-12 );

    algorithm.solve();
    algorithm.getParameters("rosenbrock_result.txt");

    return 0;
}
コード例 #3
0
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    Parameter               V ;
    Parameter               km;


    // READ THE MEASUREMENT FROM A DATA FILE:
    // --------------------------------------
    Matrix m = readFromFile( "michaelis_menten_data.txt" );


    // DEFINE A MEASUREMENT FUNCTION:
    // ------------------------------
    Function h;  // the measurement function

    int i;
    for( i = 0; i < (int) m.getNumRows(); i++ )
        h << V*m(i,0)/(km + m(i,0)) - m(i,1);


    // DEFINE A PARAMETER ESTIMATION PROBLEM:
    // --------------------------------------
    NLP nlp;
    nlp.minimizeLSQ( h );

    nlp.subjectTo( 0.0 <= V  <= 2.0 );
    nlp.subjectTo( 0.0 <= km <= 2.0 );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE ESTIMATION PROBLEM:
    // ------------------------------------------------------------------
    ParameterEstimationAlgorithm algorithm(nlp);
    algorithm.solve();


    VariablesGrid parameters;
    algorithm.getParameters( parameters );

	return 0;
	
    // GET THE VARIANCE COVARIANCE IN THE SOLUTION:
    // ---------------------------------------------
    Matrix var;
    algorithm.getParameterVarianceCovariance( var );

    double LSSE = 2.0*algorithm.getObjectiveValue();
    double MSE  = LSSE/( m.getNumRows() - 2.0 );   // m.getNumRows() == number of measurements
                                                   // 2              == number of parameters

    var *=  MSE;  // rescale the variance-covariance with the MSE factor.


    // PRINT THE RESULT ON THE TERMINAL:
    // -----------------------------------------------------------------------
	printf("\n\nResults for the parameters: \n");
	printf("-----------------------------------------------\n");
	printf("   V   =  %.3e  +/-  %.3e \n", parameters(0,0), sqrt( var(0,0) ) );
	printf("   km  =  %.3e  +/-  %.3e \n", parameters(0,1), sqrt( var(1,1) ) );
	printf("-----------------------------------------------\n\n\n");

    return 0;
}
コード例 #4
0
ファイル: scalar2_nbi.cpp プロジェクト: drewm1980/acado
/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------
    Parameter y1,y2;


    // DEFINE AN OPTIMIZATION PROBLEM:
    // -------------------------------
    NLP nlp;
    nlp.minimize( 0, y1 );
    nlp.minimize( 1, y2 );

    nlp.subjectTo( 0.0 <= y1 <= 5.0 );
    nlp.subjectTo( 0.0 <= y2 <= 5.2 );
    nlp.subjectTo( 0.0 <= y2 - 5.0*exp(-y1) - 2.0*exp(-0.5*(y1-3.0)*(y1-3.0)) );


    // DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE NLP:
    // -----------------------------------------------------
    MultiObjectiveAlgorithm algorithm(nlp);

    algorithm.set( PARETO_FRONT_GENERATION, PFG_NORMAL_BOUNDARY_INTERSECTION );
    algorithm.set( PARETO_FRONT_DISCRETIZATION, 41 );
    algorithm.set( KKT_TOLERANCE, 1e-12 );

    // Minimize individual objective function  
    algorithm.initializeParameters("initial_scalar2_2.txt");
    algorithm.solveSingleObjective(1);

    // Minimize individual objective function
    algorithm.solveSingleObjective(0);

    // Generate Pareto set 
    algorithm.solve();


    // GET THE RESULT FOR THE PARETO FRONT AND PLOT IT:
    // ------------------------------------------------
    VariablesGrid paretoFront;
    algorithm.getParetoFront( paretoFront );
    algorithm.getWeights("scalar2_nbi_weights.txt");

    GnuplotWindow window1;
    window1.addSubplot( paretoFront, "Pareto Front y1 vs y2", "y1","y2", PM_POINTS );
    window1.plot( );

    FILE *file = fopen("scalar2_nbi_pareto.txt","w");
    paretoFront.print();
    file << paretoFront;
    fclose(file);


    // FILTER THE PARETO FRONT AND PLOT IT:
    // ------------------------------------
    algorithm.getParetoFrontWithFilter( paretoFront );
    algorithm.getWeightsWithFilter("scalar2_nbi_weights_filtered.txt");

    GnuplotWindow window2;
    window2.addSubplot( paretoFront, "Pareto Front (with filter) y1 vs y2", "y1","y2", PM_POINTS );
    window2.plot( );

    FILE *file2 = fopen("scalar2_nbi_pareto_filtered.txt","w");
    paretoFront.print();
    file2 << paretoFront;
    fclose(file2);


    // PRINT INFORMATION ABOUT THE ALGORITHM:
    // --------------------------------------
    algorithm.printInfo();

    return 0;
}
コード例 #5
0
ファイル: equilibrium.cpp プロジェクト: drewm1980/planepower
int main( int argc, char * const argv[] ){


	//======================================================================
	// PARSE THE INPUT ARGUMENTS:
	// ----------------------------------
	double IC[4];
	
	/* arguments are passed to the main function by string
	 *  there are 'int argc' arguments
	 *		the first one is the name of the program
	 *		the next ones are arguments passed in the call like
	 *			program number1 number2
	 *  in this stupid simple parser, we directly read doubles from the arguments
	 */
	 
	int i=1;
	while (i < argc) {
		// load the i-th string argument passed to the main function
		char* input = argv[i];
		// parse the string to a double value
		IC[i-1] = atof(input);
		i++;
	}
	
	cout << "Initial Conditions" << endl;
	cout << "------------" << endl;
	for (i = 0; i < argc-1; ++i) {
		cout << i+1 << ":\t" << IC[i] << endl;
	}
	//======================================================================	
	
	
	 
	 
   USING_NAMESPACE_ACADO


// DIFFERENTIAL STATES :
// -------------------------
  
	Parameter      x;      // Position
	Parameter	  y;      //  
	Parameter      z;      //  
// -------------------------      //  -------------------------------------------
  
	Parameter     dx;      //  Speed
	Parameter     dy;      //  
	Parameter     dz;      //  
	
	Parameter    e11;
	Parameter    e12;
	Parameter    e13;
	Parameter    e21;
	Parameter    e22;
	Parameter    e23;
	Parameter    e31;
	Parameter    e32;
	Parameter    e33;
	
	
	Parameter    w1;
	Parameter    w2;
	Parameter    w3;	
// -------------------------      //  -------------------------------------------
	Parameter      r;      //  Kite distance
	Parameter     dr;      //  Kite distance / dt

//-------------------------      //  -------------------------------------------
 
	Parameter  delta;      //  Carousel
	Parameter ddelta;      //  
	Parameter     ur;
	Parameter     up;      //  Ailerons  
	
	// Collect all the states in a vector
	const int n_XD = 24; // Number of states
	
	IntermediateState XD[n_XD];    // Vector collecting all states
	XD[0]  = x;
	XD[1]  = y;
	XD[2]  = z;
	XD[3]  = dx;
	XD[4]  = dy;
	XD[5]  = dz;
	XD[6]  = e11;
	XD[7]  = e12;
	XD[8]  = e13;
	XD[9]  = e21;
	XD[10] = e22;
	XD[11] = e23;
	XD[12] = e31;
	XD[13] = e32;
	XD[14] = e33;
	XD[15] = w1;
	XD[16] = w2;
	XD[17] = w3;
	XD[18] = r;
	XD[19] = dr;
	XD[20] = delta;
	XD[21] = ddelta;
	XD[22] = ur;
	XD[23] = up;
	
// CONTROL :
// -------------------------
	Parameter             dddelta;  //  Carousel acceleration
	Parameter             ddr;
	Parameter             dur;
	Parameter             dup;      //  Ailerons  
	
	// Collect all the controls in a vector
	const int n_U = 4; // Number of controls
	
	IntermediateState U[n_U];    // Vector collecting all states
	U[0] = dddelta;
	U[1] = ddr;
	U[2] = dur;
	U[3] = dup;
	
// PARAMETERS
// -----------------------
// 	Parameter SlackCLmax;
// 	Parameter SlackLambda;

	
	// DEFINITION OF PI :
	// ------------------------
	
	double PI = 3.1415926535897932;
	
	
	//TAIL LENGTH
	double LT = 0.45;
	
	//ROLL DAMPING
	double RDfac = 1;
	double RD0 = 1e-2; 
	double RD = RDfac*RD0;    
	
// CONSTANTS :
// ------------------------
	
	//  PARAMETERS OF THE KITE :
	//  -----------------------------
	double mk =  0.463;      //  mass of the kite               //  [ kg    ]
	
	
	//   PHYSICAL CONSTANTS :
	//  -----------------------------
	double g =    9.81;      //  gravitational constant         //  [ m /s^2]
	double rho =    1.23;      //  density of the air             //  [ kg/m^3]
	
	//  PARAMETERS OF THE CABLE :
	//  -----------------------------
	double rhoc = 1450.00;      //  density of the cable           //  [ kg/m^3]
	double cc =   1.00;      //  frictional constant            //  [       ]
	double dc = 1e-3;      //  diameter                       //  [ m     ]
	
	double AQ      =  PI*dc*dc/4.0;
	
	//CAROUSEL ARM LENGTH
	double rA = 1.085; //(dixit Kurt)
	
	
	//INERTIA MATRIX (Kurt's direct measurements)
	// Note: low sensitivity to I1,2,3... high sensitivity to I31...
	double I1 =  0.0163;
	double I31 = 0.0006;
	double I2 =  0.0078;
	double I3 =  0.0229;
	
	
	//WIND-TUNNEL PARAMETERS
					 
	//Lift (report p. 67)
	//Sensitivity to CLA error low
	double CLA = 5.064;
	//Sensitivity to CLe error low
	double CLe = 0.318;
	//Sensitivity to CLr error low
	double CLr = 0.85; //?!?!?!?!?
	//HIGH sensitivity to CL0 !!
	double CL0 = 0.239;
					 
	//Drag (report p. 70)
	//Sensitivity to CDA error low
	double CDA = -0.195;
	double CDA2 = 4.268;
	double CDB2 = 0;
	//Sensitivity to CDe error low
	double CDe = 0.044;
	//Sensitivity to CDr error low
	double CDr = 0.111;
	//Sensitivity to CD0 error low
	double CD0 = 0.026;
					 
	//Roll (report p. 72)
	//HIGH sensitivity to CRB !!
	double CRB = -0.062;
	//HIGH sensitivity to CRAB !!
	double CRAB = -0.271;
	//Sensitivity to CRr error low
	double CRr = -0.244;
					 
	//Pitch (report p. 74)
	//HIGH sensitivity to CPA !!
	double CPA = 0.293;
	//Sensitivity to CPe error low
	double CPe = -0.821;
	//Sensitivity to CPr error low
	double CPr = -0.647; //?!?!?!?!?
	//HIGH sensitivity to CP0 !!
	double CP0 = 0.03;
					 
	//Yaw (report p. 76)
	//HIGH sensitivity to CYB !!
	double CYB = 0.05;
	//HIGH sensitivity to CYAB !!
	double CYAB = 0.229;
					 
	double SPAN = 0.96;
	double CHORD = 0.1;
		
		

// OTHER VARIABLES :
// ------------------------
	
	IntermediateState     mc;      //  mass of the cable
	IntermediateState     m ;      //  effective inertial mass
	IntermediateState  mgrav;      //  gravific mass
	//  IntermediateState     dmc;      //  first  derivative of m     with respect to t

// ORIENTATION AND FORCES :
// ------------------------
	
	IntermediateState wind               ;      //  the wind at altitude 	
	
	IntermediateState Cf              ;      //  cable drag
	IntermediateState CD              ;      //  the aerodynamic drag coefficient
	IntermediateState CL              ;      //  the aerodynamic lift coefficient
	IntermediateState CR              ;      //  the aerodynamic roll coefficient
	IntermediateState CP              ;      //  the aerodynamic pitch coefficient
	IntermediateState CY              ;      //  the aerodynamic yaw coefficient
	
	IntermediateState F           [ 3];      //  aero forces + gravity
	IntermediateState FL          [ 3];      //  the lift force
	IntermediateState FD          [ 3];      //  the drag force
	IntermediateState Ff          [ 3];      //  the frictional force
// 	IntermediateState Fcable          ;      //  force in the cable
	
	IntermediateState er          [ 3];      // X normed to 1
	IntermediateState eTe         [ 3];      //unrotated transversal vector (psi = 0)
	IntermediateState eLe         [ 3];      //unrotated lift vector (psi = 0)
	IntermediateState we          [ 3];      //  effective wind vector
	IntermediateState wE          [ 3];      //  effective wind vector
	IntermediateState wp              ;		
	IntermediateState wep         [ 3];		// effective wind projected in the plane orthogonal to X
	
	IntermediateState VKite           ;     //Kite (relative) speed
	IntermediateState VKite2          ;     //Squared (relative) kite speed
		
	IntermediateState Vp;
	IntermediateState VT			[3];
	IntermediateState alpha;
	IntermediateState beta;
	IntermediateState alphaTail;
	IntermediateState T			    [3];
	
	
	// TERMS ON RIGHT-HAND-SIDE
	// OF THE DIFFERENTIAL
	// EQUATIONS              :
	// ------------------------
	
	IntermediateState de11;
	IntermediateState de12;
	IntermediateState de13;
	IntermediateState de21;
	IntermediateState de22;
	IntermediateState de23;
	IntermediateState de31;
	IntermediateState de32;
	IntermediateState de33;
	
	
//                        MODEL EQUATIONS :
// ===============================================================

	// CROSS AREA OF THE CABLE :
	// ---------------------------------------------------------------
	
// 	AQ      =  PI*dc*dc/4.0                                       ;
	
	// THE EFECTIVE MASS' :
	// ---------------------------------------------------------------
	
	mc      =  rhoc*AQ*r  ;   // mass of the cable
	m       =  mk + mc/3.0;   // effective inertial mass
	mgrav   =  mk + mc/2.0;   // effective inertial mass
	
	// -----------------------------   // ----------------------------
	//   dm      =  (rhoc*AQ/ 3.0)*dr;   // time derivative of the mass
	
	
	// WIND SHEAR MODEL :
	// ---------------------------------------------------------------
	
	wind       =  0.;
	
	
	// EFFECTIVE WIND IN THE KITE`S SYSTEM :
	// ---------------------------------------------------------------
	
	we[0]   = -wind + dx;
	we[1]   =		  dy;
	we[2]   =		  dz;
	
	VKite2 = (we[0]*we[0] + we[1]*we[1] + we[2]*we[2]); 
	VKite = sqrt(VKite2); 
	
	// CALCULATION OF THE FORCES :
	// ---------------------------------------------------------------
	
	// er
    er[0] = x/r;
	er[1] = y/r;
	er[2] = z/r;
	
	//Velocity accross X (cable drag)
	wp = er[0]*we[0] + er[1]*we[1] + er[2]*we[2];
	wep[0] = we[0] - wp*er[0];
	wep[1] = we[1] - wp*er[1];
	wep[2] = we[2] - wp*er[2];
	
	//Aero coeff.
	
	
	// LIFT DIRECTION VECTOR
	// -------------------------

	//Relative wind speed in Airfoil's referential 'E'
	wE[0] = e11*we[0]  + e21*we[1]  + e31*we[2];
	wE[1] = e12*we[0]  + e22*we[1]  + e32*we[2];
	wE[2] = e13*we[0]  + e23*we[1]  + e33*we[2];


	//Airfoil's transversal axis in fixed referential 'e'
	eTe[0] = e12;
	eTe[1] = e22;
	eTe[2] = e32;


	// Lift axis ** Normed to we !! **
	eLe[0] = - eTe[1]*we[2] + eTe[2]*we[1];
	eLe[1] = - eTe[2]*we[0] + eTe[0]*we[2];
	eLe[2] = - eTe[0]*we[1] + eTe[1]*we[0];

	// AERODYNAMIC COEEFICIENTS
	// ----------------------------------
	//VT = cross([w1;w2;w3],[-LT;0;0]) + wE;

	VT[0] =          wE[0];
	VT[1] = -LT*w3 + wE[1];
	VT[2] =  LT*w2 + wE[2];

	alpha = -wE[2]/wE[0];

	//Note: beta & alphaTail are compensated for the tail motion induced by omega
	beta = VT[1]/sqrt(VT[0]*VT[0] + VT[2]*VT[2]);
	alphaTail = -VT[2]/VT[0];

	CL = CLA*alpha + CLe*up     + CLr*ur + CL0;
	CD = CDA*alpha + CDA2*alpha*alpha + CDB2*beta*beta + CDe*up + CDr*ur + CD0;
	CR = -RD*w1 + CRB*beta + CRAB*alphaTail*beta + CRr*ur;
	CP = CPA*alphaTail + CPe*up + CPr*ur + CP0;
	CY = CYB*beta + CYAB*alphaTail*beta;


	Cf = rho*dc*r*VKite/8.0;

	// THE FRICTION OF THE CABLE :
	// ---------------------------------------------------------------

	Ff[0] = -rho*dc*r*VKite*cc*wep[0]/8.0;
	Ff[1] = -rho*dc*r*VKite*cc*wep[1]/8.0;
	Ff[2] = -rho*dc*r*VKite*cc*wep[2]/8.0;

	// LIFT :
	// ---------------------------------------------------------------

	FL[0] =  rho*CL*eLe[0]*VKite/2.0;
	FL[1] =  rho*CL*eLe[1]*VKite/2.0;
	FL[2] =  rho*CL*eLe[2]*VKite/2.0;

	// DRAG :
	// ---------------------------------------------------------------

	FD[0] = -rho*VKite*CD*we[0]/2.0;
	FD[1] = -rho*VKite*CD*we[1]/2.0; 
	FD[2] = -rho*VKite*CD*we[2]/2.0; 


	// FORCES (AERO)
	// ---------------------------------------------------------------

	F[0] = FL[0] + FD[0] + Ff[0];
	F[1] = FL[1] + FD[1] + Ff[1];
	F[2] = FL[2] + FD[2] + Ff[2];

	// TORQUES (AERO)
	// ---------------------------------------------------------------

	T[0] =  0.5*rho*VKite2*SPAN*CR;
	T[1] =  0.5*rho*VKite2*CHORD*CP;
	T[2] =  0.5*rho*VKite2*SPAN*CY;



	// ATTITUDE DYNAMICS
	// -----------------------------------------------------------

	de11 =  e12*w3 - e13*w2;
	de12 =  e13*w1 - e11*w3;
	de13 =  e11*w2 - e12*w1;
	de21 =  e22*w3 - e23*w2;
	de22 =  e23*w1 - e21*w3;
	de23 =  e21*w2 - e22*w1;
	de31 =  e32*w3 - e33*w2;
	de32 =  e33*w1 - e31*w3;
	de33 =  e31*w2 - e32*w1;



	//////////////////////////////////////////////////////////////////////// 
	//                                                                    // 
	//  AUTO-GENERATED EQUATIONS (S. Gros, HIGHWIND, OPTEC, KU Leuven)    // 
	//                                                                    // 
	//////////////////////////////////////////////////////////////////////// 
	
	// Equations read: 
	// IMA = inv(MA) 
	// ddX = IMA*(Bx - CA*lambda) 
	// lambdaNum = CA^T*IMA*Bx - Blambda 
	// lambdaDen = CA^T*IMA*CA 
	// lambda = lambdaNum/lambdaDen 
	
	// Arm 
	IntermediateState xA; 
	IntermediateState dxA; 
	IntermediateState ddxA; 
	IntermediateState yA; 
	IntermediateState dyA; 
	IntermediateState ddyA; 
	
	xA = -rA*sin(delta); 
	dxA = -(ddelta*rA*cos(delta)); 
	ddxA = -(dddelta*rA*cos(delta) - ddelta*ddelta*rA*sin(delta)); 
	yA = rA*cos(delta); 
	dyA = -ddelta*rA*sin(delta); 
	ddyA = - rA*cos(delta)*ddelta*ddelta - dddelta*rA*sin(delta); 
	
	// BUILD DYNAMICS 
	IntermediateState lambdaNum; 
	lambdaNum = ddxA*xA - 2*dy*dyA - ddr*r - ddxA*x - 2*dx*dxA - ddyA*y + ddyA*yA - dr*dr + dx*dx + dxA*dxA + dy*dy + dyA*dyA + dz*dz + (F[0]*(x - xA))/m + (F[1]*(y - yA))/m + (z*(F[2] - g*mgrav))/m; 
	
	IntermediateState lambdaDen; 
	lambdaDen = x*x/m + xA*xA/m + y*y/m + yA*yA/m + z*z/m - (2*x*xA)/m - (2*y*yA)/m; 
	
	IntermediateState lambda; 
	lambda = lambdaNum/lambdaDen; 
	
// 	IntermediateState ddX(6,1); 
	IntermediateState ddx = (F[0] - lambda*(x - xA))/m; 
	IntermediateState ddy = (F[1] - lambda*(y - yA))/m; 
	IntermediateState ddz = -(g*mgrav - F[2] + lambda*z)/m; 
	IntermediateState dw1 = (I31*(T[2] + w2*(I1*w1 + I31*w3) - I2*w1*w2))/(I31*I31 - I1*I3) - (I3*(T[0] - w2*(I31*w1 + I3*w3) + I2*w2*w3))/(I31*I31 - I1*I3); 
	IntermediateState dw2 = (T[1] + w1*(I31*w1 + I3*w3) - w3*(I1*w1 + I31*w3))/I2; 
	IntermediateState dw3 = (I31*(T[0] - w2*(I31*w1 + I3*w3) + I2*w2*w3))/(I31*I31 - I1*I3) - (I1*(T[2] + w2*(I1*w1 + I31*w3) - I2*w1*w2))/(I31*I31 - I1*I3); 
	
	// BUILD CONSTRAINTS 
	IntermediateState Const, dConst; 
	Const = - r*r/2 + x*x/2 - x*xA + xA*xA/2 + y*y/2 - y*yA + yA*yA/2 + z*z/2; 
	dConst = dx*x - dr*r - dxA*x - dx*xA + dxA*xA + dy*y - dyA*y - dy*yA + dyA*yA + dz*z; 
	
	///////////////////////////// END OF AUTO-GENERATED CODE ////////////////////////////////////////////////////// 
	
// 	Fcable = lambda*r;

	
	
	// ===============================================================
	//                        END OF MODEL EQUATIONS
	// ===============================================================
	
	
	
	IntermediateState Cost = 0;
	for ( i=0; i < n_U; i++ ) {
		Cost += U[i]*U[i];
	}

    // DEFINE THE NLP:
    // ----------------------------------
	NLP nlp;
	nlp.minimize( Cost );
	nlp.minimize( (CL-0.5)*(CL-0.5) );
	
    
    // CONSTRAINTS:
    // ---------------------------------
	
	// State bounds
	nlp.subjectTo( -0.2 <= ur <= 0.2  );
	nlp.subjectTo( -0.2 <= up <= 0.2  );
	nlp.subjectTo(  0.0 <= x  );
	nlp.subjectTo(   rA <= y  );
	
/*	nlp.subjectTo( -1.0 <= e11 <= 1.0  );
	nlp.subjectTo( -1.0 <= e12 <= 1.0  );
	nlp.subjectTo( -1.0 <= e13 <= 1.0  );
	nlp.subjectTo( -1.0 <= e21 <= 1.0  );
	nlp.subjectTo( -1.0 <= e22 <= 1.0  );
	nlp.subjectTo( -1.0 <= e23 <= 1.0  );
	nlp.subjectTo( -1.0 <= e31 <= 1.0  );
	nlp.subjectTo( -1.0 <= e32 <= 1.0  );
	nlp.subjectTo( -1.0 <= e33 <= 1.0  );*/
	
	nlp.subjectTo(  delta == IC[2] );
	nlp.subjectTo( ddelta == IC[3] );
	
	// Flight envelope
	nlp.subjectTo( 0 <= CL <= 1  );
// 	nlp.subjectTo( CL == 0.5  );
	nlp.subjectTo( 1 <= lambda/10  );
	nlp.subjectTo( -10*PI/180 <= beta <= 10*PI/180 );
// 	nlp.subjectTo( -10*PI/180 <= alpha <= 10*PI/180 );
	nlp.subjectTo( -15*PI/180 <= alphaTail <= 15*PI/180 );
	nlp.subjectTo( 0 <= wE[0]  );
	
	// Invariants
	nlp.subjectTo( Const == 0 );
	nlp.subjectTo( dConst == 0 );
	
	nlp.subjectTo( e11*e11 + e12*e12 + e13*e13 - 1 == 0 );
	nlp.subjectTo( e11*e21 + e12*e22 + e13*e23     == 0 );
	nlp.subjectTo( e11*e31 + e12*e32 + e13*e33     == 0 );
	nlp.subjectTo( e21*e21 + e22*e22 + e23*e23 - 1 == 0 );
	nlp.subjectTo( e21*e31 + e22*e32 + e23*e33     == 0 );
	nlp.subjectTo( e31*e31 + e32*e32 + e33*e33 - 1 == 0 );
	
	// Equilibrium
// ROTATE BACK THE SYSTEM DYNAMICS:
// ---------------------------------------------------------------

	nlp.subjectTo(      z == IC[0] );
	nlp.subjectTo(      r == IC[1] );
	//        Rdot*X                                                 R*dX
// 	nlp.subjectTo( (-x)*ddelta       + dy == 0 );
	nlp.subjectTo( (-y)*ddelta       - dx == 0 );	// You never have y=0
	nlp.subjectTo( dz == 0 );						
	nlp.subjectTo( dr == 0 );
	
	//               mul(Rdotdot,X)                                                                                                                    + 2*mul(Rdot,Xdot)                                      + mul(R,Xdotdot)
// 	nlp.subjectTo( ( ( -dddelta )*x + ( -ddelta*ddelta )*y ) + 2*(-dx)*ddelta  + ddy == 0 );
	nlp.subjectTo( ( (  ddelta*ddelta )*x + ( -dddelta )*y ) + 2*(-dy)*ddelta  - ddx == 0 );
	nlp.subjectTo( ddz == 0 );

	nlp.subjectTo( e11*w1 + e12*w2 + e13*w3          == 0 );
	nlp.subjectTo( e21*w1 + e22*w2 + e23*w3          == 0 );
	nlp.subjectTo( e31*w1 + e32*w2 + e33*w3 - ddelta == 0 );
	
	nlp.subjectTo( dw1 == 0 );
	nlp.subjectTo( dw2 == 0 );
	nlp.subjectTo( dw3 == 0 );
	
	
	nlp.subjectTo( dddelta == 0 );
	nlp.subjectTo( ddr == 0 );
	nlp.subjectTo( dur == 0 );
	nlp.subjectTo( dup == 0 );
	
	
	// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
	OptimizationAlgorithm algorithm(nlp);	

	algorithm.set                         ( MAX_NUM_ITERATIONS, 100 );
    algorithm.set                         ( KKT_TOLERANCE    , 1e-6 );
	

	algorithm.initializeParameters("EQ_init.txt"      );
	
	algorithm.set( MIN_LINESEARCH_PARAMETER, 1e-4 );
	algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
	
	// 	algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );
	
	algorithm.solve();
	
	algorithm.getParameters("EQ_params.txt"    );
	

    return 0;
}
コード例 #6
0
ファイル: scalar3_nnc.cpp プロジェクト: drewm1980/acado
/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------
    Parameter y1,y2,y3;


    // DEFINE AN OPTIMIZATION PROBLEM:
    // -------------------------------
    NLP nlp;
    nlp.minimize( 0, y1 );
    nlp.minimize( 1, y2 );
    nlp.minimize( 2, y3 );


    nlp.subjectTo( -5.0 <= y1 <= 5.0 );
    nlp.subjectTo( -5.0 <= y2 <= 5.0 );
    nlp.subjectTo( -5.0 <= y3 <= 5.0 );

    nlp.subjectTo( y1*y1+y2*y2+y3*y3 <= 4.0 );

    // DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE NLP:
    // -----------------------------------------------------
    MultiObjectiveAlgorithm algorithm(nlp);

    algorithm.set( PARETO_FRONT_GENERATION, PFG_NORMALIZED_NORMAL_CONSTRAINT );
    algorithm.set( PARETO_FRONT_DISCRETIZATION, 11 );

    // Minimize individual objective function
    algorithm.solveSingleObjective(0);

    // Minimize individual objective function
    algorithm.solveSingleObjective(1);

    // Minimize individual objective function
    algorithm.solveSingleObjective(2);

    // Generate Pareto set 
    algorithm.solve();

    algorithm.getWeights("scalar3_nnc_weights.txt");


    // GET THE RESULT FOR THE PARETO FRONT AND PLOT IT:
    // ------------------------------------------------
    VariablesGrid paretoFront;
    // algorithm.getParetoFrontWithFilter( paretoFront );
    algorithm.getParetoFront( paretoFront );

    //GnuplotWindow window;
    //window.addSubplot3D( paretoFront, "Pareto Front y1 vs y2 vs y3","y1","y2", PM_POINTS );
    //window.plot( );

    FILE *file = fopen("scalar3_nnc_pareto.txt","w");
    paretoFront.print();
    file << paretoFront;
    fclose(file);


    // PRINT INFORMATION ABOUT THE ALGORITHM:
    // --------------------------------------
    algorithm.printInfo();

    return 0;
}
コード例 #7
0
ファイル: scalar2_ws.cpp プロジェクト: borishouska/acado
/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // ------------------------
    Parameter y1,y2;


    // DEFINE AN OPTIMIZATION PROBLEM:
    // -------------------------------
    NLP nlp;
    nlp.minimize( 0, y1 );
    nlp.minimize( 1, y2 );

    nlp.subjectTo( 0.0 <= y1 <= 5.0 );
    nlp.subjectTo( 0.0 <= y2 <= 5.2 );
    nlp.subjectTo( 0.0 <= y2 - 5.0*exp(-y1) - 2.0*exp(-0.5*(y1-3.0)*(y1-3.0)) );


    // DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE NLP:
    // -----------------------------------------------------
    MultiObjectiveAlgorithm algorithm(nlp);

    algorithm.set( PARETO_FRONT_GENERATION, PFG_WEIGHTED_SUM );
    algorithm.set( PARETO_FRONT_DISCRETIZATION, 41 );
    algorithm.set( KKT_TOLERANCE, 1e-12 );

    // Generate Pareto set 
    algorithm.solve();

    algorithm.getWeights("scalar2_ws_weights.txt");


    // GET THE RESULT FOR THE PARETO FRONT AND PLOT IT:
    // ------------------------------------------------
    VariablesGrid paretoFront;
    algorithm.getParetoFront( paretoFront );

    GnuplotWindow window1;
    window1.addSubplot( paretoFront, "Pareto Front y1 vs y2", "y1","y2", PM_POINTS );
    window1.plot( );

    paretoFront.print();


    // FILTER THE PARETO FRONT AND PLOT IT:
    // ------------------------------------
    algorithm.getParetoFrontWithFilter( paretoFront );

    GnuplotWindow window2;
    window2.addSubplot( paretoFront, "Pareto Front (with filter) y1 vs y2", "y1","y2", PM_POINTS );
    window2.plot( );

    paretoFront.print();


    // PRINT INFORMATION ABOUT THE ALGORITHM:
    // --------------------------------------
    algorithm.printInfo();

    return 0;
}
コード例 #8
0
ファイル: ninf_full.cpp プロジェクト: akemala/DMP
int main(int argc, char **argv)
{
  USING_NAMESPACE_ACADO
  const double KKT_tol = 1e-6;

  //READ THE DEMO LENGTHS & nBF FROM THE COMMAND LINE
  std::deque<std::string> args(argv + 1, argv + argc + !argc);
  const int nBF=atoi(args[0].c_str());
  args.pop_front();
  const int nD=(int)args.size();
  int nS=0;
  for(int i=0; i<nD;i++)
    nS+=atoi(args[i].c_str());


  //READ DATA
  std::string path=DATA_PATH;
  Matrix D = readFromFile((path+"demos.dat").c_str()); //d(:,0)=time, d(:,1)=x, d(:,2)=dx, d(:,3)=ddx;
  Vector pI = readFromFile((path+"initial_guess.dat").c_str());
  Matrix A = readFromFile((path+"inequality_constraint_matrix.dat").c_str());
  Vector b = readFromFile((path+"inequality_constraint_vector.dat").c_str());
  Matrix S = readFromFile((path+"scale_matrix.dat").c_str());

  //RELEVANT INDEX SETS
  std::vector<std::vector<int> > d_ind=getDemoInd(args);
  std::vector<int> a_ind=getAInd(nBF,nD);
  std::vector<int> b_ind=getBInd(nBF,nD);
  std::vector<std::vector<int> > w_ind=getWInd(nBF,nD);
  std::vector<int> r_ind=getRInd(nBF,nD);
  std::vector<int> c_ind=getCInd(nBF,nD);

  //PARAMETER & OBJECTIVE FUNCTION
 Parameter p(2*nD+nBF*(2+nD)+1,1); 

 Matrix BM(nS,2*nD+nBF*(2+nD)+1); BM.setZero();
 Expression B(BM);
 double t,x,dx;

 for (int d=0; d<nD; d++)
   for(int s=0;s<(int)d_ind[d].size();s++)
     {
       t=D(d_ind[d][s],0);
       x=D(d_ind[d][s],1);
       dx=D(d_ind[d][s],2);

       B(d_ind[d][s],a_ind[d])=x;
       B(d_ind[d][s],b_ind[d])=dx;

       for(int n=0;n<nBF;n++){
	  B(d_ind[d][s],w_ind[d][n])=(-0.5*(t-p(c_ind[n])*S(c_ind[n],c_ind[n])).getPowInt(2)/(p(r_ind[n])*p(r_ind[n])*S(r_ind[n],r_ind[n])*S(r_ind[n],r_ind[n]))).getExp();
	 // std::cout<<d<<std::endl;
	 //std::cout<< S(r_ind[d],r_ind[d])<<std::endl;
       }
     }           

 Expression f;
 f<<B*S*p-D.getCol(3);

 Expression ez(nS);
 for (int i=0; i<nS; i++)
   ez(i)=p(2*nD+nBF*(2+nD));


 Vector e(nS); e.setAll(1.0);
 Vector null(nS); null.setAll(0.0);


  NLP nlp;
  nlp.minimize(p(2*nD+nBF*(2+nD)));
  nlp.subjectTo(f - ez <= null);
  nlp.subjectTo(f + ez >= null);
  //nlp.subjectTo(A*S*p <= b);
  
  //ALGORITHM 
  ParameterEstimationAlgorithm algorithm(nlp);
  VariablesGrid initial_guess(2*nD+nBF*(2+nD)+1,0.0,0.0,1 );
  initial_guess.setVector( 0,S.getInverse()*pI );
  algorithm.initializeParameters(initial_guess);
  
  // OPTIONS
  algorithm.set( KKT_TOLERANCE, KKT_tol);
  algorithm.set( ABSOLUTE_TOLERANCE, 1e-4);
  algorithm.set( PRINTLEVEL,HIGH);
  algorithm.set( MAX_NUM_ITERATIONS, 2000 );
  algorithm.set (PRINT_COPYRIGHT, NO);
  // algorithm.set (PRINT_SCP_METHOD_PROFILE, YES);
  algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); 
  algorithm.set(GLOBALIZATION_STRATEGY, GS_LINESEARCH ); 
  algorithm.set(LINESEARCH_TOLERANCE, 1e-2 ); 
  algorithm.set(INFEASIBLE_QP_HANDLING,IQH_RELAX_L2);
  algorithm.set(FEASIBILITY_CHECK,BT_TRUE);


  // LOGGING
  LogRecord logRecord( LOG_AT_EACH_ITERATION,(path+"log.dat").c_str(),PS_PLAIN);
  logRecord << LOG_OBJECTIVE_VALUE;
  algorithm << logRecord;
 
  //SOLVING
  double clock1 = clock();
  algorithm.solve();
  double clock2 = clock();
  Vector solution;
  algorithm.getParameters(solution);
  // solution.print("optimal solution \n");
  solution.printToFile((path+"solution.dat").c_str(),"",PS_PLAIN);

  printf("\n computation time (ACADO) = %.16e \n", (clock2-clock1)/CLOCKS_PER_SEC);

  return 0;
}