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