Exemplo n.º 1
0
int main( ){

    USING_NAMESPACE_ACADO

    TIME autotime;
    DifferentialState x(2);
    AlgebraicState z;
    Control u;
    DifferentialEquation f1;
    IntermediateState setc_is_1(5);
    setc_is_1(0) = autotime;
    setc_is_1(1) = x(0);
    setc_is_1(2) = x(1);
    setc_is_1(3) = z;
    setc_is_1(4) = u;


    CFunction cLinkModel_1( 3, myAcadoDifferentialEquation1 );
    f1 << cLinkModel_1(setc_is_1);


    double dconstant1 = 0.0;
    double dconstant2 = 5.0;
    int dconstant3 = 10;
    OCP ocp1(dconstant1, dconstant2, dconstant3);
    ocp1.minimizeMayerTerm(x(1));
    ocp1.subjectTo(f1);
    ocp1.subjectTo(AT_START, x(0) == 1.0 );
    ocp1.subjectTo(AT_START, x(1) == 0.0 );


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


    OptimizationAlgorithm algo1(ocp1);
    algo1.set( KKT_TOLERANCE, 1.000000E-05 );
    algo1.set( RELAXATION_PARAMETER, 1.500000E+00 );

 // algo1.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
 // 
    algo1 << window;

    algo1.solve();

    return 0;
}
Exemplo n.º 2
0
USING_NAMESPACE_ACADO

#include <mex.h>


void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) 
 { 
 
    MatlabConsoleStreamBuf mybuf;
    RedirectStream redirect(std::cout, mybuf);
    clearAllStaticCounters( ); 
 
    mexPrintf("\nACADO Toolkit for Matlab - Developed by David Ariens and Rien Quirynen, 2009-2013 \n"); 
    mexPrintf("Support available at http://www.acadotoolkit.org/matlab \n \n"); 

    if (nrhs != 0){ 
      mexErrMsgTxt("This problem expects 0 right hand side argument(s) since you have defined 0 MexInput(s)");
    } 
 
    TIME autotime;
    DifferentialState u;
    DifferentialState v;
    DifferentialState w;
    DifferentialState p;
    DifferentialState q;
    DifferentialState r;
    DifferentialState phi;
    DifferentialState theta;
    DifferentialState intg_V;
    DifferentialState x_w2_uT;
    DifferentialState x_w2_uE;
    DifferentialState x_w2_uA;
    DifferentialState x_w2_uR;
    DifferentialState x_w2_sv;
    Control uT;
    Control uE;
    Control uA;
    Control uR;
    Control sv;
    OnlineData Aw2; 
    OnlineData Bw2; 
    OnlineData Cw2; 
    OnlineData Dw2; 
    OnlineData kiV; 
    BMatrix acadodata_M1;
    acadodata_M1.read( "nmpc_ext_data_acadodata_M1.txt" );
    BMatrix acadodata_M2;
    acadodata_M2.read( "nmpc_ext_data_acadodata_M2.txt" );
    OCP ocp1(0, 0.5, 10);
    ocp1.minimizeLSQ(acadodata_M1, "evaluateLSQ");
    ocp1.minimizeLSQEndTerm(acadodata_M2, "evaluateLSQEndTerm");
    ocp1.subjectTo(1.000000E-01 <= uT <= 1.000000E+00);
    ocp1.subjectTo((-3.490659E-01) <= uE <= 3.490659E-01);
    ocp1.subjectTo((-3.490659E-01) <= uA <= 3.490659E-01);
    ocp1.subjectTo((-3.490659E-01) <= uR <= 3.490659E-01);
    ocp1.subjectTo((-1.047198E+00) <= phi <= 1.047198E+00);
    ocp1.subjectTo((-7.853982E-01) <= theta <= 7.853982E-01);
    ocp1.subjectTo((-3.490659E-02) <= (atan(1/u*w)+sv));
    ocp1.subjectTo((atan(1/u*w)-sv) <= 1.745329E-01);
    ocp1.subjectTo(0.000000E+00 <= sv <= 5.235988E-02);
    ocp1.setNOD( 5 );
    ocp1.setNP( 0 );
    ocp1.setNU( 5 );
    ocp1.setModel( "model", "rhs", "rhs_jac" );
    ocp1.setDimensions( 0, 14, 0, 0, 0, 5, 5, 0 );


    OCPexport ExportModule1( ocp1 );
    ExportModule1.set( GENERATE_MATLAB_INTERFACE, 1 );
    uint options_flag;
    options_flag = ExportModule1.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: HESSIAN_APPROXIMATION");
    options_flag = ExportModule1.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: DISCRETIZATION_TYPE");
    options_flag = ExportModule1.set( SPARSE_QP_SOLUTION, FULL_CONDENSING );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: SPARSE_QP_SOLUTION");
    options_flag = ExportModule1.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: INTEGRATOR_TYPE");
    options_flag = ExportModule1.set( NUM_INTEGRATOR_STEPS, 10 );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: NUM_INTEGRATOR_STEPS");
    options_flag = ExportModule1.set( QP_SOLVER, QP_QPOASES );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: QP_SOLVER");
    options_flag = ExportModule1.set( HOTSTART_QP, NO );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: HOTSTART_QP");
    options_flag = ExportModule1.set( LEVENBERG_MARQUARDT, 1.000000E-10 );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: LEVENBERG_MARQUARDT");
    options_flag = ExportModule1.set( GENERATE_MAKE_FILE, YES );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: GENERATE_MAKE_FILE");
    options_flag = ExportModule1.set( GENERATE_TEST_FILE, YES );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: GENERATE_TEST_FILE");
    options_flag = ExportModule1.set( GENERATE_SIMULINK_INTERFACE, YES );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: GENERATE_SIMULINK_INTERFACE");
    options_flag = ExportModule1.set( CG_HARDCODE_CONSTRAINT_VALUES, YES );
    if(options_flag != 0) mexErrMsgTxt("ACADO export failed when setting the following option: CG_HARDCODE_CONSTRAINT_VALUES");
    uint export_flag;
    export_flag = ExportModule1.exportCode( "export_nmpc_ext" );
    if(export_flag != 0) mexErrMsgTxt("ACADO export failed because of the above error(s)!");


    clearAllStaticCounters( ); 
 
} 
Exemplo n.º 3
0
Arquivo: test.cpp Projeto: rtkg/OC
USING_NAMESPACE_ACADO

#include <mex.h>


void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{

    MatlabConsoleStreamBuf mybuf;
    RedirectStream redirect(std::cout, mybuf);
    clearAllStaticCounters( );

    mexPrintf("\nACADO Toolkit for Matlab - Developed by David Ariens and Rien Quirynen, 2009-2013 \n");
    mexPrintf("Support available at http://www.acadotoolkit.org/matlab \n \n");

    if (nrhs != 0) {
        mexErrMsgTxt("This problem expects 0 right hand side argument(s) since you have defined 0 MexInput(s)");
    }

    TIME autotime;
    DifferentialState x;
    DifferentialState y;
    DifferentialState th;
    DifferentialState L;
    Control v;
    Control w;
    DifferentialEquation acadodata_f1;
    acadodata_f1 << dot(x) == cos(th)*v;
    acadodata_f1 << dot(y) == sin(th)*v;
    acadodata_f1 << dot(th) == w;
    acadodata_f1 << dot(L) == w*w;

    OCP ocp1(0, 10, 20);
    ocp1.minimizeMayerTerm(L);
    ocp1.subjectTo(acadodata_f1);
    ocp1.subjectTo(AT_START, x == (-1.000000E+00));
    ocp1.subjectTo(AT_START, y == (-1.000000E+00));
    ocp1.subjectTo(AT_START, th == 0.000000E+00);
    ocp1.subjectTo(AT_END, x == 0.000000E+00);
    ocp1.subjectTo(AT_END, y == 0.000000E+00);


    OptimizationAlgorithm algo1(ocp1);
    returnValue returnvalue = algo1.solve();

    VariablesGrid out_states;
    VariablesGrid out_parameters;
    VariablesGrid out_controls;
    VariablesGrid out_disturbances;
    VariablesGrid out_algstates;
    algo1.getDifferentialStates(out_states);
    algo1.getControls(out_controls);
    const char* outputFieldNames[] = {"STATES", "CONTROLS", "PARAMETERS", "DISTURBANCES", "ALGEBRAICSTATES", "CONVERGENCE_ACHIEVED"};
    plhs[0] = mxCreateStructMatrix( 1,1,6,outputFieldNames );
    mxArray *OutS = NULL;
    double  *outS = NULL;
    OutS = mxCreateDoubleMatrix( out_states.getNumPoints(),1+out_states.getNumValues(),mxREAL );
    outS = mxGetPr( OutS );
    for( int i=0; i<out_states.getNumPoints(); ++i ) {
        outS[0*out_states.getNumPoints() + i] = out_states.getTime(i);
        for( int j=0; j<out_states.getNumValues(); ++j ) {
            outS[(1+j)*out_states.getNumPoints() + i] = out_states(i, j);
        }
    }

    mxSetField( plhs[0],0,"STATES",OutS );
    mxArray *OutC = NULL;
    double  *outC = NULL;
    OutC = mxCreateDoubleMatrix( out_controls.getNumPoints(),1+out_controls.getNumValues(),mxREAL );
    outC = mxGetPr( OutC );
    for( int i=0; i<out_controls.getNumPoints(); ++i ) {
        outC[0*out_controls.getNumPoints() + i] = out_controls.getTime(i);
        for( int j=0; j<out_controls.getNumValues(); ++j ) {
            outC[(1+j)*out_controls.getNumPoints() + i] = out_controls(i, j);
        }
    }

    mxSetField( plhs[0],0,"CONTROLS",OutC );
    mxArray *OutP = NULL;
    double  *outP = NULL;
    OutP = mxCreateDoubleMatrix( out_parameters.getNumPoints(),1+out_parameters.getNumValues(),mxREAL );
    outP = mxGetPr( OutP );
    for( int i=0; i<out_parameters.getNumPoints(); ++i ) {
        outP[0*out_parameters.getNumPoints() + i] = out_parameters.getTime(i);
        for( int j=0; j<out_parameters.getNumValues(); ++j ) {
            outP[(1+j)*out_parameters.getNumPoints() + i] = out_parameters(i, j);
        }
    }

    mxSetField( plhs[0],0,"PARAMETERS",OutP );
    mxArray *OutW = NULL;
    double  *outW = NULL;
    OutW = mxCreateDoubleMatrix( out_disturbances.getNumPoints(),1+out_disturbances.getNumValues(),mxREAL );
    outW = mxGetPr( OutW );
    for( int i=0; i<out_disturbances.getNumPoints(); ++i ) {
        outW[0*out_disturbances.getNumPoints() + i] = out_disturbances.getTime(i);
        for( int j=0; j<out_disturbances.getNumValues(); ++j ) {
            outW[(1+j)*out_disturbances.getNumPoints() + i] = out_disturbances(i, j);
        }
    }

    mxSetField( plhs[0],0,"DISTURBANCES",OutW );
    mxArray *OutZ = NULL;
    double  *outZ = NULL;
    OutZ = mxCreateDoubleMatrix( out_algstates.getNumPoints(),1+out_algstates.getNumValues(),mxREAL );
    outZ = mxGetPr( OutZ );
    for( int i=0; i<out_algstates.getNumPoints(); ++i ) {
        outZ[0*out_algstates.getNumPoints() + i] = out_algstates.getTime(i);
        for( int j=0; j<out_algstates.getNumValues(); ++j ) {
            outZ[(1+j)*out_algstates.getNumPoints() + i] = out_algstates(i, j);
        }
    }

    mxSetField( plhs[0],0,"ALGEBRAICSTATES",OutZ );
    mxArray *OutConv = NULL;
    if ( returnvalue == SUCCESSFUL_RETURN ) {
        OutConv = mxCreateDoubleScalar( 1 );
    }
    else {
        OutConv = mxCreateDoubleScalar( 0 );
    }
    mxSetField( plhs[0],0,"CONVERGENCE_ACHIEVED",OutConv );


    clearAllStaticCounters( );

}