コード例 #1
0
int main( ){

    USING_NAMESPACE_ACADO

    DifferentialState         x;             // definition of a differential state
    AlgebraicState            z;             // definition of an algebraic state
    Control                   u;             // definition of a control
    Parameter                 p;             // definition of a parameter
    DifferentialEquation      f;             // a differential equation


    f << dot(x) == -0.5*x-z+u*u;             // an example for a differential-
    f <<      0 ==  z+exp(z)+x-1.0+u;        // algebraic equation.

    OCP ocp( 0.0, 4.0 );                     // define an OCP with t_0 = 0.0 and T = 4.0
    ocp.minimizeMayerTerm( x*x + p*p );      // a Mayer term to be minimized

    ocp.subjectTo( f );                      // OCP should regard the DAE
    ocp.subjectTo( AT_START, x     == 1.0 ); // an initial value constraint
    ocp.subjectTo( AT_END  , x + p == 1.0 ); // an end (or terminal) constraint

    ocp.subjectTo( -1.0 <= x*u <= 1.0 );     // a path constraint

    OptimizationAlgorithm algorithm(ocp);    // define an algorithm
    algorithm.set( KKT_TOLERANCE, 1e-5 );    // define a termination criterion
	algorithm.solve();                       // to solve the OCP.

    return 0;
}
コード例 #2
0
ファイル: test.cpp プロジェクト: rtkg/acado
int main( ){

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------

    DifferentialState       x,L;
    Control                   u;
    DifferentialEquation    f,g;

    const double t_start =  0.0;
    const double t_end   = 10.0;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------

    f << dot(x) == -x + 0.01*x*x + u;
    f << dot(L) == -x + 0.01*x*x + u;

    g << dot(x) ==  x - 2.00*x*x + u;
    g << dot(L) ==  x*x + u*u;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 60 );

    ocp.minimizeMayerTerm( L );

    ocp.subjectTo( f, 30 );
    ocp.subjectTo( g, 30 );

    ocp.subjectTo( AT_START, x == 1.0 );
    ocp.subjectTo( AT_START, L == 0.0 );


    // Additionally, flush a plotting object
    GnuplotWindow window1;
	window1.addSubplot( x,"DifferentialState x" );
	window1.addSubplot( u,"Control u" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);
    algorithm << window1;
    algorithm.solve();


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

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState         x;
    DifferentialState         l;
    AlgebraicState            z;
    Control                   u;
    DifferentialEquation      f;

    const double t_start =  0.0;
    const double t_end   = 10.0;

    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    f << dot(x) == -x + 0.5*x*x + u + 0.5*z;
    f << dot(l) ==  x*x + 3.0*u*u            ;
    f <<      0 ==  z + exp(z) - 1.0 + x     ;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 10 );
    ocp.minimizeMayerTerm( l );

    ocp.subjectTo( f );
    ocp.subjectTo( AT_START, x == 1.0 );
    ocp.subjectTo( AT_START, l == 0.0 );

    GnuplotWindow window;
        window.addSubplot(x,"DIFFERENTIAL STATE  x");
        window.addSubplot(z,"ALGEBRAIC STATE  z"   );
        window.addSubplot(u,"CONTROL u"            );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ----------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);

	algorithm.set( ABSOLUTE_TOLERANCE   , 1.0e-7        );
	algorithm.set( INTEGRATOR_TOLERANCE , 1.0e-7        );
	algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
	//algorithm.set( GLOBALIZATION_STRATEGY, GS_FULLSTEP );

	algorithm << window;
    algorithm.solve();

    return 0;
}
コード例 #4
0
ファイル: getting_started.cpp プロジェクト: OspreyX/acado
int main( ){

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------

    DifferentialState         x;
    Control                   u;
    Disturbance               w;
    Parameter               p,q;
    DifferentialEquation      f;

    const double t_start = 0.0;
    const double t_end   = 1.0;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------

    f << -dot(x) -x*x + p + u*u + w;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 20 );

    ocp.minimizeMayerTerm( x + p*p + q*q );
    ocp.subjectTo( f );
    ocp.subjectTo( AT_START, x == 1.0 );
    ocp.subjectTo(  0.1 <= u <= 2.0 );
    ocp.subjectTo( -0.1 <= w <= 2.1 );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);

//    algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
    algorithm.solve();


    return 0;
}
コード例 #5
0
ファイル: simple_ocp.cpp プロジェクト: drewm1980/acado
int main( ){

    USING_NAMESPACE_ACADO


    DifferentialState        s,v,m      ;     // the differential states
    Control                  u          ;     // the control input u
    Parameter                T          ;     // the time horizon T
    DifferentialEquation     f( 0.0, T );     // the differential equation

//  -------------------------------------
    OCP ocp( 0.0, T );                        // time horizon of the OCP: [0,T]
    ocp.minimizeMayerTerm( T );               // the time T should be optimized

    f << dot(s) == v;                         // an implementation
    f << dot(v) == (u-0.2*v*v)/m;             // of the model equations
    f << dot(m) == -0.01*u*u;                 // for the rocket.

    ocp.subjectTo( f                   );     // minimize T s.t. the model,
    ocp.subjectTo( AT_START, s ==  0.0 );     // the initial values for s,
    ocp.subjectTo( AT_START, v ==  0.0 );     // v,
    ocp.subjectTo( AT_START, m ==  1.0 );     // and m,

    ocp.subjectTo( AT_END  , s == 10.0 );     // the terminal constraints for s
    ocp.subjectTo( AT_END  , v ==  0.0 );     // and v,

    ocp.subjectTo( -0.1 <= v <=  1.7   );     // as well as the bounds on v
    ocp.subjectTo( -1.1 <= u <=  1.1   );     // the control input u,
    ocp.subjectTo(  5.0 <= T <= 15.0   );     // and the time horizon T.
//  -------------------------------------

    GnuplotWindow window;
        window.addSubplot( s, "THE DISTANCE s"      );
        window.addSubplot( v, "THE VELOCITY v"      );
        window.addSubplot( m, "THE MASS m"          );
        window.addSubplot( u, "THE CONTROL INPUT u" );

    OptimizationAlgorithm algorithm(ocp);     // the optimization algorithm
    algorithm << window;
    algorithm.solve();                        // solves the problem.


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

    USING_NAMESPACE_ACADO


    DifferentialState          phi, omega;    // the states of the pendulum
    Parameter                  l, alpha  ;    // its length and the friction
    const double               g = 9.81  ;    // the gravitational constant
    DifferentialEquation       f         ;    // the model equations
    Function                   h         ;    // the measurement function

    VariablesGrid measurements;                 // read the measurements
    measurements = readFromFile( "data.txt" );  // from a file.

//  --------------------------------------
    OCP ocp(measurements.getTimePoints());    // construct an OCP
    h << phi                             ;    // the state phi is measured
    ocp.minimizeLSQ( h, measurements )   ;    // fit h to the data

    f << dot(phi  ) == omega             ;    // a symbolic implementation
    f << dot(omega) == -(g/l) *sin(phi )      // of the model
                       - alpha*omega     ;    // equations

    ocp.subjectTo( f                    );    // solve OCP s.t. the model,
    ocp.subjectTo( 0.0 <= alpha <= 4.0  );    // the bounds on alpha
    ocp.subjectTo( 0.0 <=   l   <= 2.0  );    // and the bounds on l.
//  --------------------------------------

    GnuplotWindow window;
        window.addSubplot( phi  , "The angle phi", "time [s]", "angle [rad]" );
        window.addSubplot( omega, "The angular velocity dphi"                );
        window.addData( 0, measurements(0) );

    ParameterEstimationAlgorithm algorithm(ocp); // the parameter estimation
    algorithm << window;
    algorithm.solve();                           // algorithm solves the problem.


    return 0;
}
コード例 #7
0
void Shell::select(const std::string& exec, size_t argc, const std::string* argv)
{
	if(exec == "open")
		open(argc, argv);
	else if(exec == "seek")
		seek(argc, argv);
	else if(exec == "tell")
		tell(argc, argv);
	else if(exec == "write")
		write(argc, argv);
	else if(exec == "read")
		read(argc, argv);
	else if(exec == "close")
		close(argc, argv);
	else if(exec == "useradd")
		useradd(argc, argv);
	else if(exec == "chpsd")
		chpsd(argc, argv);
	else if(exec == "cd")
		cd(argc, argv);
	else if(exec == "mkdir")
		mkdir(argc, argv);
	else if(exec == "mkfile")
		mkfile(argc, argv);
	else if(exec == "rm")
		rm(argc, argv);
	else if(exec == "cat")
		cat(argc, argv);
	else if(exec == "ls")
		ls(argc, argv);
	else if(exec == "chmod")
		chmod(argc, argv);
	else if(exec == "see")
		see(argc, argv);
	else if(exec == "ocp")
		ocp(argc, argv);
	else
		runshell(exec, argc, argv);
}
コード例 #8
0
ファイル: getting_started.cpp プロジェクト: OspreyX/acado
int main( )
{
    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------
	DifferentialState xB; //Body Position
	DifferentialState xW; //Wheel Position
	DifferentialState vB; //Body Velocity
	DifferentialState vW; //Wheel Velocity

	Control F;
	Disturbance R;

	double mB = 350.0;
	double mW = 50.0;
	double kS = 20000.0;
	double kT = 200000.0;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    DifferentialEquation f;

	f << dot(xB) == vB;
	f << dot(xW) == vW;
	f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB;
	f << dot(vW) == (  kS*xB - (kT+kS)*xW + kT*R - F ) / mW;


    // DEFINE LEAST SQUARE FUNCTION:
    // -----------------------------
    Function h;

    h << xB;
    h << xW;
	h << vB;
    h << vW;
	h << F;

    DMatrix Q(5,5); // LSQ coefficient matrix
	Q(0,0) = 10.0;
	Q(1,1) = 10.0;
	Q(2,2) = 1.0;
	Q(3,3) = 1.0;
	Q(4,4) = 1.0e-8;

    DVector r(5); // Reference
    r.setAll( 0.0 );


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    const double tStart = 0.0;
    const double tEnd   = 1.0;

    OCP ocp( tStart, tEnd, 20 );

    ocp.minimizeLSQ( Q, h, r );

	ocp.subjectTo( f );

	ocp.subjectTo( -200.0 <= F <= 200.0 );
	ocp.subjectTo( R == 0.0 );


    // SETTING UP THE REAL-TIME ALGORITHM:
    // -----------------------------------
	RealTimeAlgorithm alg( ocp,0.025 );
	alg.set( MAX_NUM_ITERATIONS, 1 );
	alg.set( PLOT_RESOLUTION, MEDIUM );

	GnuplotWindow window;
	  window.addSubplot( xB, "Body Position [m]" );
	  window.addSubplot( xW, "Wheel Position [m]" );
	  window.addSubplot( vB, "Body Velocity [m/s]" );
	  window.addSubplot( vW, "Wheel Velocity [m/s]" );
	  window.addSubplot( F,  "Damping Force [N]" );
	  window.addSubplot( R,  "Road Excitation [m]" );

	alg << window;


    // SETUP CONTROLLER AND PERFORM A STEP:
    // ------------------------------------
	StaticReferenceTrajectory zeroReference( "ref.txt" );

	Controller controller( alg,zeroReference );

	DVector y( 4 );
	y.setZero( );
	y(0) = 0.01;

	if (controller.init( 0.0,y ) != SUCCESSFUL_RETURN)
		exit( 1 );
	if (controller.step( 0.0,y ) != SUCCESSFUL_RETURN)
		exit( 1 );

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

    USING_NAMESPACE_ACADO

    // INTRODUCE FIXED PARAMETERS:
    // ---------------------------
    #define  Csin	2.8


    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState     x1,x2,x3,x4,x5;
    IntermediateState     mu,sigma,pif;
    Control               u           ;
    Parameter             tf          ;
    DifferentialEquation  f(0.0,tf)   ;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    mu    = 0.125*x2/x4;
    sigma = mu/0.135;
    pif   = (-384.0*mu*mu + 134.0*mu);

    f << dot(x1) ==  mu*x1;
    f << dot(x2) == -sigma*x1 + u*Csin;
    f << dot(x3) ==  pif*x1;
    f << dot(x4) ==  u;
    f << dot(x5) ==  0.001*(u*u + 0.01*tf*tf);


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0.0, tf, 50 );
    ocp.minimizeMayerTerm(0, 0.01*x5 -x3/tf          ); // Solve productivity optimal problem (Note: - due to maximization, small regularisation term)
    ocp.minimizeMayerTerm(1, 0.01*x5 -x3/(Csin*x4-x2)); // Solve yield optimal problem (Note: Csin = x2(t=0)/x4(t=0); - due to maximization, small regularisation term)

    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, x1 ==   0.1           );
    ocp.subjectTo( AT_START, x2 ==  14.0           );
    ocp.subjectTo( AT_START, x3 ==   0.0           );
    ocp.subjectTo( AT_START, x4 ==   5.0           );
    ocp.subjectTo( AT_START, x5 ==   0.0           );

    ocp.subjectTo(  0.0 <= x1 <=   15.0  );
    ocp.subjectTo(  0.0 <= x2 <=   30.0  );
    ocp.subjectTo( -0.1 <= x3 <= 1000.0  );
    ocp.subjectTo(  5.0 <= x4 <=   20.0  );
    ocp.subjectTo( 20.0 <= tf <=   40.0  );
    ocp.subjectTo(  0.0 <= u  <=    2.0  );


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

    algorithm.set( PARETO_FRONT_DISCRETIZATION, 21                               );
    algorithm.set( PARETO_FRONT_GENERATION    , PFG_NORMALIZED_NORMAL_CONSTRAINT );
    algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
    algorithm.set( KKT_TOLERANCE, 1e-7 );

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

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

    // Generate Pareto set
    algorithm.set( KKT_TOLERANCE, 1e-6 );
    algorithm.solve();

    algorithm.getWeights("fed_batch_bioreactor_nnc_weights.txt");
    algorithm.getAllDifferentialStates("fed_batch_bioreactor_nnc_states.txt");
    algorithm.getAllControls("fed_batch_bioreactor_nnc_controls.txt");
    algorithm.getAllParameters("fed_batch_bioreactor_nnc_parameters.txt");


    // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
    // ------------------------------------------
    VariablesGrid paretoFront;
    algorithm.getParetoFront( paretoFront );

    GnuplotWindow window1;
    window1.addSubplot( paretoFront, "Pareto Front (yield versus productivity)", "-PRODUCTIVTY", "-YIELD", PM_POINTS );
    window1.plot( );


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


    // SAVE INFORMATION:
    // -----------------
    FILE *file = fopen("fed_batch_bioreactor_nnc_pareto.txt","w");
    paretoFront.print();
    file << paretoFront;
    fclose(file);

    return 0;
}
コード例 #10
0
ファイル: dae_simulation.cpp プロジェクト: drewm1980/acado
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState         x;
    DifferentialState         l;
    AlgebraicState            z;
    Control                   u;
    DifferentialEquation      f;
//     Disturbance R;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    f << dot(x) == -x + 0.5*x*x + u + 0.5*z  ;
    f << dot(l) ==  x*x + 3.0*u*u         ;
    f <<      0 ==  z + exp(z) - 1.0 + x     ;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0.0, 5.0, 10 );
    ocp.minimizeMayerTerm( l );

    ocp.subjectTo( f );
//     ocp.subjectTo( R == 0.0 );


    // SETTING UP THE (SIMULATED) PROCESS:
    // -----------------------------------
	OutputFcn identity;
	DynamicSystem dynamicSystem( f,identity );

	Process process( dynamicSystem,INT_BDF );

	//VariablesGrid disturbance = readFromFile( "dae_simulation_disturbance.txt" );
	//process.setProcessDisturbance( disturbance );


    // SETTING UP THE MPC CONTROLLER:
    // ------------------------------
	RealTimeAlgorithm alg( ocp,0.5 );

	StaticReferenceTrajectory zeroReference;
	Controller controller( alg,zeroReference );


    // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
    // ----------------------------------------------------------
	SimulationEnvironment sim( 0.0,15.0,process,controller );

	Vector x0(2);
	x0(0) = 1;
	x0(1) = 0;

	sim.init( x0 );
	sim.run( );


    // ...AND PLOT THE RESULTS
    // ----------------------------------------------------------
	VariablesGrid diffStates;
	sim.getProcessDifferentialStates( diffStates );
	diffStates.printToFile( "diffStates.txt" );
	diffStates.printToFile( "diffStates.m","DIFFSTATES",PS_MATLAB );

	VariablesGrid sampledProcessOutput;
    sim.getSampledProcessOutput( sampledProcessOutput );
    sampledProcessOutput.printToFile( "sampledOut.txt" );
    sampledProcessOutput.printToFile( "sampledOut.m","OUT",PS_MATLAB );

    VariablesGrid feedbackControl;
    sim.getFeedbackControl( feedbackControl );
	feedbackControl.printToFile( "controls.txt" );
	feedbackControl.printToFile( "controls.m","CONTROL",PS_MATLAB );

	VariablesGrid algStates;
	sim.getProcessAlgebraicStates( algStates );
	algStates.printToFile( "algStates.txt" );
	algStates.printToFile( "algStates.m","ALGSTATES",PS_MATLAB );


    GnuplotWindow window;
		window.addSubplot( diffStates(0), "DIFFERENTIAL STATE: x" );
		window.addSubplot( diffStates(1), "DIFFERENTIAL STATE: l" );
		window.addSubplot( algStates(0),            "ALGEBRAIC STATE: z"    );
		window.addSubplot( feedbackControl(0),      "CONTRUL: u"            );
    window.plot( );


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

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // ----------------------------
    DifferentialState    x(10);    // a differential state vector with dimension 10. (vector)
    DifferentialState    y    ;    // another differential state y                   (scalar)
    Control              u(2 );    // a control input with dimension 2.              (vector)
    Parameter            p    ;    // a parameter (here a scalar).                   (scalar)

    DifferentialEquation f    ;    // the differential equation

    const double t_start =  0.0;
    const double t_end   =  1.0;


    // READ A MATRIX "A" FROM A FILE:
    // ------------------------------
    Matrix A = readFromFile( "matrix_vector_ocp_A.txt" );
    Matrix B = readFromFile( "matrix_vector_ocp_B.txt" );


    // READ A VECTOR "x0" FROM A FILE:
    // -------------------------------
    Vector x0 = readFromFile( "matrix_vector_ocp_x0.txt" );


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    f << dot(x) == -(A*x) + B*u;                           // matrix vector notation for a linear equation
    f << dot(y) == x.transpose()*x + 2.0*u.transpose()*u;  // matrix vector notation:  x^x  = scalar product = ||x||_2^2
                                                           //                          u^u  = scalar product = ||u||_2^2


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 20 );
    ocp.minimizeMayerTerm( y );
    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, x == x0  );
    ocp.subjectTo( AT_START, y == 0.0 );


    GnuplotWindow window;
        window.addSubplot( x(0),"x0" );
        window.addSubplot( x(6),"x6" );
        window.addSubplot( u(0),"u0" );
        window.addSubplot( u(1),"u1" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);

    algorithm.set( MAX_NUM_ITERATIONS, 20 );
    algorithm.set( KKT_TOLERANCE, 1e-10 );

    algorithm << window;
    algorithm.solve();

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

	// DEFINE THE VARIABLES:
	// ----------------------------------------------------------
	DifferentialState   p    ;  // the trolley position
	DifferentialState   v    ;  // the trolley velocity 
	DifferentialState   phi  ;  // the excitation angle
	DifferentialState   omega;  // the angular velocity
	Control             a    ;  // the acc. of the trolley

	const double     g = 9.81;  // the gravitational constant 
	const double     b = 0.20;  // the friction coefficient
	// ----------------------------------------------------------


	// DEFINE THE MODEL EQUATIONS:
	// ----------------------------------------------------------
	DifferentialEquation f; 

	f << dot( p     )  ==  v                                ;
	f << dot( v     )  ==  a                                ;
	f << dot( phi   )  ==  omega                            ;
	f << dot( omega )  == -g*sin(phi) - a*cos(phi) - b*omega;
	// ----------------------------------------------------------


	// DEFINE THE WEIGHTING MATRICES:
	// ----------------------------------------------------------
	Matrix Q  = eye(4);
	Matrix R  = eye(1);
	
	Matrix P  = eye(4);
	P *= 5.0;
	// ----------------------------------------------------------


	// SET UP THE MPC - OPTIMAL CONTROL PROBLEM:
	// ----------------------------------------------------------
	OCP ocp( 0.0,3.0, 10 );

	ocp.minimizeLSQ       ( Q,R );
	ocp.minimizeLSQEndTerm( P   );

	ocp.subjectTo( f );
	ocp.subjectTo( -1.0 <= a <= 1.0 );
// 	ocp.subjectTo( -0.5 <= v <= 1.5 );
	// ----------------------------------------------------------


	// DEFINE AN MPC EXPORT MODULE AND GENERATE THE CODE:
	// ----------------------------------------------------------
	MPCexport mpc( ocp );

	mpc.set( HESSIAN_APPROXIMATION,       GAUSS_NEWTON    );
	mpc.set( DISCRETIZATION_TYPE,         SINGLE_SHOOTING );
	mpc.set( INTEGRATOR_TYPE,             INT_RK4         );
	mpc.set( NUM_INTEGRATOR_STEPS,        30              );
	mpc.set( QP_SOLVER,                   QP_QPOASES      );
// 	mpc.set( MAX_NUM_QP_ITERATIONS,       20              );
// 	mpc.set( HOTSTART_QP,                 YES             );
// 	mpc.set( LEVENBERG_MARQUARDT,         1.0e-4          );
	mpc.set( GENERATE_TEST_FILE,          YES             );
	mpc.set( GENERATE_MAKE_FILE,          YES             );
	mpc.set( GENERATE_SIMULINK_INTERFACE, NO              );
// 	mpc.set( OPERATING_SYSTEM,            OS_WINDOWS      );
// 	mpc.set( USE_SINGLE_PRECISION,        YES             );

	mpc.exportCode( "getting_started_export" );
	// ----------------------------------------------------------

	return 0;
}
コード例 #13
0
USING_NAMESPACE_ACADO


int main( ){


    // INTRODUCE THE VARIABLES:
    // --------------------------------------------------
    DifferentialState     xp, yp, zp, phi, theta, psi, vx, vy, vz, p, q, r, cost;
    Control               u1, u2, u3, u4;
    Parameter             T;

    // DEFINE THE DIMENSIONS OF THE C-FUNCTIONS:
    // --------------------------------------------------
    CFunction F( 13, myDifferentialEquation     );
    //CFunction M( NJ, myObjectiveFunction        );
    //CFunction I( NI, myInitialValueConstraint   );
    //CFunction E( NE, myEndPointConstraint       );
    //CFunction H( NH, myInequalityPathConstraint );


    DifferentialEquation  f      ;

//    const double h = 0.01;
//    DiscretizedDifferentialEquation  f(h);
//    F.setUserData((void*) &h);

    // DEFINE THE OPTIMIZATION VARIABLES:
    // --------------------------------------------------

    IntermediateState x(17);

     x(0) = xp;  x(1) = yp;  x(2) = zp;  x(3) = phi;  x(4) = theta;  x(5) = psi;
     x(6) = vx;  x(7) = vy;  x(8) = vz;  x(9) = p; x(10) = q; x(11) = r; x(12) = cost;
     x(13) = u1; x(14) = u2; x(15) = u3; x(16) = u4;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0.0, T, 20 );

    ocp.minimizeMayerTerm(T);//x(12));

    ocp.subjectTo( f << F(x)*T );

    ocp.subjectTo( 3 <= T <= 15);

    // Start constraints
    for (unsigned int k=0 ; k < 13 ; k++) {
        ocp.subjectTo(AT_START, x(k) == 0.0);
    }

    // End constraints
    ocp.subjectTo( AT_END, x(0) ==  10.0 );
    ocp.subjectTo( AT_END, x(1) ==  0.0 );
    ocp.subjectTo( AT_END, x(2) ==  0.0 );
    for (unsigned int k=3 ; k < 12 ; k++) {
        ocp.subjectTo(AT_END, x(k) == 0.0 );
    }

    // Limits on commands
    ocp.subjectTo(0 <= x(13) <= 60);
    ocp.subjectTo(-5 <= x(14) <= 5);
    ocp.subjectTo(-5 <= x(15) <= 5);
    ocp.subjectTo(-5 <= x(16) <= 5);

    ocp.subjectTo(x(5) == 0.);
    ocp.subjectTo( -1.57 <= x(3) <= 1.57);
    ocp.subjectTo( -1.57 <= x(4) <= 1.57);
//    ocp.subjectTo(x(4) == 0.);

    // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
    // ------------------------------------------
    GnuplotWindow window1(PLOT_AT_EACH_ITERATION);
    window1.addSubplot( x(0),"x" );
    window1.addSubplot( x(1) , "y");
    window1.addSubplot( x(2), "z");
    window1.addSubplot( x(3),"phi" );
    window1.addSubplot( x(4) , "theta");
    window1.addSubplot( x(5), "psi");
    window1.addSubplot( x(13),"u1" );
    window1.addSubplot( x(14),"u2" );
    window1.addSubplot( x(15),"u3" );
    window1.addSubplot( x(16),"u4" );

    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);
    // algorithm.set( INTEGRATOR_TOLERANCE, 1e-3 );
//    algorithm.set( DISCRETIZATION_TYPE, SINGLE_SHOOTING   );
    //algorithm.set( HESSIAN_APPROXIMATION, FULL_BFGS_UPDATE );
    algorithm.set( INTEGRATOR_TYPE, INT_RK45 );
    algorithm.set( MAX_NUM_ITERATIONS, 500 );
    algorithm.set( KKT_TOLERANCE, 1e-12);
    algorithm << window1;

//    double testx[16];
//    for (i=0 ; i<12 ; i++) {
//        testx[i] = 0.;
//    }
//    testx[4] = 0;
//    testx[3] = 1.57;
//    testx[13] = 0.;
//    testx[14] = 0.;
//    testx[15] = 0.;
//    testx[16] = 1.;

//    double testf[13];
//    for (i=0 ; i<12 ; i++) {
//        testf[i] = 0;
//    }
//    myDifferentialEquation( testx, testf, testf );

//    for ( unsigned int k=0 ; k<12 ; k++ ) {
//        std::cout << testf[k] << std::endl;
//    }
    algorithm.solve();

    return 0;

}
コード例 #14
0
ファイル: dev_powerkite_on.cpp プロジェクト: OspreyX/acado
USING_NAMESPACE_ACADO

int main( )
{

// DIFFERENTIAL STATES :
// -------------------------
   DifferentialState      r;      //  the length r of the cable
   DifferentialState    phi;      //  the angle phi
   DifferentialState  theta;      //  the angle theta (grosser wert- naeher am boden/kleiner wert - weiter oben)
// -------------------------      //  -------------------------------------------
   DifferentialState     dr;      //  first  derivative of r0    with respect to t
   DifferentialState   dphi;      //  first  derivative of phi   with respect to t
   DifferentialState dtheta;      //  first  derivative of theta with respect to t
// -------------------------      //  -------------------------------------------
   DifferentialState      n;      //  winding number(rauslassen)
// -------------------------      //  -------------------------------------------
   DifferentialState    Psi;      //  the roll angle Psi
   DifferentialState     CL;      //  the aerodynamic lift coefficient
// -------------------------      //  -------------------------------------------
   DifferentialState      W;      //  integral over the power at the generator
                                  //  ( = ENERGY )(rauslassen)


   // MEASUREMENT FUNCTION :
// -------------------------
   Function                   model_response         ;    // the measurement function


// CONTROL :
// -------------------------
   Control             ddr0;      //  second derivative of r0    with respect to t
   Control             dPsi;      //  first  derivative of Psi   with respect to t
   Control              dCL;      //  first  derivative of CL    with respect to t


   Disturbance       w_extra;


// PARAMETERS :
// ------------------------
                                     //  PARAMETERS OF THE KITE :
                                     //  -----------------------------
   double         mk =  2000.00;      //  mass of the kite               //  [ kg    ](maybe change to 5000kg)
   double          A =  500.00;      //  effective area                 //  [ m^2   ]
   double          V =  720.00;      //  volume                         //  [ m^3   ]
   double        cd0 =    0.04;      //  aerodynamic drag coefficient   //  [       ]
                                     //  ( cd0: without downwash )
   double          K =    0.04;      //  induced drag constant          //  [       ]


                                     //   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 = 0.05614;      //  diameter                       //  [ m     ]


                                     //  PARAMETERS OF THE WIND :
                                     //  -----------------------------
   double         w0 =   10.00;      //  wind velocity at altitude h0   //  [ m/s   ]
   double         h0 =  100.00;      //  the altitude h0                //  [ m     ]
   double         hr =    0.10;      //  roughness length               //  [ m     ]



// OTHER VARIABLES :
// ------------------------

   double AQ                       ;      //  cross sectional area

   IntermediateState     mc;      //  mass of the cable
   IntermediateState     m ;      //  effective inertial mass
   IntermediateState     m_;      //  effective gravitational mass

   IntermediateState     Cg;

   IntermediateState     dm;      //  first  derivative of m     with respect to t


// DEFINITION OF PI :
// ------------------------

   double PI = 3.1415926535897932;


// ORIENTATION AND FORCES :
// ------------------------

   IntermediateState h               ;      //  altitude of the kite
   IntermediateState w               ;      //  the wind at altitude h
   IntermediateState we          [ 3];      //  effective wind vector
   IntermediateState nwe             ;      //  norm of the effective wind vector
   IntermediateState nwep            ;      //  -
   IntermediateState ewep        [ 3];      //  projection of ewe
   IntermediateState eta             ;      //  angle eta: cf. [2]
   IntermediateState et          [ 3];      //  unit vector from the left to the right wing tip                          
   IntermediateState Caer            ;
   IntermediateState Cf              ;      //  simple constants
   IntermediateState CD              ;      //  the aerodynamic drag coefficient
   IntermediateState Fg          [ 3];      //  the gravitational force
   IntermediateState Faer        [ 3];      //  the aerodynamic force
   IntermediateState Ff          [ 3];      //  the frictional force
   IntermediateState F           [ 3];      //  the total force


// TERMS ON RIGHT-HAND-SIDE
// OF THE DIFFERENTIAL
// EQUATIONS              :
// ------------------------

   IntermediateState a_pseudo    [ 3];      //  the pseudo accelaration
   IntermediateState dn              ;      //  the time derivate of the kite's winding number
   IntermediateState ddr             ;      //  second derivative of s     with respect to t
   IntermediateState ddphi           ;      //  second derivative of phi   with respect to t
   IntermediateState ddtheta         ;      //  second derivative of theta with respect to t
   IntermediateState power           ;      //  the power at the generator
// ------------------------        ------                   //  ----------------------------------------------
   IntermediateState regularisation  ;      //  regularisation of controls



//                        MODEL EQUATIONS :
// ===============================================================



// SPRING CONSTANT 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
   m_      =  mk + mc     / 2.0;   // effective gravitational mass
// -----------------------------   // ----------------------------
   dm      =  (rhoc*AQ/ 3.0)*dr;   // time derivative of the mass


// WIND SHEAR MODEL :
// ---------------------------------------------------------------
// for startup +60m
   h       =  r*cos(theta)+60.0                                       ;
//    h       =  r*cos(theta)                                       ;
   w       =  log(h/hr) / log(h0/hr) *(w0+w_extra)                    ;


// EFFECTIVE WIND IN THE KITE`S SYSTEM :
// ---------------------------------------------------------------

   we[0]   =  w*sin(theta)*cos(phi) -              dr    ;
   we[1]   = -w*sin(phi)            - r*sin(theta)*dphi  ;
   we[2]   = -w*cos(theta)*cos(phi) + r           *dtheta;


// CALCULATION OF THE KITE`S TRANSVERSAL AXIS :
// ---------------------------------------------------------------

   nwep    =  pow(                we[1]*we[1] + we[2]*we[2], 0.5 );
   nwe     =  pow(  we[0]*we[0] + we[1]*we[1] + we[2]*we[2], 0.5 );
   eta     =  asin( we[0]*tan(Psi)/ nwep )                       ;

// ---------------------------------------------------------------

// ewep[0] =  0.0                                                ;
   ewep[1] =  we[1] / nwep                                       ;
   ewep[2] =  we[2] / nwep                                       ;

// ---------------------------------------------------------------

   et  [0] =  sin(Psi)                                                  ;
   et  [1] =  (-cos(Psi)*sin(eta))*ewep[1] - (cos(Psi)*cos(eta))*ewep[2];
   et  [2] =  (-cos(Psi)*sin(eta))*ewep[2] + (cos(Psi)*cos(eta))*ewep[1];




// CONSTANTS FOR SEVERAL FORCES :
// ---------------------------------------------------------------

   Cg      =  (V*rho-m_)*g                                       ;
   Caer    =  (rho*A/2.0 )*nwe                                   ;
   Cf      =  (rho*dc/8.0)*r*nwe                                 ;


// THE DRAG-COEFFICIENT :
// ---------------------------------------------------------------

   CD      =  cd0 + K*CL*CL                                      ;



// SUM OF GRAVITATIONAL AND LIFTING FORCE :
// ---------------------------------------------------------------

   Fg  [0] =  Cg  *  cos(theta)                                  ;
// Fg  [1] =  Cg  *  0.0                                         ;
   Fg  [2] =  Cg  *  sin(theta)                                  ;


// SUM OF THE AERODYNAMIC FORCES :
// ---------------------------------------------------------------

   Faer[0] =  Caer*( CL*(we[1]*et[2]-we[2]*et[1]) + CD*we[0] )   ;
   Faer[1] =  Caer*( CL*(we[2]*et[0]-we[0]*et[2]) + CD*we[1] )   ;
   Faer[2] =  Caer*( CL*(we[0]*et[1]-we[1]*et[0]) + CD*we[2] )   ;


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

// Ff  [0] =  Cf  *  cc* we[0]                                   ;
   Ff  [1] =  Cf  *  cc* we[1]                                   ;
   Ff  [2] =  Cf  *  cc* we[2]                                   ;



// THE TOTAL FORCE :
// ---------------------------------------------------------------

   F   [0] = Fg[0] + Faer[0]                                     ;
   F   [1] =         Faer[1] + Ff[1]                             ;
   F   [2] = Fg[2] + Faer[2] + Ff[2]                             ;



// THE PSEUDO ACCELERATION:
// ---------------------------------------------------------------

   a_pseudo [0] =  - ddr0
                   + r*(                         dtheta*dtheta
                         + sin(theta)*sin(theta)*dphi  *dphi   )
                   - dm/m*dr                                     ;

   a_pseudo [1] =  - 2.0*cos(theta)/sin(theta)*dphi*dtheta
                   - 2.0*dr/r*dphi
                   - dm/m*dphi                                   ;

   a_pseudo [2] =    cos(theta)*sin(theta)*dphi*dphi
                   - 2.0*dr/r*dtheta
                   - dm/m*dtheta                                 ;




// THE EQUATIONS OF MOTION:
// ---------------------------------------------------------------

   ddr          =  F[0]/m                + a_pseudo[0]           ;
   ddphi        =  F[1]/(m*r*sin(theta)) + a_pseudo[1]           ;
   ddtheta      = -F[2]/(m*r           ) + a_pseudo[2]           ;





// THE DERIVATIVE OF THE WINDING NUMBER :
// ---------------------------------------------------------------

   dn           =  (        dphi*ddtheta - dtheta*ddphi     ) /
                   (2.0*PI*(dphi*dphi    + dtheta*dtheta)   )      ;



// THE POWER AT THE GENERATOR :
// ---------------------------------------------------------------

   power        =   m*ddr*dr                                     ;



// REGULARISATION TERMS :
// ---------------------------------------------------------------


   regularisation =    5.0e2 * ddr0    * ddr0
                     + 1.0e8 * dPsi    * dPsi
                     + 1.0e5 * dCL     * dCL
                     + 2.5e5 * dn      * dn
                     + 2.5e7 * ddphi   * ddphi;
                     + 2.5e7 * ddtheta * ddtheta;
                     + 2.5e6 * dtheta  * dtheta;
//                   ---------------------------




// REFERENCE TRAJECTORY:
// ---------------------------------------------------------------
        VariablesGrid myReference; myReference.read( "ref_w_zeros.txt" );// read the measurements
        PeriodicReferenceTrajectory reference( myReference );


// THE "RIGHT-HAND-SIDE" OF THE ODE:
// ---------------------------------------------------------------
   DifferentialEquation f;

   f  << dot(r)      ==  dr                             ;
   f  << dot(phi)    ==  dphi                           ;
   f  << dot(theta)  ==  dtheta                         ;
   f  << dot(dr)     ==  ddr0                           ;
   f  << dot(dphi)   ==  ddphi                          ;
   f  << dot(dtheta) ==  ddtheta                        ;
   f  << dot(n)      ==  dn                             ;
   f  << dot(Psi)    ==  dPsi                           ;
   f  << dot(CL)     ==  dCL                            ;
   f  << dot(W)      == (-power + regularisation)*1.0e-6;


   model_response << r                             ;    // the state r is measured
   model_response << phi  ;
   model_response << theta;
   model_response << dr   ;
   model_response << dphi ;
   model_response << dtheta;
   model_response << ddr0;
   model_response << dPsi;
   model_response << dCL ;


   DVector x_scal(9);

	x_scal(0) =   60.0; 
	x_scal(1) =   1.0e-1;
	x_scal(2) =   1.0e-1;
	x_scal(3) =   40.0; 
	x_scal(4) =   1.0e-1;
	x_scal(5) =   1.0e-1;
	x_scal(6) =   60.0; 
	x_scal(7) =   1.5e-1;
	x_scal(8) =   2.5;  

                 
   DMatrix Q(9,9);
   Q.setIdentity();
   DMatrix Q_end(9,9);
   Q_end.setIdentity();
   int i;
   for( i = 0; i < 6; i++ ){
	   Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i));
           Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i));            
     }
   for( i = 6; i < 9; i++ ){
	   Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i));
           Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i));            
     }                                           

     DVector measurements(9);
     measurements.setAll( 0.0 );


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
   const double t_start = 0.0;
   const double t_end   = 10.0;
   OCP ocp( t_start, t_end, 10 );
    ocp.minimizeLSQ( Q,model_response, measurements )   ;    // fit h to the data    
    ocp.minimizeLSQEndTerm( Q_end,model_response, measurements )   ;
    ocp.subjectTo( f );


    ocp.subjectTo( -0.34   <= phi   <= 0.34   );
    ocp.subjectTo(  0.85   <= theta <= 1.45   );
    ocp.subjectTo( -40.0   <= dr    <= 10.0   );
    ocp.subjectTo( -0.29   <= Psi   <= 0.29   );
    ocp.subjectTo(  0.1    <= CL    <= 1.50   );
    ocp.subjectTo( -0.7    <= n     <= 0.90   );
    ocp.subjectTo( -25.0   <= ddr0  <= 25.0   );
    ocp.subjectTo( -0.065  <= dPsi  <= 0.065  );
    ocp.subjectTo( -3.5    <= dCL   <= 3.5    );
    ocp.subjectTo( -60.0   <= cos(theta)*r    );
	ocp.subjectTo( w_extra == 0.0  );


// SETTING UP THE (SIMULATED) PROCESS:
    // -----------------------------------
    OutputFcn identity;
    DynamicSystem dynamicSystem( f,identity );
    Process process( dynamicSystem,INT_RK45 );


	VariablesGrid disturbance; disturbance.read( "my_wind_disturbance_controlsfree.txt" );
	if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
		exit( EXIT_FAILURE );

    // SETUP OF THE ALGORITHM AND THE TUNING OPTIONS:
    // ----------------------------------------------
    double samplingTime = 1.0;
    RealTimeAlgorithm  algorithm( ocp, samplingTime );
    if (algorithm.initializeDifferentialStates("p_s_ref.txt"    ) != SUCCESSFUL_RETURN)
    	exit( EXIT_FAILURE );
    if (algorithm.initializeControls          ("p_c_ref.txt"  ) != SUCCESSFUL_RETURN)
    	exit( EXIT_FAILURE );

    algorithm.set( MAX_NUM_ITERATIONS, 2  );
    algorithm.set( KKT_TOLERANCE    , 1e-4 );
    algorithm.set( HESSIAN_APPROXIMATION,GAUSS_NEWTON);
    algorithm.set( INTEGRATOR_TOLERANCE, 1e-6           );
	algorithm.set( GLOBALIZATION_STRATEGY,GS_FULLSTEP );
// 	algorithm.set( USE_IMMEDIATE_FEEDBACK, YES );
	algorithm.set( USE_REALTIME_SHIFTS, YES );
	algorithm.set(LEVENBERG_MARQUARDT, 1e-5);


    DVector x0(10);
    x0(0) =  1.8264164528775887e+03;
    x0(1) = -5.1770453309520573e-03;
    x0(2) =  1.2706440287266794e+00;
    x0(3) =  2.1977888424944396e+00;
    x0(4) =  3.1840786108641383e-03;
    x0(5) = -3.8281200674676448e-02;
    x0(6) =  0.0000000000000000e+00;
    x0(7) = -1.0372313936413566e-02;
    x0(8) =  1.4999999999999616e+00;
    x0(9) =  0.0000000000000000e+00;


    // SETTING UP THE NMPC CONTROLLER:
    // -------------------------------

	Controller controller( algorithm, reference );

    // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
    // ----------------------------------------------------------
    double simulationStart =  0.0;
    double simulationEnd   =  10.0;

    SimulationEnvironment sim( simulationStart, simulationEnd, process, controller );

    if (sim.init( x0 ) != SUCCESSFUL_RETURN)
    	exit( EXIT_FAILURE );
    if (sim.run( ) != SUCCESSFUL_RETURN)
    	exit( EXIT_FAILURE );

    // ...AND PLOT THE RESULTS
    // ----------------------------------------------------------

	VariablesGrid diffStates;
	sim.getProcessDifferentialStates( diffStates );
	diffStates.print( "diffStates.txt" );
	diffStates.print( "diffStates.m","DIFFSTATES",PS_MATLAB );

	VariablesGrid interStates;
	sim.getProcessIntermediateStates( interStates );
	interStates.print( "interStates.txt" );
	interStates.print( "interStates.m","INTERSTATES",PS_MATLAB );

    VariablesGrid sampledProcessOutput;
    sim.getSampledProcessOutput( sampledProcessOutput );
    sampledProcessOutput.print( "sampledOut.txt" );
    sampledProcessOutput.print( "sampledOut.m","OUT",PS_MATLAB );

    VariablesGrid feedbackControl;
    sim.getFeedbackControl( feedbackControl );
	feedbackControl.print( "controls.txt" );
	feedbackControl.print( "controls.m","CONTROL",PS_MATLAB );

    GnuplotWindow window;
		window.addSubplot( sampledProcessOutput(0), "DIFFERENTIAL STATE: r" );
		window.addSubplot( sampledProcessOutput(1), "DIFFERENTIAL STATE: phi" );
		window.addSubplot( sampledProcessOutput(2), "DIFFERENTIAL STATE: theta" );
		window.addSubplot( sampledProcessOutput(3), "DIFFERENTIAL STATE: dr" );
		window.addSubplot( sampledProcessOutput(4), "DIFFERENTIAL STATE: dphi" );
		window.addSubplot( sampledProcessOutput(5), "DIFFERENTIAL STATE: dtheta" );
		window.addSubplot( sampledProcessOutput(7), "DIFFERENTIAL STATE: Psi" );
		window.addSubplot( sampledProcessOutput(8), "DIFFERENTIAL STATE: CL" );
		window.addSubplot( sampledProcessOutput(9), "DIFFERENTIAL STATE: W" );
	
		window.addSubplot( feedbackControl(0), "CONTROL 1 DDR0" );
		window.addSubplot( feedbackControl(1), "CONTROL 1 DPSI" );
		window.addSubplot( feedbackControl(2), "CONTROL 1 DCL" );
    window.plot( );
	
	GnuplotWindow window2;
	window2.addSubplot( interStates(1) );
	window2.plot();
	
	return 0;
}
コード例 #15
0
int main()
{
	USING_NAMESPACE_ACADO

	// Variables:
	DifferentialState phi;
	DifferentialState theta;
	DifferentialState dphi;
	DifferentialState dtheta;

	Control u1;
	Control u2;

	IntermediateState c;// c := rho * |we|^2 / (2*m)
	DifferentialEquation f;// the right-hand side of the model

	// Parameters:
	const double R = 1.00;// radius of the carousel arm
	const double Omega = 1.00;// constant rotational velocity of the carousel arm
	const double m = 0.80;// the mass of the plane
	const double r = 1.00;// length of the cable between arm and plane
	const double A = 0.15;// wing area of the plane

	const double rho = 1.20;// density of the air
	const double CL = 1.00;// nominal lift coefficient
	const double CD = 0.15;// nominal drag coefficient
	const double b = 15.00;// roll stabilization coefficient
	const double g = 9.81;// gravitational constant

	// COMPUTE THE CONSTANT c:
	// -------------------------
	c = ( (R*R*Omega*Omega)
			+ (r*r)*( (Omega+dphi)*(Omega+dphi) + dtheta*dtheta )
			+ (2.0*r*R*Omega)*( (Omega+dphi)*sin(theta)*cos(phi)
					+ dtheta*cos(theta)*sin(phi) ) ) * ( A*rho/( 2.0*m ) );

	f << dot( phi ) == dphi;
	f << dot( theta ) == dtheta;

	f << dot( dphi ) == ( 2.0*r*(Omega+dphi)*dtheta*cos(theta)
			+ (R*Omega*Omega)*sin(phi)
			+ c*(CD*(1.0+u2)+CL*(1.0+0.5*u2)*phi) ) / (-r*sin(theta));

	f << dot( dtheta ) == ( (R*Omega*Omega)*cos(theta)*cos(phi)
			+ r*(Omega+dphi)*sin(theta)*cos(theta)
			+ g*sin(theta) - c*( CL*u1 + b*dtheta ) ) / r;

	// Reference functions and weighting matrices:
	Function h, hN;
	h << phi << theta << dphi << dtheta << u1 << u2;
	hN << phi << theta << dphi << dtheta;

	Matrix W = eye( h.getDim() );
	Matrix WN = eye( hN.getDim() );

	W(0,0) = 5.000;
	W(1,1) = 1.000;
	W(2,2) = 10.000;
	W(3,3) = 10.000;
	W(4,4) = 0.1;
	W(5,5) = 0.1;

	WN(0,0) = 1.0584373059248833e+01;
	WN(0,1) = 1.2682415889087276e-01;
	WN(0,2) = 1.2922232012424681e+00;
	WN(0,3) = 3.7982078044271374e-02;
	WN(1,0) = 1.2682415889087265e-01;
	WN(1,1) = 3.2598407653299275e+00;
	WN(1,2) = -1.1779732282636639e-01;
	WN(1,3) = 9.8830655348904548e-02;
	WN(2,0) = 1.2922232012424684e+00;
	WN(2,1) = -1.1779732282636640e-01;
	WN(2,2) = 4.3662024133354898e+00;
	WN(2,3) = -5.9269992411260908e-02;
	WN(3,0) = 3.7982078044271367e-02;
	WN(3,1) = 9.8830655348904534e-02;
	WN(3,2) = -5.9269992411260901e-02;
	WN(3,3) = 3.6495564367004318e-01;

	//
	// Optimal Control Problem
	//

//	OCP ocp( 0.0, 12.0, 15 );
	OCP ocp( 0.0, 2.0 * M_PI, 10 );

	ocp.subjectTo( f );

	ocp.minimizeLSQ(W, h);
	ocp.minimizeLSQEndTerm(WN, hN);

	ocp.subjectTo( 18.0 <= u1 <= 22.0 );
	ocp.subjectTo( -0.2 <= u2 <= 0.2 );

	// Export the code:
	OCPexport mpc(ocp);

	mpc.set( INTEGRATOR_TYPE , INT_RK4 );
	mpc.set( NUM_INTEGRATOR_STEPS , 30 );
	mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
	mpc.set( HOTSTART_QP, YES );
	mpc.set( GENERATE_TEST_FILE, NO );

	if (mpc.exportCode("kite_carousel_export") != SUCCESSFUL_RETURN)
		exit( EXIT_FAILURE );

	mpc.printDimensionsQP( );

// 	DifferentialState Gx(4,4), Gu(4,2);
// 
// 	Expression rhs;
// 	f.getExpression( rhs );
// 
// 	Function Df;
// 	Df << rhs;
// 	Df << forwardDerivative( rhs, x ) * Gx;
// 	Df << forwardDerivative( rhs, x ) * Gu + forwardDerivative( rhs, u );
// 
// 
// 	EvaluationPoint z(Df);
// 	
// 	Vector xx(28);
// 	xx.setZero();
// 
//     xx(0) = -4.2155955213988627e-02;
// 	xx(1) =  1.8015724412870739e+00;
// 	xx(2) =  0.0;
// 	xx(3) =  0.0;
// 
// 	Vector uu(2);
//     uu(0) = 20.5;
// 	uu(1) = -0.1;
// 	
// 	z.setX( xx );
//     z.setU( uu );
// 
//     Vector result = Df.evaluate( z );
// 	result.print( "x" );

	return EXIT_SUCCESS;
}
コード例 #16
0
/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // ------------------------------------
    DifferentialState                v,s,m;
    Control                          u    ;

    const double t_start =    0.0;
    const double t_end   =   10.0;
    const double h       =   0.01;

    DiscretizedDifferentialEquation  f(h) ;


    // DEFINE A DISCRETE-TIME SYTSEM:
    // -------------------------------
    f << next(s) == s + h*v;
    f << next(v) == v + h*(u-0.02*v*v)/m;
    f << next(m) == m - h*0.01*u*u;

	
	Function eta;
	eta << u;

    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 50 );

    //ocp.minimizeLagrangeTerm( u*u );
	ocp.minimizeLSQ( eta );
    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, s ==  0.0 );
    ocp.subjectTo( AT_START, v ==  0.0 );
    ocp.subjectTo( AT_START, m ==  1.0 );

    ocp.subjectTo( AT_END  , s == 10.0 );
    ocp.subjectTo( AT_END  , v ==  0.0 );

    ocp.subjectTo( -0.01 <= v <= 1.3 );


    // DEFINE A PLOT WINDOW:
    // ---------------------
    GnuplotWindow window;
        window.addSubplot( s,"DifferentialState s" );
        window.addSubplot( v,"DifferentialState v" );
        window.addSubplot( m,"DifferentialState m" );
        window.addSubplot( u,"Control u" );
        window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );
        window.addSubplot( 0.5 * m * v*v,"Kinetic Energy" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);
	algorithm.set( INTEGRATOR_TYPE, INT_DISCRETE );
    algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
    algorithm.set( KKT_TOLERANCE, 1e-10 );

    algorithm << window;
    algorithm.solve();

    return 0;
}
コード例 #17
0
ファイル: plotting.cpp プロジェクト: drewm1980/acado
int main( ){

    USING_NAMESPACE_ACADO


	// DEFINE SAMPLE OCP
	// -----------------
	DifferentialState x, y;
	Control           u;

	DifferentialEquation f;
	
	f << dot(x) == y;
    f << dot(y) == u;

    OCP ocp( 0.0, 5.0 );
    ocp.minimizeLagrangeTerm( x*x + 0.01*u*u );

    ocp.subjectTo( f );
    ocp.subjectTo( AT_START, x ==  1.0 );
    ocp.subjectTo( AT_START, y ==  0.0 );

	ocp.subjectTo( -1.0 <= u <=  1.0 );


	// SETUP OPTIMIZATION ALGORITHM AND DEFINE VARIOUS PLOT WINDOWS
	// ------------------------------------------------------------
	OptimizationAlgorithm algorithm( ocp );

	GnuplotWindow window1;
	window1.addSubplot( x, "1st state" );
	window1.addSubplot( y, "2nd state","x label","y label",PM_POINTS );
	window1.addSubplot( u, "control input","x label","y label",PM_LINES,0,5,-2,2 );
	window1.addSubplot( 2.0*sin(x)+y*u, "Why not plotting fancy stuff!?" );
// 	window1.addSubplot( PLOT_KKT_TOLERANCE, "","iteration","KKT tolerance" );
	
	GnuplotWindow window2( PLOT_AT_EACH_ITERATION );
	window2.addSubplot( x, "1st state evolving..." );
	
	algorithm << window1;
	algorithm << window2;
	
	algorithm.solve( );


	// For illustration, an alternative way to plot differential states
	// in form of a VariablesGrid
	VariablesGrid states;
	algorithm.getDifferentialStates( states );
	
	GnuplotWindow window3;
	window3.addSubplot( states(0), "1st state obtained as VariablesGrid" );
	window3.addSubplot( states(1), "2nd state obtained as VariablesGrid" );
	window3.plot();

	VariablesGrid data( 1, 0.0,10.0,11 );
	data.setZero();
	data( 2,0 ) = 1.0;
	data( 5,0 ) = -1.0;
	data( 8,0 ) = 2.0;
	data( 9,0 ) = 2.0;
	
// 	data.setType( VT_CONTROL );
	
	GnuplotWindow window4;
	window4.addSubplot( data, "Any data can be plotted" );
	window4.plot();


    return 0;
}
コード例 #18
0
ファイル: cstr22.cpp プロジェクト: drewm1980/acado
int main( ){

    USING_NAMESPACE_ACADO
	
    // Define a Right-Hand-Side:
    // -------------------------

    DifferentialState cA, cB, theta, thetaK;
	Control u(2);

    DifferentialEquation f;

    IntermediateState k1, k2, k3;

	k1 = k10*exp(E1/(273.15 +theta));
	k2 = k20*exp(E2/(273.15 +theta));
	k3 = k30*exp(E3/(273.15 +theta));

	f << dot(cA) == (1/TIMEUNITS_PER_HOUR)*(u(0)*(cA0-cA) - k1*cA - k3*cA*cA); 
	f << dot(cB) == (1/TIMEUNITS_PER_HOUR)* (- u(0)*cB + k1*cA - k2*cB); 
	f << dot(theta) == (1/TIMEUNITS_PER_HOUR)*(u(0)*(theta0-theta) - (1/(rho*Cp)) *(k1*cA*H1 + k2*cB*H2 + k3*cA*cA*H3)+(kw*AR/(rho*Cp*VR))*(thetaK -theta)); 
	f << dot(thetaK) == (1/TIMEUNITS_PER_HOUR)*((1/(mK*CPK))*(u(1) + kw*AR*(theta-thetaK)));


    // DEFINE THE WEIGHTING MATRICES:
    // ----------------------------------------------------------
       Matrix Q  = eye(4);
//       Matrix S  = eye(4);
       Matrix R  = eye(2);
    // ----------------------------------------------------------

	Q(0,0) = sqrt(0.2);
	Q(1,1) = sqrt(1.0);
	Q(2,2) = sqrt(0.5);
	Q(3,3) = sqrt(0.2);

	R(0,0) = 0.5000;
	R(1,1) = 0.0000005;



    // SET UP THE MPC - OPTIMAL CONTROL PROBLEM:
    // ----------------------------------------------------------
       OCP ocp( 0.0, 1500.0, 22 );

       ocp.minimizeLSQ ( Q, R );
// ocp.minimizeLSQEndTerm( S    );

       ocp.subjectTo( f );
	ocp.subjectTo( 3.0     <= u(0) <= 35.0 );
	ocp.subjectTo( -9000.0 <= u(1) <= 0.0 );
	
	
    // ----------------------------------------------------------


    // DEFINE AN MPC EXPORT MODULE AND GENERATE THE CODE:
    // ----------------------------------------------------------
       MPCexport mpc(ocp);

       mpc.set( INTEGRATOR_TYPE      , INT_RK4      );
       mpc.set( NUM_INTEGRATOR_STEPS , 23           );
       mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
	   mpc.set( GENERATE_TEST_FILE,NO );

       mpc.exportCode("cstr22_export");
    // ----------------------------------------------------------


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

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState     x  , dx  ;   // the position of the mounting point and its velocity
    DifferentialState     L  , dL  ;   // the length of the cable and its velocity
    DifferentialState     phi, dphi;   // the angle phi and its velocity
    DifferentialState     P0,P1,P2 ;   // the variance-covariance states
    Control               ddx, ddL ;   // the accelarations
    Parameter                    T ;   // duration of the maneuver
    Parameter                gamma ;   // the confidence level

    double const           g = 9.81;   // the gravitational constant
    double const           m = 10.0;   // the mass at the end of the crane
    double const           b = 0.1 ;   // a frictional constant

    DifferentialEquation   f(0.0,T);

    const double F2 = 50.0;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    f << dot(x   )  == dx   + 0.000001*gamma;	// small regularization term
    f << dot(dx  )  == ddx ;
    f << dot(L   )  == dL  ;
    f << dot(dL  )  == ddL ;
    f << dot(phi )  == dphi;
    f << dot(dphi)  == -(g/L)*phi - ( b + 2.0*dL/L )*dphi - ddx/L;

    f << dot(P0)    == 2.0*P1;
    f << dot(P1)    == -(g/L)*P0 - ( b + 2.0*dL/L )*P1 + P2;
    f << dot(P2)    == -2.0*(g/L)*P1 - 2.0*( b + 2.0*dL/L )*P2 + F2/(m*m*L*L);


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp(  0.0, T, 20 );
    ocp.minimizeMayerTerm( 0, T       );
    ocp.minimizeMayerTerm( 1, -gamma  );

    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, x    ==    0.0 );
    ocp.subjectTo( AT_START, dx   ==    0.0 );
    ocp.subjectTo( AT_START, L    ==   70.0 );
    ocp.subjectTo( AT_START, dL   ==    0.0 );
    ocp.subjectTo( AT_START, phi  ==    0.0 );
    ocp.subjectTo( AT_START, dphi ==    0.0 );

    ocp.subjectTo( AT_START, P0   ==    0.0 );
    ocp.subjectTo( AT_START, P1   ==    0.0 );
    ocp.subjectTo( AT_START, P2   ==    0.0 );

    ocp.subjectTo( AT_END  , x    ==   10.0 );
    ocp.subjectTo( AT_END  , dx   ==    0.0 );
    ocp.subjectTo( AT_END  , L    ==   70.0 );
    ocp.subjectTo( AT_END  , dL   ==    0.0 );

    ocp.subjectTo( AT_END  , -0.075 <= phi - gamma*sqrt(P0)           );
    ocp.subjectTo( AT_END  ,           phi + gamma*sqrt(P0) <= 0.075  );

    ocp.subjectTo( gamma >= 0.0 );

    ocp.subjectTo( 5.0 <=  T  <=  17.0 );

    ocp.subjectTo( -0.3 <= ddx <= 0.3 );
    ocp.subjectTo( -1.0 <= ddL <= 1.0 );

    ocp.subjectTo( -10.0 <= x   <=  50.0 );
    ocp.subjectTo( -20.0 <= dx  <=  20.0 );
    ocp.subjectTo(  30.0 <= L   <=  75.0 );
    ocp.subjectTo( -20.0 <= dL  <=  20.0 );


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

    algorithm.set( PARETO_FRONT_DISCRETIZATION, 31 );
    algorithm.set( PARETO_FRONT_GENERATION, PFG_NORMALIZED_NORMAL_CONSTRAINT );
    //algorithm.set( DISCRETIZATION_TYPE        , SINGLE_SHOOTING                  );

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

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

    // Generate Pareto set
    //algorithm.set( PARETO_FRONT_HOTSTART      , BT_FALSE         		   );
    algorithm.solve();

    algorithm.getWeights("crane_nnc_weights.txt");
    algorithm.getAllDifferentialStates("crane_nnc_states.txt");
    algorithm.getAllControls("crane_nnc_controls.txt");
    algorithm.getAllParameters("crane_nnc_parameters.txt");


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

    GnuplotWindow window1;
    window1.addSubplot( paretoFront, "Pareto Front (robustness versus time)", "TIME","ROBUSTNESS", PM_POINTS );
    window1.plot( );


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


    // SAVE INFORMATION:
    // -----------------
    paretoFront.print( "crane_nnc_pareto.txt" );
コード例 #20
0
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE FIXED PARAMETERS:
    // ---------------------------
    #define  v		0.1
    #define  L		1.0
    #define  Beta	0.2
    #define  Delta	0.25
    #define  E		11250.0
    #define  k0		1E+06
    #define  R		1.986
    #define  K1		250000.0
    #define  Cin	0.02
    #define  Tin	340.0


    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState     x1,x2;
    Control               u    ;
    DifferentialEquation  f( 0.0, L );


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    double Alpha, Gamma;
    Alpha = k0*exp(-E/(R*Tin));
    Gamma = E/(R*Tin);

    f << dot(x1) ==  Alpha       /v * (1.0-x1) * exp((Gamma*x2)/(1.0+x2));
    f << dot(x2) == (Alpha*Delta)/v * (1.0-x1) * exp((Gamma*x2)/(1.0+x2)) + Beta/v * (u-x2);


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0.0, L, 50 );
    ocp.minimizeMayerTerm( 0, Cin*(1.0-x1)            ); // Solve conversion optimal problem
    ocp.minimizeMayerTerm( 1, (pow((Tin*x2),2.0)/K1) + 0.005*Cin*(1.0-x1) ); // Solve energy optimal problem (perturbed by small conversion cost; 
									     // otherwise the problem is ill-defined.)

    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, x1 ==  0.0 );
    ocp.subjectTo( AT_START, x2 ==  0.0 );

    ocp.subjectTo(  0.0            <= x1 <=  1.0             );
    ocp.subjectTo( (280.0-Tin)/Tin <= x2 <= (400.0-Tin)/Tin  );
    ocp.subjectTo( (280.0-Tin)/Tin <= u  <= (400.0-Tin)/Tin  );


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

    algorithm.set( INTEGRATOR_TYPE, INT_BDF );
    algorithm.set( KKT_TOLERANCE, 1e-8 );

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

    // Generate Pareto set 
    algorithm.solve();

    algorithm.getWeights("plug_flow_reactor_ws_weights.txt");
    algorithm.getAllDifferentialStates("plug_flow_reactor_ws_states.txt");
    algorithm.getAllControls("plug_flow_reactor_ws_controls.txt");


    // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
    // ------------------------------------------
    VariablesGrid paretoFront;
    algorithm.getParetoFront( paretoFront );

    GnuplotWindow window1;
    window1.addSubplot( paretoFront, "Pareto Front (conversion versus energy)", "OUTLET CONCENTRATION", "ENERGY",PM_POINTS );
    window1.plot( );


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


    // SAVE INFORMATION:
    // -----------------
    paretoFront.print();

    return 0;
}
コード例 #21
0
ファイル: pendulum_dae_nmpc.cpp プロジェクト: OspreyX/acado
USING_NAMESPACE_ACADO

int main(int argc, char * const argv[ ])
{
	//
	// Variables
	//

	DifferentialState	x, y, w, dx, dy, dw;
	AlgebraicState		mu;
	Control				F;

	IntermediateState	c, dc;

	const double		m = 1.0;
	const double		mc = 1.0;
	const double		L = 1.0;
	const double		g = 9.81;
	const double		p = 5.0;

	c	= 0.5 * ((x - w) * (x - w) + y * y - L * L);
	dc	= dy * y + (dw - dx) * (w - x);

	//
	// Differential algebraic equation
	//

	DifferentialEquation f;

	f << 0 == dot( x ) - dx;
	f << 0 == dot( y ) - dy;
	f << 0 == dot( w ) - dw;

	f << 0 == m * dot( dx ) + (x - w) * mu;
	f << 0 == m * dot( dy ) + y * mu + m * g;
	f << 0 == mc * dot( dw ) + (w - x) * mu  - F;

	f << 0 == (x - w) * dot( dx ) + y * dot( dy ) + (w - x) * dot( dw )
					- (-p * p * c - 2 * p * dc - dy * dy - (dw - dx) * (dw - dx));

	//
	// Weighting matrices and reference functions
	//

	Function rf;
	Function rfN;

	rf << x << y << w << dx << dy << dw << F;
	rfN << x << y << w << dx << dy << dw;

	DMatrix W = eye<double>( rf.getDim() );
	DMatrix WN = eye<double>( rfN.getDim() ) * 10;

	//
	// Optimal Control Problem
	//

	const int N  = 10;
	const int Ni = 4;
	const double Ts = 0.1;

	OCP ocp(0, N * Ts, N);

	ocp.subjectTo( f );

	ocp.minimizeLSQ(W, rf);
	ocp.minimizeLSQEndTerm(WN, rfN);

	ocp.subjectTo(-20 <= F <= 20);
//	ocp.subjectTo( -5 <= x <= 5 );

	//
	// Export the code:
	//
	OCPexport mpc( ocp );

	mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON);
	mpc.set(DISCRETIZATION_TYPE, MULTIPLE_SHOOTING);

	mpc.set(INTEGRATOR_TYPE, INT_IRK_RIIA3);
	mpc.set(NUM_INTEGRATOR_STEPS, N * Ni);

	mpc.set(SPARSE_QP_SOLUTION, FULL_CONDENSING);
//	mpc.set(SPARSE_QP_SOLUTION, CONDENSING);
	mpc.set(QP_SOLVER, QP_QPOASES);
//	mpc.set(MAX_NUM_QP_ITERATIONS, 20);
	mpc.set(HOTSTART_QP, YES);

//	mpc.set(SPARSE_QP_SOLUTION, SPARSE_SOLVER);
//	mpc.set(QP_SOLVER, QP_FORCES);

//	mpc.set(LEVENBERG_MARQUARDT, 1.0e-10);

	mpc.set(GENERATE_TEST_FILE, NO);
	mpc.set(GENERATE_MAKE_FILE, NO);
	mpc.set(GENERATE_MATLAB_INTERFACE, YES);

//	mpc.set(USE_SINGLE_PRECISION, YES);
//	mpc.set(CG_USE_OPENMP, YES);

	if (mpc.exportCode( "pendulum_dae_nmpc_export" ) != SUCCESSFUL_RETURN)
		exit( EXIT_FAILURE );

	mpc.printDimensionsQP( );

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

    USING_NAMESPACE_ACADO;


    // INTRODUCE THE VARIABLES:
    // -------------------------

    DifferentialState         x1,x2;
    Control                   u;
    Parameter                 p;
    Parameter                 T;
    DifferentialEquation      f(0.0,T);

    const double t_start =  0.0;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------

    f << dot(x1) ==  (1.0-x2*x2)*x1 - x2 + p*u;
    f << dot(x2) ==  x1;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, T, 27 );


 //   ocp.minimizeMayerTerm( T );
    ocp.minimizeLagrangeTerm(10*x1*x1 + 10*x2*x2 + u*u);

    ocp.subjectTo( f );
    ocp.subjectTo( AT_START, x1 ==  0.0 );
    ocp.subjectTo( AT_START, x2 ==  1.0 );

    ocp.subjectTo( AT_END  , x1 ==  0.0 );
    ocp.subjectTo( AT_END  , x2 ==  0.0 );

    ocp.subjectTo( -0.5 <= u <= 1.0 );

    ocp.subjectTo( p == 1.0 );
    ocp.subjectTo( 0.0 <= T <= 20.0 );


    // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
    // ------------------------------------------
    GnuplotWindow window;
        window << x1;
        window << x2;
        window << u;
        window << T;

    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);

    algorithm.initializeControls("van_der_pol_controls.txt");

    algorithm << window;
    algorithm.solve();

    algorithm.getControls("van_der_pol_controls2.txt");

    return 0;
}
コード例 #23
0
ファイル: wave_energy.cpp プロジェクト: drewm1980/acado
int main( ){

  USING_NAMESPACE_ACADO;

  // Parameters
  double h_hw = 10;    // water level
  double A_hw = 1.0;   // amplitude of the waves
  double T_hw = 5.0;   // duration of a wave
  double h_b  = 3.0;   // height of the buoy
  double rho  = 1000;  // density of water
  double A    = 1.0;   // bottom area of the buoy
  double m    = 100;   // mass of the buoy
  double g    = 9.81;  // gravitational constant

  // Free parameter
  Control u;

  // Variables
  DifferentialState h; // Position of the buoy
  DifferentialState v; // Velocity of the buoy
  DifferentialState w; // Produced wave energy
  TIME t;

  // Differential equation
  DifferentialEquation f;
  
  // Height of the wave
  IntermediateState hw;
  hw = h_hw + A_hw*sin(2*M_PI*t/T_hw);
  
  f << dot(h) ==  v;
  f << dot(v) ==  rho*A*(hw-h)/m - g - u;
  f << dot(w) ==  u*v;

  // DEFINE AN OPTIMAL CONTROL PROBLEM:
  // ----------------------------------

  const double t_start    =  0.0        ;
  const double t_end      =  15;

  OCP ocp( t_start, t_end, 100 );
  ocp.maximizeMayerTerm( w );
  ocp.subjectTo( f );
  
//   double x_start[3];
//   x_start[0] = h_hw - 0*A_hw;
//   x_start[1] = 0;
//   x_start[2] = 0;

  ocp.subjectTo( AT_START, h - (h_hw-A_hw) ==  0.0 );
  ocp.subjectTo( AT_START, v ==  0.0 );
  ocp.subjectTo( AT_START, w ==  0.0 );
  
  ocp.subjectTo( -h_b <= h-hw <= 0.0 );
  ocp.subjectTo( 0.0 <= u <= 100.0 );
  
  

      // DEFINE A PLOT WINDOW:
    // ---------------------
  GnuplotWindow window;
  window.addSubplot( h,"Height of buoy" );
  window.addSubplot( v,"Velocity of buoy" );
  window.addSubplot( w,"Objective function " );
  window.addSubplot( u,"Resistance" );
  window.addSubplot( hw,"Wave height" );
  //  window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );

  // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
  // ---------------------------------------------------
  OptimizationAlgorithm algorithm(ocp);
  algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
  algorithm.set( MAX_NUM_ITERATIONS, 100 );
  //  algorithm.set( KKT_TOLERANCE, 1e-10 );
  
  algorithm << window;
  algorithm.solve();

  return 0;
}
int main(int argc, char* argv[])
{
    if(argc < 2)
    {
        std::cerr << "You should provide a system as argument, either gazebo or flat_fish." << std::endl;
        return 1;
    }

    //========================================================================================
    //                                  SETTING VARIABLES
    //========================================================================================

    std::string system = argv[1];

    double weight;
    double buoyancy;
    std::string dataFolder;

    if(system == "gazebo")
    {
        weight = 4600;
        buoyancy = 4618;
        dataFolder = "./config/gazebo/";
    }
    else if(system == "flatfish" || system == "flat_fish")
    {
        weight = 2800;
        buoyancy = 2812;
        dataFolder = "./config/flatfish/";
    }
    else
    {
        std::cerr << "Unknown system " << system << "." << std::endl;
        return 1;
    }

    double uncertainty = 0;

     /* true  - Export the code to the specified folder
      * false - Simulates the controller inside Acado's environment */
    std::string rockFolder = "/home/rafaelsaback/rock/1_dev/control/uwv_model_pred_control/src";

    /***********************
     * CONTROLLER SETTINGS
     ***********************/

    // Prediction and control horizon
    double horizon         = 2;
    double sampleTime      = 0.1;
    int    iteractions     = horizon / sampleTime;
    bool   positionControl = false;

    //========================================================================================
    //                                  DEFINING VARIABLES
    //========================================================================================

    DifferentialEquation f;                     // Variable that will carry 12 ODEs (6 for position and 6 for velocity)

    DifferentialState v("Velocity", 6, 1);      // Velocity States
    DifferentialState n("Pose", 6, 1);          // Pose States
    DifferentialState tau("Efforts", 6, 1);     // Effort
    Control tauDot("Efforts rate", 6, 1);       // Effort rate of change

    DVector rg(3);                              // Center of gravity
    DVector rb(3);                              // Center of buoyancy

    DMatrix M(6, 6);                            // Inertia matrix
    DMatrix Minv(6, 6);                         // Inverse of inertia matrix
    DMatrix Dl(6, 6);                           // Linear damping matrix
    DMatrix Dq(6, 6);                           // Quadratic damping matrix

    // H-representation of the output domain's polyhedron
    DMatrix AHRep(12,6);
    DMatrix BHRep(12,1);

    Expression linearDamping(6, 6);             // Dl*v
    Expression quadraticDamping(6, 6);          // Dq*|v|*v
    Expression gravityBuoyancy(6);              // g(n)
    Expression absV(6, 6);                      // |v|
    Expression Jv(6);                           // J(n)*v
    Expression vDot(6);                         // AUV Fossen's equation

    Function J;                                 // Cost function
    Function Jn;                                // Terminal cost function

    AHRep = readVariable("./config/", "a_h_rep.dat");
    BHRep = readVariable("./config/", "b_h_rep.dat");
    M = readVariable(dataFolder, "m_matrix.dat");
    Dl = readVariable(dataFolder, "dl_matrix.dat");
    Dq = readVariable(dataFolder, "dq_matrix.dat");

    /***********************
     * LEAST-SQUARES PROBLEM
     ***********************/

    // Cost Functions
    if (positionControl)
    {
        J  << n;
        Jn << n;
    }
    else
    {
        J  << v;
        Jn << v;
    }

    J << tauDot;

    // Weighting Matrices for simulational controller
    DMatrix Q  = eye<double>(J.getDim());
    DMatrix QN = eye<double>(Jn.getDim());

    Q = readVariable("./config/", "q_matrix.dat");

    //========================================================================================
    //                                  MOTION MODEL EQUATIONS
    //========================================================================================
    rg(0) = 0;  rg(1) = 0;  rg(2) = 0;
    rb(0) = 0;  rb(1) = 0;  rb(2) = 0;

    M  *= (2 -(1 + uncertainty));
    Dl *= 1 + uncertainty;
    Dq *= 1 + uncertainty;

    SetAbsV(absV, v);

    Minv = M.inverse();
    linearDamping = Dl * v;
    quadraticDamping = Dq * absV * v;
    SetGravityBuoyancy(gravityBuoyancy, n, rg, rb, weight, buoyancy);
    SetJv(Jv, v, n);

    // Dynamic Equation
    vDot = Minv * (tau - linearDamping - quadraticDamping - gravityBuoyancy);

    // Differential Equations
    f << dot(v) == vDot;
    f << dot(n) == Jv;
    f << dot(tau) == tauDot;

    //========================================================================================
    //                              OPTIMAL CONTROL PROBLEM (OCP)
    //========================================================================================

    OCP ocp(0, horizon, iteractions);

    // Weighting Matrices for exported controller
    BMatrix Qexp = eye<bool>(J.getDim());
    BMatrix QNexp = eye<bool>(Jn.getDim());

    ocp.minimizeLSQ(Qexp, J);
    ocp.minimizeLSQEndTerm(QNexp, Jn);
    ocp.subjectTo(f);

    // Individual degrees of freedom constraints
    ocp.subjectTo(tau(3) == 0);
    ocp.subjectTo(tau(4) == 0);

    /* Note: If the constraint for Roll is not defined the MPC does not
     * work since there's no possible actuation in that DOF.
     * Always consider Roll equal to zero. */

    // Polyhedron set of inequalities
    ocp.subjectTo(AHRep*tau - BHRep <= 0);


    //========================================================================================
    //                                  CODE GENERATION
    //========================================================================================

    // Export code to ROCK
    OCPexport mpc(ocp);

    int Ni = 1;

    mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON);
    mpc.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING);
    mpc.set(INTEGRATOR_TYPE, INT_RK4);
    mpc.set(NUM_INTEGRATOR_STEPS, iteractions * Ni);
    mpc.set(HOTSTART_QP, YES);
    mpc.set(QP_SOLVER, QP_QPOASES);
    mpc.set(GENERATE_TEST_FILE, NO);
    mpc.set(GENERATE_MAKE_FILE, NO);
    mpc.set(GENERATE_MATLAB_INTERFACE, NO);
    mpc.set(GENERATE_SIMULINK_INTERFACE, NO);
    mpc.set(CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
    mpc.set(CG_HARDCODE_CONSTRAINT_VALUES, YES);

    if (mpc.exportCode(rockFolder.c_str()) != SUCCESSFUL_RETURN)
        exit( EXIT_FAILURE);

    mpc.printDimensionsQP();

    return EXIT_SUCCESS;
}
コード例 #25
0
ファイル: powerkite_c.cpp プロジェクト: borishouska/acado
int main( ){


	counter = 0;

	// DIFFERENTIAL STATES :
	// -------------------------
	   DifferentialState      r;      //  the length r of the cable
	   DifferentialState    phi;      //  the angle phi
	   DifferentialState  theta;      //  the angle theta
	// -------------------------      //  -------------------------------------------
	   DifferentialState     dr;      //  first  derivative of r0    with respect to t
	   DifferentialState   dphi;      //  first  derivative of phi   with respect to t
	   DifferentialState dtheta;      //  first  derivative of theta with respect to t
	// -------------------------      //  -------------------------------------------
	   DifferentialState      n;      //  winding number
	// -------------------------      //  -------------------------------------------
	   DifferentialState    Psi;      //  the roll angle Psi
	   DifferentialState     CL;      //  the aerodynamic lift coefficient
	// -------------------------      //  -------------------------------------------
	   DifferentialState      W;      //  integral over the power at the generator
	                                  //  ( = ENERGY )


	// CONTROL :
	// -------------------------
	   Control             ddr0;      //  second derivative of r0    with respect to t
	   Control             dPsi;      //  first  derivative of Psi   with respect to t
	   Control              dCL;      //  first  derivative of CL    with respect to t



	// THE "RIGHT-HAND-SIDE" OF THE ODE:
	// ---------------------------------------------------------------
	   DifferentialEquation f;

       const int NX = 10;
       const int NU = 3 ;

       IntermediateState x( NX + NU );

       x(0) = r;
       x(1) = phi;
       x(2) = theta;
       x(3) = dr;
       x(4) = dphi;
       x(5) = dtheta;
       x(6) = n;
       x(7) = Psi;
       x(8) = CL;
       x(9) = W;

       x(10) = ddr0;
       x(11) = dPsi;
       x(12) = dCL;

       CFunction kiteModel( 10, ffcn_model );

       f << kiteModel( x );


	    // DEFINE AN OPTIMAL CONTROL PROBLEM:
	    // ----------------------------------
	    OCP ocp( 0.0, 18.0, 18 );
	    ocp.minimizeMayerTerm( W );
	    ocp.subjectTo( f );


	    // INITIAL VALUE CONSTRAINTS:
	    // ---------------------------------
	    ocp.subjectTo( AT_START, n == 0.0 );
	    ocp.subjectTo( AT_START, W == 0.0 );


	    // PERIODIC BOUNDARY CONSTRAINTS:
	    // ----------------------------------------
	    ocp.subjectTo( 0.0, r     , -r     , 0.0 );
	    ocp.subjectTo( 0.0, phi   , -phi   , 0.0 );
	    ocp.subjectTo( 0.0, theta , -theta , 0.0 );
	    ocp.subjectTo( 0.0, dr    , -dr    , 0.0 );
	    ocp.subjectTo( 0.0, dphi  , -dphi  , 0.0 );
	    ocp.subjectTo( 0.0, dtheta, -dtheta, 0.0 );
	    ocp.subjectTo( 0.0, Psi   , -Psi   , 0.0 );
	    ocp.subjectTo( 0.0, CL    , -CL    , 0.0 );

	    ocp.subjectTo( -0.34   <= phi   <= 0.34   );
	    ocp.subjectTo(  0.85   <= theta <= 1.45   );
	    ocp.subjectTo( -40.0   <= dr    <= 10.0   );
	    ocp.subjectTo( -0.29   <= Psi   <= 0.29   );
	    ocp.subjectTo(  0.1    <= CL    <= 1.50   );
	    ocp.subjectTo( -0.7    <= n     <= 0.90   );
	    ocp.subjectTo( -25.0   <= ddr0  <= 25.0   );
	    ocp.subjectTo( -0.065  <= dPsi  <= 0.065  );
	    ocp.subjectTo( -3.5    <= dCL   <= 3.5    );


	    // CREATE A PLOT WINDOW AND VISUALIZE THE RESULT:
	    // ----------------------------------------------

	    GnuplotWindow window;
	    window.addSubplot( r,    "CABLE LENGTH  r [m]" );
	    window.addSubplot( phi,  "POSITION ANGLE  phi [rad]" );
	    window.addSubplot( theta,"POSITION ANGLE theta [rad]" );
	    window.addSubplot( Psi,  "ROLL ANGLE  psi [rad]" );
	    window.addSubplot( CL,   "LIFT COEFFICIENT  CL" );
	    window.addSubplot( W,    "ENERGY  W [MJ]" );
		window.addSubplot( phi,theta, "Kite Orbit","theta [rad]","phi [rad]" );

	    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
	    // ---------------------------------------------------

	    OptimizationAlgorithm algorithm(ocp);

	    algorithm.initializeDifferentialStates("powerkite_states.txt"    );
	    algorithm.initializeControls          ("powerkite_controls.txt"  );
	    algorithm.set                         ( MAX_NUM_ITERATIONS, 100  );
	    algorithm.set                         ( KKT_TOLERANCE    , 1e-2 );

		algorithm << window;

        algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );

	    algorithm.solve();



    return 0;
}
コード例 #26
0
ファイル: crane_mpc3_stepped.cpp プロジェクト: OspreyX/acado
int main( ){

    USING_NAMESPACE_ACADO;

    // VARIABLES:
    // ------------------------
    DifferentialState        x;   // Position of the trolley
    DifferentialState        v;   // Velocity of the trolley
    DifferentialState      phi;   // excitation angle
    DifferentialState     dphi;   // rotational velocity

	Control 				ax;   // trolley accelaration
	Disturbance 			 W;   // disturbance

    double L = 1.0 ;              // length
	double m = 1.0 ;              // mass
	double g = 9.81;              // gravitational constant
	double b = 0.2 ;              // friction coefficient


    // DIFFERENTIAL EQUATION:
    // ------------------------
    DifferentialEquation     f, fSim;   // The model equations

    f << dot(x) ==  v;
    f << dot(v) ==  ax;
    f << dot(phi ) == dphi;
    f << dot(dphi) == -g/L*sin(phi) -ax/L*cos(phi) - b/(m*L*L)*dphi;

	L = 1.2;							// introduce model plant mismatch
	
	fSim << dot(x) ==  v;
	fSim << dot(v) ==  ax + W;
	fSim << dot(phi ) == dphi;
	fSim << dot(dphi) == -g/L*sin(phi) -ax/L*cos(phi) - b/(m*L*L)*dphi;
	

    // DEFINE LEAST SQUARE FUNCTION:
    // -----------------------------
    Function h;

    h << x;
    h << v;
    h << phi;
    h << dphi;

    DMatrix Q(4,4); // LSQ coefficient matrix
    Q.setIdentity();

    DVector r(4); // Reference


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    const double t_start = 0.0;
    const double t_end   = 5.0;

    OCP ocp( t_start, t_end, 25 );

    ocp.minimizeLSQ( Q, h, r );
    ocp.subjectTo( f );
    ocp.subjectTo( -5.0 <= ax <= 5.0 );


    // SETTING UP THE (SIMULATED) PROCESS:
    // -----------------------------------
	OutputFcn identity;
	DynamicSystem dynamicSystem( fSim,identity );

	Process process( dynamicSystem,INT_RK45 );

	VariablesGrid disturbance; disturbance.read( "dist.txt" );
	if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
		exit( EXIT_FAILURE );

    // SETTING UP THE MPC CONTROLLER:
    // ------------------------------
    double samplingTime = 0.1;
	RealTimeAlgorithm alg( ocp, samplingTime );
//  	alg.set( USE_REALTIME_ITERATIONS,NO );
//  	alg.set( MAX_NUM_ITERATIONS,20 );

	StaticReferenceTrajectory zeroReference;

	Controller controller( alg, zeroReference );
	
	DVector x0(4);
	x0.setZero();
	x0(3) = 1.0;

	double startTime =  0.0;
	double endTime   = 20.0;


	DVector uCon;
	VariablesGrid ySim;
	
	if (controller.init( startTime,x0 ) != SUCCESSFUL_RETURN)
		exit( EXIT_FAILURE );
	controller.getU( uCon );
	
	if (process.init( startTime,x0,uCon ) != SUCCESSFUL_RETURN)
		exit( EXIT_FAILURE );
	process.getY( ySim );


	// 	hand-coding call to 
	//	sim.run( )

	int nSteps = 0;
	double currentTime = startTime;

	while ( currentTime <= endTime )
	{
		printf( "\n*** Simulation Loop No. %d (starting at time %.3f) ***\n",nSteps,currentTime );
	
		if (controller.step( currentTime,ySim.getLastVector() ) != SUCCESSFUL_RETURN)
			exit( EXIT_FAILURE );
		controller.getU( uCon );
		
		if (process.step( currentTime,currentTime+samplingTime,uCon ) != SUCCESSFUL_RETURN)
			exit( EXIT_FAILURE );
		process.getY( ySim );
		
		++nSteps;
		currentTime = (double)nSteps * samplingTime;
	}

    return EXIT_SUCCESS;
}
コード例 #27
0
ファイル: cstr.cpp プロジェクト: ThomasBesselmann/acado
int main()
{

	USING_NAMESPACE_ACADO

	// Define a Right-Hand-Side:
	DifferentialState cA, cB, theta, thetaK;
	Control           u( 2 );

	DifferentialEquation f;

	IntermediateState k1, k2, k3;

	k1 = k10*exp(E1/(273.15 +theta));
	k2 = k20*exp(E2/(273.15 +theta));
	k3 = k30*exp(E3/(273.15 +theta));

	f << dot(cA) == (1/TIMEUNITS_PER_HOUR)*(u(0)*(cA0-cA) - k1*cA - k3*cA*cA);
	f << dot(cB) == (1/TIMEUNITS_PER_HOUR)* (- u(0)*cB + k1*cA - k2*cB);
	f << dot(theta) == (1/TIMEUNITS_PER_HOUR)*(u(0)*(theta0-theta) - (1/(rho*Cp)) *(k1*cA*H1 + k2*cB*H2 + k3*cA*cA*H3)+(kw*AR/(rho*Cp*VR))*(thetaK -theta));
	f << dot(thetaK) == (1/TIMEUNITS_PER_HOUR)*((1/(mK*CPK))*(u(1) + kw*AR*(theta-thetaK)));

	// Reference functions and weighting matrices:
	Function h, hN;
	h << cA << cB << theta << thetaK << u;
	hN << cA << cB << theta << thetaK;

	Matrix W = eye( h.getDim() );
	Matrix WN = eye( hN.getDim() );

	W(0, 0) = WN(0, 0) = 0.2;
	W(1, 1) = WN(1, 1) = 1.0;
	W(2, 2) = WN(2, 2) = 0.5;
	W(3, 3) = WN(3, 3) = 0.2;

	W(4, 4) = 0.5000;
	W(5, 5) = 0.0000005;

	//
	// Optimal Control Problem
	//
	OCP ocp(0.0, 1500.0, 10);

	ocp.subjectTo( f );

	ocp.minimizeLSQ(W, h);
	ocp.minimizeLSQEndTerm(WN, hN);

	ocp.subjectTo( 3.0 <= u(0) <= 35.0 );
	ocp.subjectTo( -9000.0 <= u(1) <= 0.0 );

	ocp.subjectTo( cA <= 2.5 );
// 	ocp.subjectTo( cB <= 1.055 );

	// Export the code:
	OCPexport mpc( ocp );

	mpc.set( INTEGRATOR_TYPE , INT_RK4 );
	mpc.set( NUM_INTEGRATOR_STEPS , 20 );
	mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
	mpc.set( QP_SOLVER, QP_QPOASES );
	mpc.set( GENERATE_TEST_FILE,NO );

//	mpc.set( USE_SINGLE_PRECISION,YES );
//	mpc.set( GENERATE_SIMULINK_INTERFACE,YES );

	if (mpc.exportCode( "cstr_export" ) != SUCCESSFUL_RETURN)
		exit( EXIT_FAILURE );

	mpc.printDimensionsQP( );

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

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // ----------------------------
    DifferentialState        x1,x2;
    Control                  u    ;
    Parameter                t1   ;

    DifferentialEquation f(0.0,t1);


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    f << dot(x1) ==  x2;
    f << dot(x2) ==   u;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp(0.0,t1,25);
    ocp.minimizeMayerTerm( 0, x2         );
    ocp.minimizeMayerTerm( 1, 2.0*t1/20.0);

    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, x1 ==   0.0 );
    ocp.subjectTo( AT_START, x2 ==   0.0 );
    ocp.subjectTo( AT_END  , x1 == 200.0 );

    ocp.subjectTo( 0.0 <= x1 <= 200.0001 );
    ocp.subjectTo( 0.0 <= x2 <=  40.0    );
    ocp.subjectTo( 0.0 <= u  <=   5.0    );
    ocp.subjectTo( 0.1 <= t1 <=  50.0    );


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

    algorithm.set( PARETO_FRONT_DISCRETIZATION, 11 );
    algorithm.set( PARETO_FRONT_GENERATION, PFG_NORMAL_BOUNDARY_INTERSECTION );
    algorithm.set( KKT_TOLERANCE, 1e-8 );

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

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

    // Generate Pareto set
    algorithm.solve();

    algorithm.getWeights("car_nbi_weights.txt");
    algorithm.getAllDifferentialStates("car_nbi_states.txt");
    algorithm.getAllControls("car_nbi_controls.txt");
    algorithm.getAllParameters("car_nbi_parameters.txt");


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

    GnuplotWindow window1;
    window1.addSubplot( paretoFront, "Pareto Front (time versus energy)", "ENERGY","TIME", PM_POINTS );
    window1.plot( );


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


    // SAVE INFORMATION:
    // -----------------
    FILE *file = fopen("car_nbi_pareto.txt","w");
    file << paretoFront;
    fclose(file);

    return 0;
}
コード例 #29
0
ファイル: time_optimal_rocket.cpp プロジェクト: rtkg/acado
int main( ){

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------

    DifferentialState     s,v,m;
    Control               u    ;
    Parameter             T    ;

    DifferentialEquation  f( 0.0, T );


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------

    f << dot(s) == v;
    f << dot(v) == (u-0.2*v*v)/m;
    f << dot(m) == -0.01*u*u;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0, T, 20 );

    ocp.minimizeMayerTerm( T );
    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, s ==  0.0 );
    ocp.subjectTo( AT_START, v ==  0.0 );
    ocp.subjectTo( AT_START, m ==  1.0 );

    ocp.subjectTo( AT_END  , s == 10.0 );
    ocp.subjectTo( AT_END  , v ==  0.0 );

    ocp.subjectTo( -0.1 <= v <=  1.7  );
    ocp.subjectTo( -1.1 <= u <=  1.1  );
    ocp.subjectTo(  5.0 <= T <= 15.0  );


    // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
    // ------------------------------------------
    GnuplotWindow window;
        window.addSubplot( s, "THE DISTANCE s"      );
        window.addSubplot( v, "THE VELOCITY v"      );
        window.addSubplot( m, "THE MASS m"          );
        window.addSubplot( u, "THE CONTROL INPUT u" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);

    algorithm.set( MAX_NUM_ITERATIONS, 20 );

    algorithm << window;


//     algorithm.initializeDifferentialStates("tor_states.txt");
//     algorithm.initializeParameters("tor_pars.txt");
//     algorithm.initializeControls("tor_controls.txt");

    algorithm.solve();

//     algorithm.getDifferentialStates("tor_states.txt");
//     algorithm.getParameters("tor_pars.txt");
//     algorithm.getControls("tor_controls.txt");

    return 0;
}
コード例 #30
0
ファイル: rocket_c.cpp プロジェクト: borishouska/acado
USING_NAMESPACE_ACADO


int main( ){


    // INTRODUCE THE VARIABLES:
    // --------------------------------------------------
    DifferentialState     s,v,m,L;
    Control               u      ;
    DifferentialEquation  f      ;


    // DEFINE THE DIMENSIONS OF THE C-FUNCTIONS:
    // --------------------------------------------------
    CFunction F( NX, myDifferentialEquation     );
    CFunction M( NJ, myObjectiveFunction        );
    CFunction I( NI, myInitialValueConstraint   );
    CFunction E( NE, myEndPointConstraint       );
    CFunction H( NH, myInequalityPathConstraint );


    // DEFINE THE OPTIMIZATION VARIABLES:
    // --------------------------------------------------

    IntermediateState x(5);

    x(0) = v; x(1) = m; x(2) = u; x(3) = L; x(4) = s;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0.0, 10.0 );

    ocp.minimizeMayerTerm( M(x) );

    ocp.subjectTo( f << F(x) );

    ocp.subjectTo( AT_START, I(x) ==  0.0 );
    ocp.subjectTo( AT_END  , E(x) ==  0.0 );
    ocp.subjectTo(           H(x) <=  1.3 );


    // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
    // ------------------------------------------
    GnuplotWindow window1;
    window1.addSubplot( s,"DifferentialState s" );
    window1.addSubplot( v,"DifferentialState v" );
    window1.addSubplot( m,"DifferentialState m" );
    window1.addSubplot( u,"Control u" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);
    algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
    algorithm.set( KKT_TOLERANCE, 1e-3 );
    algorithm << window1;
    algorithm.solve();


    return 0;
}