goddard.cpp

#include <acado_toolkit.hpp>
#include <include/acado_gnuplot/gnuplot_window.hpp>

int main( ){

// planet parameters
double RBody = 600000.0; // meters
double MuBody = 3.5316e12; // gravitational parameter, m^3/s^2
double RotPeriod = 21600.0; // sidereal rotation period, seconds
double AtmSLPressure = 1.0; // sea-level pressure, atm
double AtmScaleHeight = 5000.0; // meters
//double AtmCutoffAlt = AtmScaleHeight * log(1e6); // meters
double Density1atm = 1.2230948554874; // 1-atmosphere density, kg/m^3
double g0 = 9.81; // meters/second^2
// craft parameters
double DragCoefficient = 0.008 * 0.2;
double IspAtm = 220.0; // seconds
double IspVac = 800.0; // seconds
double MaxThrust = 60.0; // kilonewtons
//double MinThrust = 0; // kilonewtons
double MassDry = 3.32; // tonnes
// initial conditions
double Mass0 = 5.32; // tonnes
double Altitude0 = 77.0; // meters
double VSpeed0 = 0.0; // meters/second
double HSpeedOrb0 = 2.0 * M_PI * (Altitude0 + RBody) / RotPeriod; // meters/second
// final conditions
//double AltitudeFinal = 74000; // meters
//double VSpeedFinal = 0; // meters/second
//double HSpeedOrbFinal = sqrt(MuBody/(RBody + AltitudeFinal)); // meters/second
// simulation parameters
double HThrust = 0.0; // kilonewtons
int NumSteps = 40;
double TfMin = 10.0; // seconds
double TfMax = 500.0; // seconds

    USING_NAMESPACE_ACADO

    DifferentialState     Mass, Altitude, VSpeed, HSpeedOrb;
    Control               VThrust;
    Parameter             Tf;
    DifferentialEquation  f(0.0, Tf);
    IntermediateState     Pressure, HSpeedSurf, SpeedSurfMagnitude;

    OCP ocp( 0.0, Tf, NumSteps );
    ocp.maximizeMayerTerm( Altitude );
    
    Pressure = AtmSLPressure * exp(-Altitude / AtmScaleHeight);
    HSpeedSurf = HSpeedOrb - 2.0 * M_PI * (Altitude + RBody) / RotPeriod;
    SpeedSurfMagnitude = sqrt(VSpeed*VSpeed + HSpeedSurf*HSpeedSurf);
    
    f << dot(Mass) == -sqrt(VThrust*VThrust + HThrust*HThrust) /
            (g0 * (IspVac + (IspAtm - IspVac) * Pressure));
    f << dot(Altitude) == VSpeed;
    f << dot(VSpeed) == VThrust / Mass - MuBody / ((Altitude + RBody) *
            (Altitude + RBody)) + HSpeedOrb * HSpeedOrb /
            (Altitude + RBody) - 0.5 * DragCoefficient * Density1atm *
            Pressure * VSpeed * SpeedSurfMagnitude;
    f << dot(HSpeedOrb) == HThrust / Mass - VSpeed * HSpeedOrb /
            (Altitude + RBody) - 0.5 * DragCoefficient * Density1atm *
            Pressure * HSpeedSurf * SpeedSurfMagnitude;
    
    ocp.subjectTo( f );

	ocp.subjectTo( AT_START, Mass == Mass0 );
    ocp.subjectTo( AT_START, Altitude == Altitude0 );
    ocp.subjectTo( AT_START, VSpeed == VSpeed0 );
    ocp.subjectTo( AT_START, HSpeedOrb == HSpeedOrb0 );
    //ocp.subjectTo( AT_END  , Mass == MassDry );

    ocp.subjectTo( MassDry <= Mass <= Mass0 );
    ocp.subjectTo( Altitude0 <= Altitude );
    ocp.subjectTo( -MaxThrust <= VThrust <= MaxThrust );
    //ocp.subjectTo( 0.0 <= VThrust <= MaxThrust );
    ocp.subjectTo( TfMin <= Tf <= TfMax );
	
	//ocp.subjectTo( u*u >= -1.0 );


    // DEFINE A PLOT WINDOW:
    // ---------------------
    GnuplotWindow window;
        window.addSubplot( Mass, "Mass" );
        window.addSubplot( Altitude, "Altitude" );
        window.addSubplot( VSpeed, "VSpeed" );
        window.addSubplot( HSpeedOrb, "HSpeedOrb" );
        window.addSubplot( VThrust, "VThrust" );
        //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_BDF );

     //algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
    //algorithm.set( ABSOLUTE_TOLERANCE, 1e-6 );
     //algorithm.set( KKT_TOLERANCE, 1e-3 );

  //algorithm.set( DYNAMIC_SENSITIVITY,  FORWARD_SENSITIVITY );
//  algorithm.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
//  algorithm.set( DISCRETIZATION_TYPE, SINGLE_SHOOTING   );
   // algorithm.set( INTEGRATOR_PRINT_LEVEL, PL_MEDIUM );
    
    //algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );

    //algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
    //algorithm.set( MAX_NUM_ITERATIONS, 20 );
    //algorithm.set( KKT_TOLERANCE, 1e-10 );
// 	algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 4 );

    algorithm << window;
    algorithm.solve();

// 	BlockMatrix sens;
// 	algorithm.getSensitivitiesX( sens );
// 	sens.print();

    return 0;
}
/* <<< end tutorial code <<< */




//
//  algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY );
//  algorithm.set( DYNAMIC_SENSITIVITY, BACKWARD_SENSITIVITY );

//  algorithm.set( INTEGRATOR_TYPE, INT_RK45 );
//  algorithm.set( INTEGRATOR_TYPE, INT_RK78 );
//  algorithm.set( INTEGRATOR_TYPE, INT_BDF );
//
//  algorithm.set( KKT_TOLERANCE, 1e-4 );
//  algorithm.set( MAX_NUM_ITERATIONS, 20 );

//  algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );