int main(int argc, char *argv[])
{
#include "setRootCase.H"
#include "createTime.H"
#include "createMesh.H"
Info << "Reading T_init" << endl;
volScalarField T_init
(
IOobject("T_init", runTime.constant(), mesh, IOobject::MUST_READ),
mesh
);
Info << "Reading P_init" << endl;
volScalarField P_init
(
IOobject("P_init", runTime.constant(), mesh, IOobject::MUST_READ),
mesh
);
Info << "Reading rt_init" << endl;
volScalarField rt_init
(
IOobject("rt_init", runTime.constant(), mesh, IOobject::MUST_READ),
mesh
);
Info << "Reading r_init" << endl;
volScalarField r_init
(
IOobject("r_init", runTime.constant(), mesh, IOobject::MUST_READ),
mesh
);
Info << "Reading or creating tracer field rhof_init" << endl;
surfaceScalarField rhof_init
(
IOobject("rhof_init", runTime.constant(), mesh, IOobject::READ_IF_PRESENT),
linearInterpolate(rt_init)
);
Info << "Creating T" << endl;
volScalarField T
(
IOobject("T", runTime.timeName(), mesh, IOobject::NO_READ),
T_init
);
Info << "Creating P" << endl;
volScalarField P
(
IOobject("P", runTime.timeName(), mesh, IOobject::NO_READ),
P_init
);
Info << "Creating rt" << endl;
volScalarField rt
(
IOobject("rt", runTime.timeName(), mesh, IOobject::NO_READ),
rt_init
);
Info << "Creating rl" << endl;
volScalarField rl
(
IOobject("rl", runTime.timeName(), mesh, IOobject::NO_READ),
r_init
);
Info << "Creating rv" << endl;
volScalarField rv
(
IOobject("rv", runTime.timeName(), mesh, IOobject::NO_READ),
r_init
);
Info << "Creating rl_diag" << endl;
volScalarField rl_diag
(
IOobject("rl_diag", runTime.timeName(), mesh, IOobject::NO_READ),
r_init
);
Info << "Creating rv_diag" << endl;
volScalarField rv_diag
(
IOobject("rv_diag", runTime.timeName(), mesh, IOobject::NO_READ),
r_init
);
Info << "Creating rl_analytic" << endl;
volScalarField rl_analytic
(
IOobject("rl_analytic", runTime.timeName(), mesh, IOobject::NO_READ),
r_init
);
Info << "Creating rv_analytic" << endl;
volScalarField rv_analytic
(
IOobject("rv_analytic", runTime.timeName(), mesh, IOobject::NO_READ),
r_init
);
Info << "Creating rt_analytic" << endl;
volScalarField rt_analytic
(
IOobject("rt_analytic", runTime.timeName(), mesh, IOobject::NO_READ),
r_init
);
Info << "Creating S" << endl;
volScalarField S
(
IOobject("S", runTime.timeName(), mesh, IOobject::NO_READ),
r_init
);
Info << "Creating rhof" << endl;
surfaceScalarField rhof
(
IOobject("rhof", runTime.timeName(), mesh, IOobject::NO_READ),
rhof_init
);
IOdictionary rtDict
(
IOobject
(
"totalMoistureDict",
mesh.time().system(),
mesh,
IOobject::READ_IF_PRESENT,
IOobject::NO_WRITE
)
);
IOdictionary rlDict
(
IOobject
(
"liquidWaterDict",
mesh.time().system(),
mesh,
IOobject::READ_IF_PRESENT,
IOobject::NO_WRITE
)
);
IOdictionary rvDict
(
IOobject
(
"waterVapourDict",
mesh.time().system(),
mesh,
IOobject::READ_IF_PRESENT,
IOobject::NO_WRITE
)
);
IOdictionary tempDict
(
IOobject
(
"tempDict",
mesh.time().system(),
mesh,
IOobject::READ_IF_PRESENT,
IOobject::NO_WRITE
)
);
IOdictionary PDict
(
IOobject
(
"pressureDict",
mesh.time().system(),
mesh,
IOobject::READ_IF_PRESENT,
IOobject::NO_WRITE
)
);
const noAdvection velocityField;
autoPtr<tracerField> rtVal(tracerField::New(rtDict, velocityField));
autoPtr<tracerField> rlVal(tracerField::New(rlDict, velocityField));
autoPtr<tracerField> rvVal(tracerField::New(rvDict, velocityField));
autoPtr<tracerField> tempVal(tracerField::New(tempDict, velocityField));
autoPtr<tracerField> PVal(tracerField::New(PDict, velocityField));
Info << "writing rt for time " << runTime.timeName() << endl;
rtVal->applyTo(rt);
rt.write();
Info << "writing rl for time " << runTime.timeName() << endl;
rlVal->applyTo(rl);
rl.write();
Info << "writing rv for time " << runTime.timeName() << endl;
rvVal->applyTo(rv);
rv.write();
Info << "writing rl_diag for time " << runTime.timeName() << endl;
rlVal->applyTo(rl_diag);
rl_diag.write();
Info << "writing rv_diag for time " << runTime.timeName() << endl;
rvVal->applyTo(rv_diag);
rv_diag.write();
Info << "writing rl_analytic for time " << runTime.timeName() << endl;
rlVal->applyTo(rl_analytic);
rl_analytic.write();
Info << "writing rv_analytic for time " << runTime.timeName() << endl;
rvVal->applyTo(rv_analytic);
rv_analytic.write();
Info << "writing rt_analytic for time " << runTime.timeName() << endl;
rtVal->applyTo(rt_analytic);
rt_analytic.write();
Info << "writing S for time " << runTime.timeName() << endl;
S.write();
Info << "writing qf for time " << runTime.timeName() << endl;
rtVal->applyTo(rhof);
rhof.write();
Info << "writing T" << endl;
tempVal->applyTo(T);
T.write();
Info << "writing P" << endl;
PVal->applyTo(P);
P.write();
return EXIT_SUCCESS;
}
int main() {
double start_x = 0.0;
double start_y = 0.0;
double end_x = 4;
double end_y = 4; // Weird, [4, 2] doesn't work. [4,4] or [4,0] works just fine w/out obstacles.
double d_safe = 0.05;
int num_discretization = 20;
double t_start = 0.0;
double t_end = 1.0;
const double t_step = (t_end - t_start)/num_discretization;
USING_NAMESPACE_ACADO
//---------------------------------------------------------------------
DifferentialState x, y, xdot, ydot; // Differential States
Control ux, uy ; // Control variables
Parameter T ; // the time horizon
DiscretizedDifferentialEquation f( t_step ) ; // Differential dynamics
//---------------------------------------------------------------------
CFunction SD( 1, signedDistanceFunction);
//CFunction F( 4, double_integrator_dynamics);
//CFunction SWVSD( 1, sweptThroughVolumeSignedDistanceFunction);
//F.setUserData((void*) &t_step);
//---------------------------------------------------------------------
IntermediateState z(7);
z(0) = x; z(1) = y; z(2) = xdot; z(3) = ydot; z(4) = ux; z(5) = uy; z(6) = T;
//IntermediateState z(4);
//z(0) = x; z(1) = y; z(2) = next(x); z(3) = next(y);
f << next(x) == x + T*xdot + .5*ux*T*T;
f << next(y) == y + T*ydot + .5*uy*T*T;
f << next(xdot) == xdot + T*ux;
f << next(ydot) == ydot + T*uy;
//---------------------------------------------------------------------
//---------------------------------------------------------------------
OCP ocp( t_start, t_end, num_discretization) ; // time horizon of the optimal control problem is [0, T]
ocp.minimizeMayerTerm( T ) ; // Minimize T
ocp.subjectTo( f ) ;
ocp.subjectTo( AT_START, x == start_x ) ;
ocp.subjectTo( AT_START, y == start_y ) ;
ocp.subjectTo( AT_START, xdot == 0.0 ) ;
ocp.subjectTo( AT_START, ydot == 0.0 ) ;
ocp.subjectTo( AT_END, x == end_x ) ;
ocp.subjectTo( AT_END, y == end_y ) ;
ocp.subjectTo( AT_END, xdot == 0.0 ) ;
ocp.subjectTo( AT_END, ydot == 0.0 ) ;
// Obstacles
//ocp.subjectTo( SD(z) >= d_safe );
//ocp.subjectTo( SWVSD(z) >= d_safe );
ocp.subjectTo( -1 <= ux <= 1 ) ;
ocp.subjectTo( -1 <= uy <= 1 ) ;
ocp.subjectTo( -1 <= xdot <= 1 ) ;
ocp.subjectTo( -1 <= ydot <= 1 ) ;
ocp.subjectTo( 0 <= T ) ;
//---------------------------------------------------------------------
// GnuplotWindow w1(PLOT_AT_START) ;
// w1.addSubplot( x, y, "Position" , "", "", PM_POINTS) ;
// w1.addSubplot( xdot, "Velocity x" ) ;
// w1.addSubplot( ydot, "Velocity y" ) ;
// w1.addSubplot( ux, "Control for x" ) ;
// w1.addSubplot( uy, "Control for y" ) ;
GnuplotWindow w2(PLOT_AT_EACH_ITERATION) ;
w2.addSubplot( x, y, "Position" , "", "", PM_POINTS) ;
w2.addSubplot( xdot, "Velocity x" ) ;
w2.addSubplot( ydot, "Velocity y" ) ;
w2.addSubplot( ux, "Control for x" ) ;
w2.addSubplot( uy, "Control for y" ) ;
//---------------------------------------------------------------------
// Initialize time to something, otherwise things get wacky
double time_init = 20.00e+00;
VariablesGrid T_init(1, 0, 1, num_discretization+1);
for (int i = 0; i <= num_discretization; i++) {
T_init(i, 0) = time_init;
}
// Initialize to straight line with 0 velocities
VariablesGrid state_init(4, 0, 1, num_discretization+1);
state_init.setZero();
for (int i = 0; i <= num_discretization; i++) {
state_init(i, 0) = ((float) i)/num_discretization * (end_x - start_x) + start_x;
}
for (int i = 0; i <= num_discretization; i++) {
state_init(i, 1) = ((float) i)/num_discretization * (end_y - start_y) + start_y;
}
// T_init.print();
// state_init.print();
//---------------------------------------------------------------------
OptimizationAlgorithm algorithm( ocp ) ;
//algorithm << w1 ;
algorithm << w2 ;
algorithm.initializeParameters(T_init) ;
algorithm.initializeDifferentialStates(state_init);
//algorithm.set(HESSIAN_APPROXIMATION, FULL_BFGS_UPDATE);
algorithm.set(HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE); // default
//algorithm.set(HESSIAN_APPROXIMATION, EXACT_HESSIAN);
algorithm.solve() ;
VariablesGrid states, parameters;
algorithm.getDifferentialStates(states);
algorithm.getParameters(parameters);
states.print();
parameters.print();
// MUST MULTIPLY PARAMETERS (WHICH IS TIME VALUE) BY NUM_DISCRETIZATION TO GET MINIMUM TIME
return 0 ;
}
