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; }