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("double_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;
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", 3, 20);
ocp.setParamGuess("tEnd", 6);
ms.boundStateAction( "th0", -trackLength/2, trackLength/2);
ms.boundStateAction( "th1", -4*M_PI, 4*M_PI);
ms.boundStateAction( "th2", -4*M_PI, 4*M_PI);
ms.boundStateAction( "th0d", -20, 20);
ms.boundStateAction( "th1d", -200, 200);
ms.boundStateAction( "th2d", -200, 200);
ms.boundStateAction("u", -100, 100 );
// initial guess
for (int k=0; k<ms.N; k++){
double zero_to_one = k/(ms.N - 1.0);
ms.setStateActionGuess( "th0", 0.0, k);
ms.setStateActionGuess( "th1", (1 - zero_to_one)*M_PI, k);
ms.setStateActionGuess( "th2", (1 - zero_to_one)*M_PI, k);
ms.setStateActionGuess( "th0d", 0.0, k);
ms.setStateActionGuess( "th1d", -M_PI/6, k);
ms.setStateActionGuess( "th2d", -M_PI/6, k);
//ms.setStateActionGuess( "u", sin(2*M_PI*zero_to_one), k);
}
/// initial conditions
ms.boundStateAction( "th0", 0.0, 0.0, 0);
ms.boundStateAction( "th1", M_PI-0.1, M_PI-0.1, 0);
ms.boundStateAction( "th2", M_PI-0.1, M_PI-0.1, 0);
ms.boundStateAction( "th0d", 0.0, 0.0, 0);
ms.boundStateAction( "th1d", 0.0, 0.0, 0);
ms.boundStateAction( "th2d", 0.0, 0.0, 0);
ms.boundStateAction( "u", 0.0, 0.0, 0);
// final conditions
ms.boundStateAction( "th0", 0.0, 0.0, ms.N - 1);
ms.boundStateAction( "th1", 0.0, 0.0, ms.N - 1);
ms.boundStateAction( "th2", 0.0, 0.0, ms.N - 1);
ms.boundStateAction( "th0d", 0.0, 0.0, ms.N - 1);
ms.boundStateAction( "th1d", 0.0, 0.0, ms.N - 1);
ms.boundStateAction( "th2d", 0.0, 0.0, ms.N - 1);
// solve
SnoptInterface si(ocp);
si.run();
ocp.writeOctaveOutput("double_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<0; 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("double_cartpole_ddp_out");
cout << "successful finish\n";
return 0;
}
