int
main()
{
Ode ode("rocket");
ode.addState("x");
ode.addState("v");
ode.addState("mass");
ode.addAction("thrust");
ode.dxdt = &dxdt;
Ocp ocp;
SX tEnd = ocp.addParam("tEnd");
MultipleShooting & ms = ocp.addMultipleShooting("rocket", ode, 0, tEnd, 60);
ocp.objFun = tEnd;
// Bounds/initial condition
double x0 = 0;
double xf = 1000;
double massFuel0 = 50;
double massShip = 100;
ocp.boundParam("tEnd", 1, 1000);
for (int k=0; k<ms.N; k++){
ms.boundStateAction("x", x0, xf, k);
ms.boundStateAction("v", -1000, 1000, k);
ms.boundStateAction("mass", massShip, massShip + massFuel0, k);
ms.boundStateAction("thrust", -3000, 3000, k);
}
// initial mass
ms.boundStateAction("mass", massShip + massFuel0, massShip + massFuel0, 0);
// initial/final position
ms.boundStateAction("x", x0, x0, 0);
ms.boundStateAction("x", xf, xf, ms.N-1);
// velocity == 0 at start/finish
ms.boundStateAction("v", 0, 0, 0);
ms.boundStateAction("v", 0, 0, ms.N-1);
return 0;
}
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()
{
/************* get solution **************/
double trackLength = 4;
Ode ode = getOde();
Ocp ocp;
SX tEnd = ocp.addParam("tEnd");
MultipleShooting & ms = ocp.addMultipleShooting("cartpole", ode, 0.0, tEnd, 60);
// cost function
SX xf = ms.getState( "x", ms.N-1);
SX thetaf = ms.getState( "theta", ms.N-1);
SX vthetaf = ms.getState( "vtheta", ms.N-1);
// ocp.objFun = tEnd + 50*cos(thetaf) + 5*vthetaf*vthetaf;
ocp.objFun = ms.N*cos(thetaf) + 5*vthetaf*vthetaf;
for (int k=0; k<ms.N-1; k++){
SX u_k = ms.getAction( "u", k );
SX u_kp1 = ms.getAction( "u", k+1 );
// ocp.objFun += 3*SQR( u_k - u_kp1 );
ocp.objFun += 0.01*SQR( u_k );
}
// bounds
ocp.boundParam("tEnd", 6, 6);
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",-10,10);
/// initial conditions
ms.boundStateAction( "x", 0.0, 0.0, 0);
ms.boundStateAction( "theta", 0.1, 0.1, 0);
ms.boundStateAction( "vx", 0.0, 0.0, 0);
ms.boundStateAction( "vtheta", 0.0, 0.0, 0);
ms.boundStateAction( "u", 0.0, 0.0, 0);
// final conditions
ms.boundStateAction( "x", 0.0, 0.0, ms.N-1);
ms.boundStateAction( "vx", 0.0, 0.0, ms.N-1);
ms.boundStateAction( "vtheta", 0.0, 0.0, ms.N-1);
ms.boundStateAction( "theta", 3.14159, 3.14159, ms.N-1);
ms.boundStateAction( "u", 0.0, 0.0, ms.N-1);
// initial guess
for (int k=0; k<ms.N; k++){
double zero_to_one = k/(ms.N - 1.0);
ms.setStateActionGuess( "u", sin(2*M_PI*zero_to_one), k);
}
// solve
SnoptInterface si(ocp);
si.run();
ocp.writeOctaveOutput("cartpole_multiple_shooting_out");
// Print the optimal cost
cout << "optimal objFcn: " << si.F[0] << endl;
/************** run ddp ****************/
cout << "\nrunning DDP\n";
double t0 = 0;
double tf = ocp.getParamSolution("tEnd");
Ddp ddp(ode, t0, tf, ms.N, &cost);
// regularization
ddp.stateRegularization[0,0] = 1;
ddp.stateRegularization[1,1] = 1;
ddp.stateRegularization[2,2] = 1;
ddp.stateRegularization[3,3] = 1;
ddp.actionRegularization[0,0] = 1;
// action bounding
vector<double> ub(1);
vector<double> lb(1);
ub.at(0) = 10;
lb.at(0) = -10;
ddp.boundAction( lb, ub );
// load ms solution into ddp
for (int k=0; k<ms.N; k++)
ddp.xTrajectory.at(k) = ocp.getStateSolution(k);
for (int k=0; k<ms.N-1; k++)
ddp.uTrajectory.at(k) = ocp.getActionSolution(k);
for (int k=0; k<ms.N-1; k++)
ddp.uTrajectory.at(k).at(0) += 2.2;
// run ddp
for (int k=0; k<200; k++){
ddp.runBackwardSweep();
ddp.runForwardSweep();
cout << "ddp iter: " << k << ", value: " << ddp.V_0.at(0) << endl;
}
ocp.setStates( ddp.xTrajectory );
ocp.setActions( ddp.uTrajectory );
ocp.writeOctaveOutput("cartpole_ddp_out");
cout << "successful finish\n";
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;
}
