int
main()
{
Ode ode0 = getRaptorOde();
Ode ode1 = getRaptorOde();
ode0.dxdt = &dxdt0;
ode1.dxdt = &dxdt1;
Ocp ocp;
SX tEnd = ocp.addParam("tEnd");
MultipleShooting & r0 = ocp.addMultipleShooting("r0", ode0, 0, 2.5, 60);
MultipleShooting & r1 = ocp.addMultipleShooting("r1", ode1, 2.5, tEnd, 20);
ocp.objFun = -tEnd; // maximum time
// Bounds
ocp.boundParam("tEnd", 2.6, 6.25);
for (int k=0; k<r0.N; k++){
r0.boundStateAction( "xh", -100, 100, k );
r0.boundStateAction( "yh", -100, 100, k );
r0.boundStateAction( "x1", -100, 100, k );
r0.boundStateAction( "y1", -100, 100, k );
r0.boundStateAction( "x2", -100, 100, k );
r0.boundStateAction( "y2", -100, 100, k );
r0.boundStateAction( "x3", -100, 100, k );
r0.boundStateAction( "y3", -100, 100, k );
r0.boundStateAction( "theta", -10, 10.0, k);
r0.boundStateAction("dtheta", -DTHETA_MAX_DEG*M_PI/180.0, DTHETA_MAX_DEG*M_PI/180.0, k);
SX d1 = r0.getOutput("d1", k);
SX d2 = r0.getOutput("d2", k);
SX d3 = r0.getOutput("d3", k);
ocp.addNonlconIneq( 0.1 - d1 );
ocp.addNonlconIneq( 0.1 - d2 );
ocp.addNonlconIneq( 0.1 - d3 );
}
for (int k=0; k<r1.N; k++){
r1.boundStateAction( "xh", -100, 100, k );
r1.boundStateAction( "yh", -100, 100, k );
r1.boundStateAction( "x1", -100, 100, k );
r1.boundStateAction( "y1", -100, 100, k );
r1.boundStateAction( "x2", -100, 100, k );
r1.boundStateAction( "y2", -100, 100, k );
r1.boundStateAction( "x3", -100, 100, k );
r1.boundStateAction( "y3", -100, 100, k );
r1.boundStateAction( "theta", -10, 10.0, k);
r1.boundStateAction("dtheta", -DTHETA_MAX_DEG*M_PI/180.0, DTHETA_MAX_DEG*M_PI/180.0, k);
SX d1 = r1.getOutput("d1", k);
SX d2 = r1.getOutput("d2", k);
SX d3 = r1.getOutput("d3", k);
ocp.addNonlconIneq( 0.1 - d1 );
ocp.addNonlconIneq( 0.1 - d2 );
ocp.addNonlconIneq( 0.1 - d3 );
}
// join stages
SX x0finish = r0.getStateMat ( r0.N - 1 );
SX u0finish = r0.getActionMat( r0.N - 1 );
SX x1start = r1.getStateMat ( 0 );
SX u1start = r1.getActionMat( 0 );
ocp.addNonlconEq( x0finish - x1start );
ocp.addNonlconEq( u0finish - u1start );
// initial conditions
r0.boundStateAction("xh", 0, 0, 0);
r0.boundStateAction("yh", 10.0*sqrt(3.0)/3.0, 10.0*sqrt(3.0)/3.0, 0);
r0.boundStateAction("x1", 0.0, 0.0, 0 );
r0.boundStateAction("y1", 20.0*sqrt(3.0)/2.0, 20.0*sqrt(3.0)/2.0, 0 );
r0.boundStateAction("x2", -10.0, -10.0, 0 );
r0.boundStateAction("y2", 0.0, 0.0, 0 );
r0.boundStateAction("x3", 10.0, 10.0, 0 );
r0.boundStateAction("y3", 0.0, 0.0, 0 );
// r0.boundStateAction("s1", 0.0, 0.0, 0 );
// r0.boundStateAction("s2", 0.0, 0.0, 0 );
// r0.boundStateAction("s3", 0.0, 0.0, 0 );
// initial guesses
ocp.setParamGuess("tEnd", 3.0);
r0.setStateActionGuess("theta", 45*M_PI/180.0);
r1.setStateActionGuess("theta", 45*M_PI/180.0);
for (int k=0; k<r0.N; k++){
r0.setStateActionGuess("xh", 0, k);
r0.setStateActionGuess("yh", 10.0*sqrt(3.0)/3.0, k);
r0.setStateActionGuess("x1", 0.0, k );
r0.setStateActionGuess("y1", 20.0*sqrt(3.0)/2.0, k );
r0.setStateActionGuess("x2", -10.0, k );
r0.setStateActionGuess("y2", 0.0, k );
r0.setStateActionGuess("x3", 10.0, k );
r0.setStateActionGuess("y3", 0.0, k );
// r0.setStateActionGuess("s1", 5.0, k );
// r0.setStateActionGuess("s2", 5.0, k );
// r0.setStateActionGuess("s3", 5.0, k );
}
for (int k=0; k<r1.N; k++){
r1.setStateActionGuess("xh", 0, k);
r1.setStateActionGuess("yh", 10.0*sqrt(3.0)/3.0, k);
r1.setStateActionGuess("x1", 0.0, k );
r1.setStateActionGuess("y1", 20.0*sqrt(3.0)/2.0, k );
r1.setStateActionGuess("x2", -10.0, k );
r1.setStateActionGuess("y2", 0.0, k );
r1.setStateActionGuess("x3", 10.0, k );
r1.setStateActionGuess("y3", 0.0, k );
// r1.setStateActionGuess("s1", 10.0, k );
// r1.setStateActionGuess("s2", 12.5, k );
// r1.setStateActionGuess("s3", 12.5, k );
}
// Create the NLP solver
SnoptInterface si(ocp);
si.run();
// Print the optimal cost
cout << "optimal time: " << -si.F[0] << endl;
ocp.writeOctaveOutput( "raptor_out" );
return 0;
}
int
main()
{
double trackLength = 4;
Ode ode = getOde();
Ocp ocp;
SX tEnd = ocp.addParam("tEnd");
MultipleShooting & ms = ocp.addMultipleShooting("cartpole", ode, 0.0, tEnd, 60);
int N = ms.N;
// cost function
SX xf = ms.getState("x", ms.N-1);
SX thetaf = ms.getState("theta", N-1);
SX vthetaf = ms.getState("vtheta", N-1);
ocp.objFun = tEnd+50*cos(thetaf)+5*(vthetaf)*vthetaf; // minimum time
//ocp.objFun = tEnd;
// bounds
ocp.boundParam("tEnd", 4, 50);
ms.boundStateAction("x", -trackLength/2, trackLength/2);
ms.boundStateAction("vx", -22, 22);
ms.boundStateAction("theta", -50, 50);
ms.boundStateAction("vtheta", -50, 50);
ms.boundStateAction("u",-20,20);
/// initial conditions
ms.boundStateAction("x",0,0,0);
ms.boundStateAction("theta",0.1,0.1,0);
ms.boundStateAction("vx",0,0,0);
ms.boundStateAction("vtheta",0,0,0);
ocp.addNonlconIneq(ms.getState("x",0), "startx");
ocp.addNonlconEq(ms.getState("vx",N/2), "xstall");
// for(int k = 0; k < ms.N; k++) {
// ms.setStateActionGuess("theta", double(k)*3.1415/ms.N, k);
// ms.setStateActionGuess("vtheta", 3.1415/ms.N, k);
// }
// ms.boundStateAction("theta",3.1415,3.1415,ms.N-1);
// ms.boundStateAction("x",0, 0, ms.N-1);
// ms.boundStateAction("vx",0, 0, ms.N-1);
// ms.boundStateAction("vtheta",0, 0, ms.N-1);
// solve
SnoptInterface si(ocp);
si.run();
// Print the optimal cost
cout << "optimal time: " << si.F[0] << endl;
ocp.writeOctaveOutput("cartpole_out");
si.writeOctaveOutput("multipliers_out");
return 0;
}
