int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState x; DifferentialState l; AlgebraicState z; Control u; DifferentialEquation f; const double t_start = 0.0; const double t_end = 10.0; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- f << dot(x) == -x + 0.5*x*x + u + 0.5*z; f << dot(l) == x*x + 3.0*u*u ; f << 0 == z + exp(z) - 1.0 + x ; // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( t_start, t_end, 10 ); ocp.minimizeMayerTerm( l ); ocp.subjectTo( f ); ocp.subjectTo( AT_START, x == 1.0 ); ocp.subjectTo( AT_START, l == 0.0 ); GnuplotWindow window; window.addSubplot(x,"DIFFERENTIAL STATE x"); window.addSubplot(z,"ALGEBRAIC STATE z" ); window.addSubplot(u,"CONTROL u" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // ---------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.set( ABSOLUTE_TOLERANCE , 1.0e-7 ); algorithm.set( INTEGRATOR_TOLERANCE , 1.0e-7 ); algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); //algorithm.set( GLOBALIZATION_STRATEGY, GS_FULLSTEP ); algorithm << window; algorithm.solve(); return 0; }
int main( ){ USING_NAMESPACE_ACADO TIME autotime; DifferentialState x(2); AlgebraicState z; Control u; DifferentialEquation f1; IntermediateState setc_is_1(5); setc_is_1(0) = autotime; setc_is_1(1) = x(0); setc_is_1(2) = x(1); setc_is_1(3) = z; setc_is_1(4) = u; CFunction cLinkModel_1( 3, myAcadoDifferentialEquation1 ); f1 << cLinkModel_1(setc_is_1); double dconstant1 = 0.0; double dconstant2 = 5.0; int dconstant3 = 10; OCP ocp1(dconstant1, dconstant2, dconstant3); ocp1.minimizeMayerTerm(x(1)); ocp1.subjectTo(f1); ocp1.subjectTo(AT_START, x(0) == 1.0 ); ocp1.subjectTo(AT_START, x(1) == 0.0 ); GnuplotWindow window; window.addSubplot(x(0),"DIFFERENTIAL STATE x"); window.addSubplot(z,"ALGEBRAIC STATE z" ); window.addSubplot(u,"CONTROL u" ); OptimizationAlgorithm algo1(ocp1); algo1.set( KKT_TOLERANCE, 1.000000E-05 ); algo1.set( RELAXATION_PARAMETER, 1.500000E+00 ); // algo1.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); // algo1 << window; algo1.solve(); return 0; }
int main( ){ USING_NAMESPACE_ACADO DifferentialState s,v,m ; // the differential states Control u ; // the control input u Parameter T ; // the time horizon T DifferentialEquation f( 0.0, T ); // the differential equation // ------------------------------------- OCP ocp( 0.0, T ); // time horizon of the OCP: [0,T] ocp.minimizeMayerTerm( T ); // the time T should be optimized f << dot(s) == v; // an implementation f << dot(v) == (u-0.2*v*v)/m; // of the model equations f << dot(m) == -0.01*u*u; // for the rocket. ocp.subjectTo( f ); // minimize T s.t. the model, ocp.subjectTo( AT_START, s == 0.0 ); // the initial values for s, ocp.subjectTo( AT_START, v == 0.0 ); // v, ocp.subjectTo( AT_START, m == 1.0 ); // and m, ocp.subjectTo( AT_END , s == 10.0 ); // the terminal constraints for s ocp.subjectTo( AT_END , v == 0.0 ); // and v, ocp.subjectTo( -0.1 <= v <= 1.7 ); // as well as the bounds on v ocp.subjectTo( -1.1 <= u <= 1.1 ); // the control input u, ocp.subjectTo( 5.0 <= T <= 15.0 ); // and the time horizon T. // ------------------------------------- GnuplotWindow window; window.addSubplot( s, "THE DISTANCE s" ); window.addSubplot( v, "THE VELOCITY v" ); window.addSubplot( m, "THE MASS m" ); window.addSubplot( u, "THE CONTROL INPUT u" ); OptimizationAlgorithm algorithm(ocp); // the optimization algorithm algorithm << window; algorithm.solve(); // solves the problem. return 0; }
int main( ){ USING_NAMESPACE_ACADO DifferentialState phi, omega; // the states of the pendulum Parameter l, alpha ; // its length and the friction const double g = 9.81 ; // the gravitational constant DifferentialEquation f ; // the model equations Function h ; // the measurement function VariablesGrid measurements; // read the measurements measurements = readFromFile( "data.txt" ); // from a file. // -------------------------------------- OCP ocp(measurements.getTimePoints()); // construct an OCP h << phi ; // the state phi is measured ocp.minimizeLSQ( h, measurements ) ; // fit h to the data f << dot(phi ) == omega ; // a symbolic implementation f << dot(omega) == -(g/l) *sin(phi ) // of the model - alpha*omega ; // equations ocp.subjectTo( f ); // solve OCP s.t. the model, ocp.subjectTo( 0.0 <= alpha <= 4.0 ); // the bounds on alpha ocp.subjectTo( 0.0 <= l <= 2.0 ); // and the bounds on l. // -------------------------------------- GnuplotWindow window; window.addSubplot( phi , "The angle phi", "time [s]", "angle [rad]" ); window.addSubplot( omega, "The angular velocity dphi" ); window.addData( 0, measurements(0) ); ParameterEstimationAlgorithm algorithm(ocp); // the parameter estimation algorithm << window; algorithm.solve(); // algorithm solves the problem. return 0; }
/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // DEFINE A VARIABLES GRID: // ------------------------ Grid dataGrid( 0.0, 5.0, 6 ); VariablesGrid data; data.init( 2, dataGrid ); data( 0, 0 ) = 0.0; data( 0, 1 ) = 1.0 ; data( 1, 0 ) = 0.2; data( 1, 1 ) = 0.8 ; data( 2, 0 ) = 0.4; data( 2, 1 ) = 0.7 ; data( 3, 0 ) = 0.6; data( 3, 1 ) = 0.65 ; data( 4, 0 ) = 0.8; data( 4, 1 ) = 0.625; data( 5, 0 ) = 1.0; data( 5, 1 ) = 0.613; // CONSTRUCT A CURVE INTERPOLATING THE DATA: // ----------------------------------------- Curve c1, c2; c1.add( data, IM_CONSTANT ); c2.add( data, IM_LINEAR ); // PLOT CURVES ON GIVEN GRID: // -------------------------- GnuplotWindow window; window.addSubplot( c1, 0.0,5.0, "Constant data Interpolation" ); window.addSubplot( c2, 0.0,5.0, "Linear data Interpolation" ); window.plot(); return 0; }
int main( ) { USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState xB; DifferentialState xW; DifferentialState vB; DifferentialState vW; Control F; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- double h = 0.05; DiscretizedDifferentialEquation f( h ); // f << next(xB) == ( 9.523918456856767e-01*xB - 3.093442425036754e-03*xW + 4.450257887258270e-04*vW - 2.380407715716160e-07*F ); // f << next(xW) == ( -1.780103154903307e+00*xB - 1.005721624707961e+00*xW - 3.093442425036752e-03*vW - 8.900515774516536e-06*F ); // f << next(vB) == ( -5.536210379145256e+00*xB - 2.021981836435758e-01*xW + 1.0*vB + 2.474992857984263e-02*vW + 1.294618052471308e-04*F ); // f << next(vW) == ( 1.237376970014700e+01*xB + 1.183104351525840e+01*xW - 1.005721624707961e+00*vW + 6.186884850073496e-05*F ); f << next(xB) == ( 0.9335*xB + 0.0252*xW + 0.048860*vB + 0.000677*vW + 3.324e-06*F ); f << next(xW) == ( 0.1764*xB - 0.9821*xW + 0.004739*vB - 0.002591*vW - 8.822e-06*F ); f << next(vB) == ( -2.5210*xB - 0.1867*xW + 0.933500*vB + 0.025200*vW + 0.0001261*F ); f << next(vW) == ( -1.3070*xB + 11.670*xW + 0.176400*vB - 0.982100*vW + 6.536e-05*F ); OutputFcn g; g << xB; g << 500.0*vB + F; DynamicSystem dynSys( f,g ); // SETUP THE PROCESS: // ------------------ Vector mean( 1 ), amplitude( 1 ); mean.setZero( ); amplitude.setAll( 50.0 ); GaussianNoise myNoise( mean,amplitude ); Actuator myActuator( 1 ); myActuator.setControlNoise( myNoise,0.1 ); myActuator.setControlDeadTimes( 0.1 ); mean.setZero( ); amplitude.setAll( 0.001 ); UniformNoise myOutputNoise1( mean,amplitude ); mean.setAll( 20.0 ); amplitude.setAll( 10.0 ); GaussianNoise myOutputNoise2( mean,amplitude ); Sensor mySensor( 2 ); mySensor.setOutputNoise( 0,myOutputNoise1,0.1 ); mySensor.setOutputNoise( 1,myOutputNoise2,0.1 ); mySensor.setOutputDeadTimes( 0.15 ); Process myProcess; myProcess.setDynamicSystem( dynSys ); myProcess.set( ABSOLUTE_TOLERANCE,1.0e-8 ); myProcess.setActuator( myActuator ); myProcess.setSensor( mySensor ); Vector x0( 4 ); x0.setZero( ); x0( 0 ) = 0.01; myProcess.initializeStartValues( x0 ); myProcess.set( PLOT_RESOLUTION,HIGH ); // myProcess.set( CONTROL_PLOTTING,PLOT_NOMINAL ); // myProcess.set( PARAMETER_PLOTTING,PLOT_NOMINAL ); myProcess.set( OUTPUT_PLOTTING,PLOT_REAL ); GnuplotWindow window; window.addSubplot( xB, "Body Position [m]" ); window.addSubplot( xW, "Wheel Position [m]" ); window.addSubplot( vB, "Body Velocity [m/s]" ); window.addSubplot( vW, "Wheel Velocity [m/s]" ); window.addSubplot( F,"Damping Force [N]" ); window.addSubplot( g(0),"Output 1" ); window.addSubplot( g(1),"Output 2" ); myProcess << window; // SIMULATE AND GET THE RESULTS: // ----------------------------- VariablesGrid u( 1,0.0,1.0,6 ); u( 0,0 ) = 10.0; u( 1,0 ) = -200.0; u( 2,0 ) = 200.0; u( 3,0 ) = 200.0; u( 4,0 ) = 0.0; u( 5,0 ) = 0.0; myProcess.init( 0.0,x0,u.getFirstVector() ); myProcess.run( u ); VariablesGrid uNom, uSim, ySim, ySens, xSim; // myProcess.getLast( LOG_NOMINAL_CONTROLS,uNom ); uNom.print( "uNom" ); // myProcess.getLast( LOG_SIMULATED_CONTROLS,uSim ); uSim.print( "uSim" ); // myProcess.getLast( LOG_SIMULATED_OUTPUT,ySim ); ySim.print( "ySim" ); // myProcess.getLast( LOG_PROCESS_OUTPUT,ySens ); ySens.print( "ySens" ); // // myProcess.getLast( LOG_DIFFERENTIAL_STATES,xSim ); return 0; }
/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------------------ DifferentialState v,s,m; Control u ; const double t_start = 0.0; const double t_end = 10.0; const double h = 0.01; DiscretizedDifferentialEquation f(h) ; // DEFINE A DISCRETE-TIME SYTSEM: // ------------------------------- f << next(s) == s + h*v; f << next(v) == v + h*(u-0.02*v*v)/m; f << next(m) == m - h*0.01*u*u; Function eta; eta << u; // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( t_start, t_end, 50 ); //ocp.minimizeLagrangeTerm( u*u ); ocp.minimizeLSQ( eta ); ocp.subjectTo( f ); ocp.subjectTo( AT_START, s == 0.0 ); ocp.subjectTo( AT_START, v == 0.0 ); ocp.subjectTo( AT_START, m == 1.0 ); ocp.subjectTo( AT_END , s == 10.0 ); ocp.subjectTo( AT_END , v == 0.0 ); ocp.subjectTo( -0.01 <= v <= 1.3 ); // DEFINE A PLOT WINDOW: // --------------------- GnuplotWindow window; window.addSubplot( s,"DifferentialState s" ); window.addSubplot( v,"DifferentialState v" ); window.addSubplot( m,"DifferentialState m" ); window.addSubplot( u,"Control u" ); window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" ); window.addSubplot( 0.5 * m * v*v,"Kinetic Energy" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.set( INTEGRATOR_TYPE, INT_DISCRETE ); algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); algorithm.set( KKT_TOLERANCE, 1e-10 ); algorithm << window; algorithm.solve(); return 0; }
USING_NAMESPACE_ACADO int main( ) { // DIFFERENTIAL STATES : // ------------------------- DifferentialState r; // the length r of the cable DifferentialState phi; // the angle phi DifferentialState theta; // the angle theta (grosser wert- naeher am boden/kleiner wert - weiter oben) // ------------------------- // ------------------------------------------- DifferentialState dr; // first derivative of r0 with respect to t DifferentialState dphi; // first derivative of phi with respect to t DifferentialState dtheta; // first derivative of theta with respect to t // ------------------------- // ------------------------------------------- DifferentialState n; // winding number(rauslassen) // ------------------------- // ------------------------------------------- DifferentialState Psi; // the roll angle Psi DifferentialState CL; // the aerodynamic lift coefficient // ------------------------- // ------------------------------------------- DifferentialState W; // integral over the power at the generator // ( = ENERGY )(rauslassen) // MEASUREMENT FUNCTION : // ------------------------- Function model_response ; // the measurement function // CONTROL : // ------------------------- Control ddr0; // second derivative of r0 with respect to t Control dPsi; // first derivative of Psi with respect to t Control dCL; // first derivative of CL with respect to t Disturbance w_extra; // PARAMETERS : // ------------------------ // PARAMETERS OF THE KITE : // ----------------------------- double mk = 2000.00; // mass of the kite // [ kg ](maybe change to 5000kg) double A = 500.00; // effective area // [ m^2 ] double V = 720.00; // volume // [ m^3 ] double cd0 = 0.04; // aerodynamic drag coefficient // [ ] // ( cd0: without downwash ) double K = 0.04; // induced drag constant // [ ] // PHYSICAL CONSTANTS : // ----------------------------- double g = 9.81; // gravitational constant // [ m /s^2] double rho = 1.23; // density of the air // [ kg/m^3] // PARAMETERS OF THE CABLE : // ----------------------------- double rhoc = 1450.00; // density of the cable // [ kg/m^3] double cc = 1.00; // frictional constant // [ ] double dc = 0.05614; // diameter // [ m ] // PARAMETERS OF THE WIND : // ----------------------------- double w0 = 10.00; // wind velocity at altitude h0 // [ m/s ] double h0 = 100.00; // the altitude h0 // [ m ] double hr = 0.10; // roughness length // [ m ] // OTHER VARIABLES : // ------------------------ double AQ ; // cross sectional area IntermediateState mc; // mass of the cable IntermediateState m ; // effective inertial mass IntermediateState m_; // effective gravitational mass IntermediateState Cg; IntermediateState dm; // first derivative of m with respect to t // DEFINITION OF PI : // ------------------------ double PI = 3.1415926535897932; // ORIENTATION AND FORCES : // ------------------------ IntermediateState h ; // altitude of the kite IntermediateState w ; // the wind at altitude h IntermediateState we [ 3]; // effective wind vector IntermediateState nwe ; // norm of the effective wind vector IntermediateState nwep ; // - IntermediateState ewep [ 3]; // projection of ewe IntermediateState eta ; // angle eta: cf. [2] IntermediateState et [ 3]; // unit vector from the left to the right wing tip IntermediateState Caer ; IntermediateState Cf ; // simple constants IntermediateState CD ; // the aerodynamic drag coefficient IntermediateState Fg [ 3]; // the gravitational force IntermediateState Faer [ 3]; // the aerodynamic force IntermediateState Ff [ 3]; // the frictional force IntermediateState F [ 3]; // the total force // TERMS ON RIGHT-HAND-SIDE // OF THE DIFFERENTIAL // EQUATIONS : // ------------------------ IntermediateState a_pseudo [ 3]; // the pseudo accelaration IntermediateState dn ; // the time derivate of the kite's winding number IntermediateState ddr ; // second derivative of s with respect to t IntermediateState ddphi ; // second derivative of phi with respect to t IntermediateState ddtheta ; // second derivative of theta with respect to t IntermediateState power ; // the power at the generator // ------------------------ ------ // ---------------------------------------------- IntermediateState regularisation ; // regularisation of controls // MODEL EQUATIONS : // =============================================================== // SPRING CONSTANT OF THE CABLE : // --------------------------------------------------------------- AQ = PI*dc*dc/4.0 ; // THE EFECTIVE MASS' : // --------------------------------------------------------------- mc = rhoc*AQ*r ; // mass of the cable m = mk + mc / 3.0; // effective inertial mass m_ = mk + mc / 2.0; // effective gravitational mass // ----------------------------- // ---------------------------- dm = (rhoc*AQ/ 3.0)*dr; // time derivative of the mass // WIND SHEAR MODEL : // --------------------------------------------------------------- // for startup +60m h = r*cos(theta)+60.0 ; // h = r*cos(theta) ; w = log(h/hr) / log(h0/hr) *(w0+w_extra) ; // EFFECTIVE WIND IN THE KITE`S SYSTEM : // --------------------------------------------------------------- we[0] = w*sin(theta)*cos(phi) - dr ; we[1] = -w*sin(phi) - r*sin(theta)*dphi ; we[2] = -w*cos(theta)*cos(phi) + r *dtheta; // CALCULATION OF THE KITE`S TRANSVERSAL AXIS : // --------------------------------------------------------------- nwep = pow( we[1]*we[1] + we[2]*we[2], 0.5 ); nwe = pow( we[0]*we[0] + we[1]*we[1] + we[2]*we[2], 0.5 ); eta = asin( we[0]*tan(Psi)/ nwep ) ; // --------------------------------------------------------------- // ewep[0] = 0.0 ; ewep[1] = we[1] / nwep ; ewep[2] = we[2] / nwep ; // --------------------------------------------------------------- et [0] = sin(Psi) ; et [1] = (-cos(Psi)*sin(eta))*ewep[1] - (cos(Psi)*cos(eta))*ewep[2]; et [2] = (-cos(Psi)*sin(eta))*ewep[2] + (cos(Psi)*cos(eta))*ewep[1]; // CONSTANTS FOR SEVERAL FORCES : // --------------------------------------------------------------- Cg = (V*rho-m_)*g ; Caer = (rho*A/2.0 )*nwe ; Cf = (rho*dc/8.0)*r*nwe ; // THE DRAG-COEFFICIENT : // --------------------------------------------------------------- CD = cd0 + K*CL*CL ; // SUM OF GRAVITATIONAL AND LIFTING FORCE : // --------------------------------------------------------------- Fg [0] = Cg * cos(theta) ; // Fg [1] = Cg * 0.0 ; Fg [2] = Cg * sin(theta) ; // SUM OF THE AERODYNAMIC FORCES : // --------------------------------------------------------------- Faer[0] = Caer*( CL*(we[1]*et[2]-we[2]*et[1]) + CD*we[0] ) ; Faer[1] = Caer*( CL*(we[2]*et[0]-we[0]*et[2]) + CD*we[1] ) ; Faer[2] = Caer*( CL*(we[0]*et[1]-we[1]*et[0]) + CD*we[2] ) ; // THE FRICTION OF THE CABLE : // --------------------------------------------------------------- // Ff [0] = Cf * cc* we[0] ; Ff [1] = Cf * cc* we[1] ; Ff [2] = Cf * cc* we[2] ; // THE TOTAL FORCE : // --------------------------------------------------------------- F [0] = Fg[0] + Faer[0] ; F [1] = Faer[1] + Ff[1] ; F [2] = Fg[2] + Faer[2] + Ff[2] ; // THE PSEUDO ACCELERATION: // --------------------------------------------------------------- a_pseudo [0] = - ddr0 + r*( dtheta*dtheta + sin(theta)*sin(theta)*dphi *dphi ) - dm/m*dr ; a_pseudo [1] = - 2.0*cos(theta)/sin(theta)*dphi*dtheta - 2.0*dr/r*dphi - dm/m*dphi ; a_pseudo [2] = cos(theta)*sin(theta)*dphi*dphi - 2.0*dr/r*dtheta - dm/m*dtheta ; // THE EQUATIONS OF MOTION: // --------------------------------------------------------------- ddr = F[0]/m + a_pseudo[0] ; ddphi = F[1]/(m*r*sin(theta)) + a_pseudo[1] ; ddtheta = -F[2]/(m*r ) + a_pseudo[2] ; // THE DERIVATIVE OF THE WINDING NUMBER : // --------------------------------------------------------------- dn = ( dphi*ddtheta - dtheta*ddphi ) / (2.0*PI*(dphi*dphi + dtheta*dtheta) ) ; // THE POWER AT THE GENERATOR : // --------------------------------------------------------------- power = m*ddr*dr ; // REGULARISATION TERMS : // --------------------------------------------------------------- regularisation = 5.0e2 * ddr0 * ddr0 + 1.0e8 * dPsi * dPsi + 1.0e5 * dCL * dCL + 2.5e5 * dn * dn + 2.5e7 * ddphi * ddphi; + 2.5e7 * ddtheta * ddtheta; + 2.5e6 * dtheta * dtheta; // --------------------------- // REFERENCE TRAJECTORY: // --------------------------------------------------------------- VariablesGrid myReference; myReference.read( "ref_w_zeros.txt" );// read the measurements PeriodicReferenceTrajectory reference( myReference ); // THE "RIGHT-HAND-SIDE" OF THE ODE: // --------------------------------------------------------------- DifferentialEquation f; f << dot(r) == dr ; f << dot(phi) == dphi ; f << dot(theta) == dtheta ; f << dot(dr) == ddr0 ; f << dot(dphi) == ddphi ; f << dot(dtheta) == ddtheta ; f << dot(n) == dn ; f << dot(Psi) == dPsi ; f << dot(CL) == dCL ; f << dot(W) == (-power + regularisation)*1.0e-6; model_response << r ; // the state r is measured model_response << phi ; model_response << theta; model_response << dr ; model_response << dphi ; model_response << dtheta; model_response << ddr0; model_response << dPsi; model_response << dCL ; DVector x_scal(9); x_scal(0) = 60.0; x_scal(1) = 1.0e-1; x_scal(2) = 1.0e-1; x_scal(3) = 40.0; x_scal(4) = 1.0e-1; x_scal(5) = 1.0e-1; x_scal(6) = 60.0; x_scal(7) = 1.5e-1; x_scal(8) = 2.5; DMatrix Q(9,9); Q.setIdentity(); DMatrix Q_end(9,9); Q_end.setIdentity(); int i; for( i = 0; i < 6; i++ ){ Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i)); Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i)); } for( i = 6; i < 9; i++ ){ Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i)); Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i)); } DVector measurements(9); measurements.setAll( 0.0 ); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- const double t_start = 0.0; const double t_end = 10.0; OCP ocp( t_start, t_end, 10 ); ocp.minimizeLSQ( Q,model_response, measurements ) ; // fit h to the data ocp.minimizeLSQEndTerm( Q_end,model_response, measurements ) ; ocp.subjectTo( f ); ocp.subjectTo( -0.34 <= phi <= 0.34 ); ocp.subjectTo( 0.85 <= theta <= 1.45 ); ocp.subjectTo( -40.0 <= dr <= 10.0 ); ocp.subjectTo( -0.29 <= Psi <= 0.29 ); ocp.subjectTo( 0.1 <= CL <= 1.50 ); ocp.subjectTo( -0.7 <= n <= 0.90 ); ocp.subjectTo( -25.0 <= ddr0 <= 25.0 ); ocp.subjectTo( -0.065 <= dPsi <= 0.065 ); ocp.subjectTo( -3.5 <= dCL <= 3.5 ); ocp.subjectTo( -60.0 <= cos(theta)*r ); ocp.subjectTo( w_extra == 0.0 ); // SETTING UP THE (SIMULATED) PROCESS: // ----------------------------------- OutputFcn identity; DynamicSystem dynamicSystem( f,identity ); Process process( dynamicSystem,INT_RK45 ); VariablesGrid disturbance; disturbance.read( "my_wind_disturbance_controlsfree.txt" ); if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); // SETUP OF THE ALGORITHM AND THE TUNING OPTIONS: // ---------------------------------------------- double samplingTime = 1.0; RealTimeAlgorithm algorithm( ocp, samplingTime ); if (algorithm.initializeDifferentialStates("p_s_ref.txt" ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); if (algorithm.initializeControls ("p_c_ref.txt" ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); algorithm.set( MAX_NUM_ITERATIONS, 2 ); algorithm.set( KKT_TOLERANCE , 1e-4 ); algorithm.set( HESSIAN_APPROXIMATION,GAUSS_NEWTON); algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 ); algorithm.set( GLOBALIZATION_STRATEGY,GS_FULLSTEP ); // algorithm.set( USE_IMMEDIATE_FEEDBACK, YES ); algorithm.set( USE_REALTIME_SHIFTS, YES ); algorithm.set(LEVENBERG_MARQUARDT, 1e-5); DVector x0(10); x0(0) = 1.8264164528775887e+03; x0(1) = -5.1770453309520573e-03; x0(2) = 1.2706440287266794e+00; x0(3) = 2.1977888424944396e+00; x0(4) = 3.1840786108641383e-03; x0(5) = -3.8281200674676448e-02; x0(6) = 0.0000000000000000e+00; x0(7) = -1.0372313936413566e-02; x0(8) = 1.4999999999999616e+00; x0(9) = 0.0000000000000000e+00; // SETTING UP THE NMPC CONTROLLER: // ------------------------------- Controller controller( algorithm, reference ); // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE... // ---------------------------------------------------------- double simulationStart = 0.0; double simulationEnd = 10.0; SimulationEnvironment sim( simulationStart, simulationEnd, process, controller ); if (sim.init( x0 ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); if (sim.run( ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); // ...AND PLOT THE RESULTS // ---------------------------------------------------------- VariablesGrid diffStates; sim.getProcessDifferentialStates( diffStates ); diffStates.print( "diffStates.txt" ); diffStates.print( "diffStates.m","DIFFSTATES",PS_MATLAB ); VariablesGrid interStates; sim.getProcessIntermediateStates( interStates ); interStates.print( "interStates.txt" ); interStates.print( "interStates.m","INTERSTATES",PS_MATLAB ); VariablesGrid sampledProcessOutput; sim.getSampledProcessOutput( sampledProcessOutput ); sampledProcessOutput.print( "sampledOut.txt" ); sampledProcessOutput.print( "sampledOut.m","OUT",PS_MATLAB ); VariablesGrid feedbackControl; sim.getFeedbackControl( feedbackControl ); feedbackControl.print( "controls.txt" ); feedbackControl.print( "controls.m","CONTROL",PS_MATLAB ); GnuplotWindow window; window.addSubplot( sampledProcessOutput(0), "DIFFERENTIAL STATE: r" ); window.addSubplot( sampledProcessOutput(1), "DIFFERENTIAL STATE: phi" ); window.addSubplot( sampledProcessOutput(2), "DIFFERENTIAL STATE: theta" ); window.addSubplot( sampledProcessOutput(3), "DIFFERENTIAL STATE: dr" ); window.addSubplot( sampledProcessOutput(4), "DIFFERENTIAL STATE: dphi" ); window.addSubplot( sampledProcessOutput(5), "DIFFERENTIAL STATE: dtheta" ); window.addSubplot( sampledProcessOutput(7), "DIFFERENTIAL STATE: Psi" ); window.addSubplot( sampledProcessOutput(8), "DIFFERENTIAL STATE: CL" ); window.addSubplot( sampledProcessOutput(9), "DIFFERENTIAL STATE: W" ); window.addSubplot( feedbackControl(0), "CONTROL 1 DDR0" ); window.addSubplot( feedbackControl(1), "CONTROL 1 DPSI" ); window.addSubplot( feedbackControl(2), "CONTROL 1 DCL" ); window.plot( ); GnuplotWindow window2; window2.addSubplot( interStates(1) ); window2.plot(); return 0; }
/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // DEFINE VALRIABLES: // --------------------------- DifferentialStateVector q(2); // the generalized coordinates of the pendulum DifferentialStateVector dq(2); // the associated velocities const double L1 = 1.00; // length of the first pendulum const double L2 = 1.00; // length of the second pendulum const double m1 = 1.00; // mass of the first pendulum const double m2 = 1.00; // mass of the second pendulum const double g = 9.81; // gravitational constant const double alpha = 0.10; // a friction constant const double J_11 = (m1+m2)*L1*L1; // auxiliary variable (inertia comp.) const double J_22 = m2 *L2*L2; // auxiliary variable (inertia comp.) const double J_12 = m2 *L1*L2; // auxiliary variable (inertia comp.) const double E1 = -(m1+m2)*g*L1; // auxiliary variable (pot energy 1) const double E2 = - m2 *g*L2; // auxiliary variable (pot energy 2) IntermediateState c1; IntermediateState c2; IntermediateState c3; IntermediateState T; IntermediateState V; IntermediateStateVector Q; // COMPUTE THE KINETIC ENERGY T AND THE POTENTIAL V: // ------------------------------------------------- c1 = cos(q(0)); c2 = cos(q(1)); c3 = cos(q(0)+q(1)); T = 0.5*J_11*dq(0)*dq(0) + 0.5*J_22*dq(1)*dq(1) + J_12*c3*dq(0)*dq(1); V = E1*c1 + E2*c2; Q = (-alpha*dq); // AUTOMATICALLY DERIVE THE EQUATIONS OF MOTION BASED ON THE LAGRANGIAN FORMALISM: // ------------------------------------------------------------------------------- DifferentialEquation f; LagrangianFormalism( f, T - V, Q, q, dq ); // Define an integrator: // --------------------- IntegratorBDF integrator( f ); // Define an initial value: // ------------------------ double x_start[4] = { 0.0, 0.5, 0.0, 0.1 }; double t_start = 0.0; double t_end = 3.0; // START THE INTEGRATION // ---------------------- integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM ); integrator.set( INTEGRATOR_TOLERANCE, 1e-12 ); integrator.freezeAll(); integrator.integrate( x_start, t_start, t_end ); VariablesGrid xres; integrator.getTrajectory(&xres,NULL,NULL,NULL,NULL,NULL); GnuplotWindow window; window.addSubplot( xres(0), "The excitation angle of pendulum 1" ); window.addSubplot( xres(1), "The excitation angle of pendulum 2" ); window.addSubplot( xres(2), "The angular velocity of pendulum 1" ); window.addSubplot( xres(3), "The angular velocity of pendulum 2" ); window.plot(); return 0; }
int main( ) { USING_NAMESPACE_ACADO // DIFFERENTIAL STATES : // ------------------------- DifferentialState r; // the length r of the cable DifferentialState phi; // the angle phi DifferentialState theta; // the angle theta // ------------------------- // ------------------------------------------- DifferentialState dr; // first derivative of r0 with respect to t DifferentialState dphi; // first derivative of phi with respect to t DifferentialState dtheta; // first derivative of theta with respect to t // ------------------------- // ------------------------------------------- DifferentialState n; // winding number // ------------------------- // ------------------------------------------- DifferentialState Psi; // the roll angle Psi DifferentialState CL; // the aerodynamic lift coefficient // ------------------------- // ------------------------------------------- DifferentialState W; // integral over the power at the generator // ( = ENERGY ) // CONTROL : // ------------------------- Control ddr0; // second derivative of r0 with respect to t Control dPsi; // first derivative of Psi with respect to t Control dCL; // first derivative of CL with respect to t // PARAMETERS : // ------------------------ // PARAMETERS OF THE KITE : // ----------------------------- double mk = 850.00; // mass of the kite // [ kg ] double A = 500.00; // effective area // [ m^2 ] double V = 720.00; // volume // [ m^3 ] double cd0 = 0.04; // aerodynamic drag coefficient // [ ] // ( cd0: without downwash ) double K = 0.04; // induced drag constant // [ ] // PHYSICAL CONSTANTS : // ----------------------------- double g = 9.81; // gravitational constant // [ m /s^2] double rho = 1.23; // density of the air // [ kg/m^3] // PARAMETERS OF THE CABLE : // ----------------------------- double rhoc = 1450.00; // density of the cable // [ kg/m^3] double cc = 1.00; // frictional constant // [ ] double dc = 0.05614; // diameter // [ m ] // PARAMETERS OF THE WIND : // ----------------------------- double w0 = 10.00; // wind velocity at altitude h0 // [ m/s ] double h0 = 100.00; // the altitude h0 // [ m ] double hr = 0.10; // roughness length // [ m ] // OTHER VARIABLES : // ------------------------ double AQ ; // cross sectional area IntermediateState mc; // mass of the cable IntermediateState m ; // effective inertial mass IntermediateState m_; // effective gravitational mass IntermediateState Cg; IntermediateState dm; // first derivative of m with respect to t // DEFINITION OF PI : // ------------------------ double PI = 3.1415926535897932; // ORIENTATION AND FORCES : // ------------------------ IntermediateState h ; // altitude of the kite IntermediateState w ; // the wind at altitude h IntermediateState we [ 3]; // effective wind vector IntermediateState nwe ; // norm of the effective wind vector IntermediateState nwep ; // - IntermediateState ewep [ 3]; // projection of ewe IntermediateState eta ; // angle eta: cf. [2] IntermediateState et [ 3]; // unit vector from the left to the right wing tip IntermediateState Caer ; IntermediateState Cf ; // simple constants IntermediateState CD ; // the aerodynamic drag coefficient IntermediateState Fg [ 3]; // the gravitational force IntermediateState Faer [ 3]; // the aerodynamic force IntermediateState Ff [ 3]; // the frictional force IntermediateState F [ 3]; // the total force // TERMS ON RIGHT-HAND-SIDE // OF THE DIFFERENTIAL // EQUATIONS : // ------------------------ IntermediateState a_pseudo [ 3]; // the pseudo accelaration IntermediateState dn ; // the time derivate of the kite's winding number IntermediateState ddr ; // second derivative of s with respect to t IntermediateState ddphi ; // second derivative of phi with respect to t IntermediateState ddtheta ; // second derivative of theta with respect to t IntermediateState power ; // the power at the generator // ------------------------ // ---------------------------------------------- IntermediateState regularisation ; // regularisation of controls // MODEL EQUATIONS : // =============================================================== // CROSS AREA OF THE CABLE : // --------------------------------------------------------------- AQ = PI*dc*dc/4.0 ; // THE EFECTIVE MASS' : // --------------------------------------------------------------- mc = rhoc*AQ*r ; // mass of the cable m = mk + mc / 3.0; // effective inertial mass m_ = mk + mc / 2.0; // effective gravitational mass // ----------------------------- // ---------------------------- dm = (rhoc*AQ/ 3.0)*dr; // time derivative of the mass // WIND SHEAR MODEL : // --------------------------------------------------------------- h = r*cos(theta) ; w = log(h/hr) / log(h0/hr) *w0 ; // EFFECTIVE WIND IN THE KITE`S SYSTEM : // --------------------------------------------------------------- we[0] = w*sin(theta)*cos(phi) - dr ; we[1] = -w*sin(phi) - r*sin(theta)*dphi ; we[2] = -w*cos(theta)*cos(phi) + r *dtheta; // CALCULATION OF THE KITE`S TRANSVERSAL AXIS : // --------------------------------------------------------------- nwep = pow( we[1]*we[1] + we[2]*we[2], 0.5 ); nwe = pow( we[0]*we[0] + we[1]*we[1] + we[2]*we[2], 0.5 ); eta = asin( we[0]*tan(Psi)/ nwep ) ; // --------------------------------------------------------------- // ewep[0] = 0.0 ; ewep[1] = we[1] / nwep ; ewep[2] = we[2] / nwep ; // --------------------------------------------------------------- et [0] = sin(Psi) ; et [1] = (-cos(Psi)*sin(eta))*ewep[1] - (cos(Psi)*cos(eta))*ewep[2]; et [2] = (-cos(Psi)*sin(eta))*ewep[2] + (cos(Psi)*cos(eta))*ewep[1]; // CONSTANTS FOR SEVERAL FORCES : // --------------------------------------------------------------- Cg = (V*rho-m_)*g ; Caer = (rho*A/2.0 )*nwe ; Cf = (rho*dc/8.0)*r*nwe ; // THE DRAG-COEFFICIENT : // --------------------------------------------------------------- CD = cd0 + K*CL*CL ; // SUM OF GRAVITATIONAL AND LIFTING FORCE : // --------------------------------------------------------------- Fg [0] = Cg * cos(theta) ; // Fg [1] = Cg * 0.0 ; Fg [2] = Cg * sin(theta) ; // SUM OF THE AERODYNAMIC FORCES : // --------------------------------------------------------------- Faer[0] = Caer*( CL*(we[1]*et[2]-we[2]*et[1]) + CD*we[0] ) ; Faer[1] = Caer*( CL*(we[2]*et[0]-we[0]*et[2]) + CD*we[1] ) ; Faer[2] = Caer*( CL*(we[0]*et[1]-we[1]*et[0]) + CD*we[2] ) ; // THE FRICTION OF THE CABLE : // --------------------------------------------------------------- // Ff [0] = Cf * cc* we[0] ; Ff [1] = Cf * cc* we[1] ; Ff [2] = Cf * cc* we[2] ; // THE TOTAL FORCE : // --------------------------------------------------------------- F [0] = Fg[0] + Faer[0] ; F [1] = Faer[1] + Ff[1] ; F [2] = Fg[2] + Faer[2] + Ff[2] ; // THE PSEUDO ACCELERATION: // --------------------------------------------------------------- a_pseudo [0] = - ddr0 + r*( dtheta*dtheta + sin(theta)*sin(theta)*dphi *dphi ) - dm/m*dr ; a_pseudo [1] = - 2.0*cos(theta)/sin(theta)*dphi*dtheta - 2.0*dr/r*dphi - dm/m*dphi ; a_pseudo [2] = cos(theta)*sin(theta)*dphi*dphi - 2.0*dr/r*dtheta - dm/m*dtheta ; // THE EQUATIONS OF MOTION: // --------------------------------------------------------------- ddr = F[0]/m + a_pseudo[0] ; ddphi = F[1]/(m*r*sin(theta)) + a_pseudo[1] ; ddtheta = -F[2]/(m*r ) + a_pseudo[2] ; // THE DERIVATIVE OF THE WINDING NUMBER : // --------------------------------------------------------------- dn = ( dphi*ddtheta - dtheta*ddphi ) / (2.0*PI*(dphi*dphi + dtheta*dtheta) ) ; // THE POWER AT THE GENERATOR : // --------------------------------------------------------------- power = m*ddr*dr ; // REGULARISATION TERMS : // --------------------------------------------------------------- regularisation = 5.0e2 * ddr0 * ddr0 + 1.0e8 * dPsi * dPsi + 1.0e5 * dCL * dCL + 2.5e5 * dn * dn + 2.5e7 * ddphi * ddphi; + 2.5e7 * ddtheta * ddtheta; + 2.5e6 * dtheta * dtheta; // --------------------------- // THE "RIGHT-HAND-SIDE" OF THE ODE: // --------------------------------------------------------------- DifferentialEquation f; f << dot(r) == dr ; f << dot(phi) == dphi ; f << dot(theta) == dtheta ; f << dot(dr) == ddr0 ; f << dot(dphi) == ddphi ; f << dot(dtheta) == ddtheta ; f << dot(n) == dn ; f << dot(Psi) == dPsi ; f << dot(CL) == dCL ; f << dot(W) == (-power + regularisation)*1.0e-6; // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( 0.0, 18.0, 18 ); ocp.minimizeMayerTerm( W ); ocp.subjectTo( f ); // INITIAL VALUE CONSTRAINTS: // --------------------------------- ocp.subjectTo( AT_START, n == 0.0 ); ocp.subjectTo( AT_START, W == 0.0 ); // PERIODIC BOUNDARY CONSTRAINTS: // ---------------------------------------- ocp.subjectTo( 0.0, r , -r , 0.0 ); ocp.subjectTo( 0.0, phi , -phi , 0.0 ); ocp.subjectTo( 0.0, theta , -theta , 0.0 ); ocp.subjectTo( 0.0, dr , -dr , 0.0 ); ocp.subjectTo( 0.0, dphi , -dphi , 0.0 ); ocp.subjectTo( 0.0, dtheta, -dtheta, 0.0 ); ocp.subjectTo( 0.0, Psi , -Psi , 0.0 ); ocp.subjectTo( 0.0, CL , -CL , 0.0 ); ocp.subjectTo( -0.34 <= phi <= 0.34 ); ocp.subjectTo( 0.85 <= theta <= 1.45 ); ocp.subjectTo( -40.0 <= dr <= 10.0 ); ocp.subjectTo( -0.29 <= Psi <= 0.29 ); ocp.subjectTo( 0.1 <= CL <= 1.50 ); ocp.subjectTo( -0.7 <= n <= 0.90 ); ocp.subjectTo( -25.0 <= ddr0 <= 25.0 ); ocp.subjectTo( -0.065 <= dPsi <= 0.065 ); ocp.subjectTo( -3.5 <= dCL <= 3.5 ); // CREATE A PLOT WINDOW AND VISUALIZE THE RESULT: // ---------------------------------------------- GnuplotWindow window; window.addSubplot( r, "CABLE LENGTH r [m]" ); window.addSubplot( phi, "POSITION ANGLE phi [rad]" ); window.addSubplot( theta,"POSITION ANGLE theta [rad]" ); window.addSubplot( Psi, "ROLL ANGLE psi [rad]" ); window.addSubplot( CL, "LIFT COEFFICIENT CL" ); window.addSubplot( W, "ENERGY W [MJ]" ); window.addSubplot( F[0], "FORCE IN CABLE [N]" ); window.addSubplot( phi,theta, "Kite Orbit","theta [rad]","phi [rad]" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.initializeDifferentialStates("powerkite_states.txt" ); algorithm.initializeControls ("powerkite_controls.txt" ); algorithm.set ( MAX_NUM_ITERATIONS, 100 ); algorithm.set ( KKT_TOLERANCE , 1e-2 ); algorithm << window; algorithm.set( PRINT_SCP_METHOD_PROFILE, YES ); algorithm.solve(); return 0; }
int main( ){ // Define a Right-Hand-Side: // ------------------------- DifferentialState x("", 4, 1), P("", 4, 4); Control u("", 2, 1); IntermediateState rhs = cstrModel( x, u ); DMatrix Q = zeros<double>(4,4); Q(0,0) = 0.2; Q(1,1) = 1.0; Q(2,2) = 0.5; Q(3,3) = 0.2; DMatrix R = zeros<double>(2,2); R(0,0) = 0.5; R(1,1) = 5e-7; DifferentialEquation f; f << dot(x) == rhs; f << dot(P) == getRiccatiODE( rhs, x, u, P, Q, R ); // Define an integrator: // --------------------- IntegratorRK45 integrator( f ); integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM ); integrator.set( PRINT_INTEGRATOR_PROFILE, YES ); // Define an initial value: // ------------------------ //double x_ss[4] = { 2.14, 1.09, 114.2, 112.9 }; double x_start[20] = { 1.0, 0.5, 100.0, 100.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 }; double u_start[2] = { 14.19, -1113.5 }; // double u_start[2] = { 10.00, -7000.0 }; Grid timeInterval( 0.0, 5000.0, 100 ); integrator.freezeAll(); integrator.integrate( timeInterval, x_start, 0 ,0, u_start ); // GET THE RESULTS // --------------- VariablesGrid differentialStates; integrator.getX( differentialStates ); DVector PP = differentialStates.getLastVector(); DMatrix PPP(4,4); for( int i=0; i<4; ++i ) for( int j=0; j<4; ++j ) PPP(i,j) = PP(4+i*4+j); PPP.print( "P1.txt","",PS_PLAIN ); // PPP.printToFile( "P2.txt","",PS_PLAIN ); GnuplotWindow window; window.addSubplot( differentialStates(0), "cA [mol/l]" ); window.addSubplot( differentialStates(1), "cB [mol/l]" ); window.addSubplot( differentialStates(2), "theta [C]" ); window.addSubplot( differentialStates(3), "thetaK [C]" ); window.addSubplot( differentialStates(4 ), "P11" ); window.addSubplot( differentialStates(9 ), "P22" ); window.addSubplot( differentialStates(14), "P33" ); window.addSubplot( differentialStates(19), "P44" ); window.plot(); return 0; }
int main( ) { USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState xB; //Body Position DifferentialState xW; //Wheel Position DifferentialState vB; //Body Velocity DifferentialState vW; //Wheel Velocity Control F; Disturbance R; double mB = 350.0; double mW = 50.0; double kS = 20000.0; double kT = 200000.0; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- DifferentialEquation f; f << dot(xB) == vB; f << dot(xW) == vW; f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB; f << dot(vW) == ( kS*xB - (kT+kS)*xW + kT*R - F ) / mW; // DEFINE LEAST SQUARE FUNCTION: // ----------------------------- Function h; h << xB; h << xW; h << vB; h << vW; h << F; DMatrix Q(5,5); // LSQ coefficient matrix Q(0,0) = 10.0; Q(1,1) = 10.0; Q(2,2) = 1.0; Q(3,3) = 1.0; Q(4,4) = 1.0e-8; DVector r(5); // Reference r.setAll( 0.0 ); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- const double tStart = 0.0; const double tEnd = 1.0; OCP ocp( tStart, tEnd, 20 ); ocp.minimizeLSQ( Q, h, r ); ocp.subjectTo( f ); ocp.subjectTo( -200.0 <= F <= 200.0 ); ocp.subjectTo( R == 0.0 ); // SETTING UP THE REAL-TIME ALGORITHM: // ----------------------------------- RealTimeAlgorithm alg( ocp,0.025 ); alg.set( MAX_NUM_ITERATIONS, 1 ); alg.set( PLOT_RESOLUTION, MEDIUM ); GnuplotWindow window; window.addSubplot( xB, "Body Position [m]" ); window.addSubplot( xW, "Wheel Position [m]" ); window.addSubplot( vB, "Body Velocity [m/s]" ); window.addSubplot( vW, "Wheel Velocity [m/s]" ); window.addSubplot( F, "Damping Force [N]" ); window.addSubplot( R, "Road Excitation [m]" ); alg << window; // SETUP CONTROLLER AND PERFORM A STEP: // ------------------------------------ StaticReferenceTrajectory zeroReference( "ref.txt" ); Controller controller( alg,zeroReference ); DVector y( 4 ); y.setZero( ); y(0) = 0.01; if (controller.init( 0.0,y ) != SUCCESSFUL_RETURN) exit( 1 ); if (controller.step( 0.0,y ) != SUCCESSFUL_RETURN) exit( 1 ); return EXIT_SUCCESS; }
int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- const int N = 2; DifferentialState x, y("", N, 1); Control u; DifferentialEquation f; const double t_start = 0.0; const double t_end = 10.0; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- f << dot(x) == -x + 0.9*x*x + u; int i; for( i = 0; i < N; i++ ) f << dot( y(i) ) == -y(i) + 0.5*y(i)*y(i) + u; // DEFINE LEAST SQUARE FUNCTION: // ----------------------------- Function h,m; h << x; h << 2.0*u; m << 10.0*x ; m << 0.1*x*x; DMatrix S(2,2); DVector r(2); S.setIdentity(); r.setAll( 0.1 ); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( t_start, t_end, 5 ); ocp.minimizeLSQ ( S, h, r ); ocp.minimizeLSQEndTerm( S, m, r ); ocp.subjectTo( f ); ocp.subjectTo( AT_START, x == 1.0 ); for( i = 0; i < N; i++ ) ocp.subjectTo( AT_START, y(i) == 1.0 ); // Additionally, flush a plotting object GnuplotWindow window; window.addSubplot( x,"DifferentialState x" ); window.addSubplot( u,"Control u" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm << window; // algorithm.set( PRINT_SCP_METHOD_PROFILE, YES ); // algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY_LIFTED ); // algorithm.set( HESSIAN_APPROXIMATION, CONSTANT_HESSIAN ); // algorithm.set( HESSIAN_APPROXIMATION, FULL_BFGS_UPDATE ); // algorithm.set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE ); algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); // algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON_WITH_BLOCK_BFGS ); // algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); // Necessary to use with GN Hessian approximation. algorithm.set( LEVENBERG_MARQUARDT, 1e-10 ); algorithm.solve(); return 0; }
int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState xB; DifferentialState xW; DifferentialState vB; DifferentialState vW; Control R; Control F; double mB = 350.0; double mW = 50.0; double kS = 20000.0; double kT = 200000.0; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- DifferentialEquation f; f << dot(xB) == vB; f << dot(xW) == vW; f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB; f << dot(vW) == ( kS*xB - (kT+kS)*xW + kT*R - F ) / mW; // DEFINE LEAST SQUARE FUNCTION: // ----------------------------- Function h; h << xB; h << xW; h << vB; h << vW; Matrix Q(4,4); Q.setIdentity(); Q(0,0) = 10.0; Q(1,1) = 10.0; Vector r(4); r.setAll( 0.0 ); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- const double t_start = 0.0; const double t_end = 1.0; OCP ocp( t_start, t_end, 20 ); ocp.minimizeLSQ( Q, h, r ); ocp.subjectTo( f ); ocp.subjectTo( -500.0 <= F <= 500.0 ); ocp.subjectTo( R == 0.0 ); // SETTING UP THE (SIMULATED) PROCESS: // ----------------------------------- OutputFcn identity; DynamicSystem dynamicSystem( f,identity ); Process process( dynamicSystem,INT_RK45 ); VariablesGrid disturbance = readFromFile( "road.txt" ); process.setProcessDisturbance( disturbance ); // SETTING UP THE MPC CONTROLLER: // ------------------------------ RealTimeAlgorithm alg( ocp,0.05 ); alg.set( MAX_NUM_ITERATIONS, 2 ); StaticReferenceTrajectory zeroReference; Controller controller( alg,zeroReference ); // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE... // ---------------------------------------------------------- SimulationEnvironment sim( 0.0,3.0,process,controller ); Vector x0(4); x0(0) = 0.01; x0(1) = 0.0; x0(2) = 0.0; x0(3) = 0.0; sim.init( x0 ); sim.run( ); // ...AND PLOT THE RESULTS // ---------------------------------------------------------- VariablesGrid sampledProcessOutput; sim.getSampledProcessOutput( sampledProcessOutput ); VariablesGrid feedbackControl; sim.getFeedbackControl( feedbackControl ); GnuplotWindow window; window.addSubplot( sampledProcessOutput(0), "Body Position [m]" ); window.addSubplot( sampledProcessOutput(1), "Wheel Position [m]" ); window.addSubplot( sampledProcessOutput(2), "Body Velocity [m/s]" ); window.addSubplot( sampledProcessOutput(3), "Wheel Velocity [m/s]" ); window.addSubplot( feedbackControl(1), "Damping Force [N]" ); window.addSubplot( feedbackControl(0), "Road Excitation [m]" ); window.plot( ); return 0; }
int main( ){ double clock1 = clock(); double t_start = 0.0; double t_end = 600.0; int intervals = 5 ; int i; TIME t; DifferentialState x("", NXD, 1); AlgebraicState z("", NXA, 1); Control u("", NU, 1); Parameter p("", NP, 1); IntermediateState is(1+NXD+NXA+NU+NP); is(0) = t; for (i=0; i < NXD; ++i) is(1+i) = x(i); for (i=0; i < NXA; ++i) is(1+NXD+i) = z(i); for (i=0; i < NU; ++i) is(1+NXD+NXA+i) = u(i); for (i=0; i < NP; ++i) is(1+NXD+NXA+NU+i) = p(i); CFunction hydroscalModel( NXD+NXA, ffcn_model ); // Define a Right-Hand-Side: // ------------------------- DifferentialEquation f; f << hydroscalModel(is); // DEFINE INITIAL VALUES: // ---------------------- double xd[NXD] = { 2.1936116177990631E-01, // X_0 3.3363028623863722E-01, // X_1 3.7313133250625952E-01, // X_2 3.9896472354654333E-01, // X_3 4.1533719381260475E-01, // X_4 4.2548399372287182E-01, // X_5 4.3168379354213621E-01, // X_6 4.3543569751236455E-01, // X_7 4.3768918647214428E-01, // X_8 4.3903262905928286E-01, // X_9 4.3982597315656735E-01, // X_10 4.4028774979047969E-01, // X_11 4.4055002518902953E-01, // X_12 4.4069238917008052E-01, // X_13 4.4076272408112094E-01, // X_14 4.4078980543461005E-01, // X_15 4.4079091412311144E-01, // X_16 4.4077642312834125E-01, // X_17 4.4075255679998443E-01, // X_18 4.4072304911231042E-01, // X_19 4.4069013958173919E-01, // X_20 6.7041926189645151E-01, // X_21 7.3517997375758948E-01, // X_22 7.8975978943631409E-01, // X_23 8.3481725159539033E-01, // X_24 8.7125377077380739E-01, // X_25 9.0027275078767721E-01, // X_26 9.2312464536394301E-01, // X_27 9.4096954980798608E-01, // X_28 9.5481731262797742E-01, // X_29 9.6551271145368878E-01, // X_30 9.7374401773010488E-01, // X_31 9.8006186072166701E-01, // X_32 9.8490109485675337E-01, // X_33 9.8860194771099286E-01, // X_34 9.9142879342008328E-01, // X_35 9.9358602331847468E-01, // X_36 9.9523105632238640E-01, // X_37 9.9648478785701988E-01, // X_38 9.9743986301741971E-01, // X_39 9.9816716097314861E-01, // X_40 9.9872084014280071E-01, // X_41 3.8633811956730968E+00, // n_1 3.9322260498028840E+00, // n_2 3.9771965626392531E+00, // n_3 4.0063070333869728E+00, // n_4 4.0246026844143410E+00, // n_5 4.0358888958821835E+00, // n_6 4.0427690398786789E+00, // n_7 4.0469300433477020E+00, // n_8 4.0494314648020326E+00, // n_9 4.0509267560029381E+00, // n_10 4.0518145583397631E+00, // n_11 4.0523364846379799E+00, // n_12 4.0526383977460299E+00, // n_13 4.0528081437632766E+00, // n_14 4.0528985491134542E+00, // n_15 4.0529413510270169E+00, // n_16 4.0529556049324462E+00, // n_17 4.0529527471448805E+00, // n_18 4.0529396392278008E+00, // n_19 4.0529203970496912E+00, // n_20 3.6071164950918582E+00, // n_21 3.7583754503438387E+00, // n_22 3.8917148481441974E+00, // n_23 4.0094300698741563E+00, // n_24 4.1102216725798293E+00, // n_25 4.1944038520620675E+00, // n_26 4.2633275166560596E+00, // n_27 4.3188755452109175E+00, // n_28 4.3630947909857642E+00, // n_29 4.3979622247841386E+00, // n_30 4.4252580012497740E+00, // n_31 4.4465128947193868E+00, // n_32 4.4630018314791968E+00, // n_33 4.4757626150015568E+00, // n_34 4.4856260094946823E+00, // n_35 4.4932488551808500E+00, // n_36 4.4991456959629330E+00, // n_37 4.5037168116896273E+00, // n_38 4.5072719605639726E+00, // n_39 4.5100498969782414E+00 // n_40 }; double ud[ NU] = { 4.1833910982822058E+00, // L_vol 2.4899344742988991E+00 // Q }; double pd[ NP] = { 1.5458567140000001E-01, // n^{ref}_{tray} = 0.155 l 1.7499999999999999E-01, // (not in use?) 3.4717208398678062E-01, // \alpha_{rect} = 35 % 6.1895708603484367E-01, // \alpha_{strip} = 62 % 1.6593025789999999E-01, // W_{tray} = 0.166 l^{-.5} s^{.1} 5.0695122527590109E-01, // Q_{loss} = 0.51 kW 8.5000000000000000E+00, // n^v_0 = 8.5 l 1.7000000000000001E-01, // n^v_{N+1} = 0.17 l 9.3885430857029321E+04, // P_{top} = 939 h Pa 2.5000000000000000E+02, // \Delta P_{strip}= 2.5 h Pa and \Delta P_{rect} = 1.9 h Pa 1.4026000000000000E+01, // F_{vol} = 14.0 l h^{-1} 3.2000000000000001E-01, // X_F = 0.32 7.1054000000000002E+01, // T_F = 71 oC 4.7163089489100003E+01, // T_C = 47.2 oC 4.1833910753991770E+00, // (not in use?) 2.4899344810136301E+00, // (not in use?) 1.8760537088149468E+02 // (not in use?) }; DVector x0(NXD, xd); DVector p0(NP, pd); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( t_start, t_end, intervals ); // LSQ Term on temperature deviations and controls Function h; // for( i = 0; i < NXD; i++ ) // h << 0.001*x(i); h << 0.1 * ( z(94) - 88.0 ); // Temperature tray 14 h << 0.1 * ( z(108) - 70.0 ); // Temperature tray 28 h << 0.01 * ( u(0) - ud[0] ); // L_vol h << 0.01 * ( u(1) - ud[1] ); // Q ocp.minimizeLSQ( h ); // W.r.t. differential equation ocp.subjectTo( f ); // Fix states ocp.subjectTo( AT_START, x == x0 ); // Fix parameters ocp.subjectTo( p == p0 ); // Path constraint on controls ocp.subjectTo( ud[0] - 2.0 <= u(0) <= ud[0] + 2.0 ); ocp.subjectTo( ud[1] - 2.0 <= u(1) <= ud[1] + 2.0 ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.initializeAlgebraicStates("hydroscal_algebraic_states.txt"); algorithm.set( INTEGRATOR_TYPE, INT_BDF ); algorithm.set( MAX_NUM_ITERATIONS, 5 ); algorithm.set( KKT_TOLERANCE, 1e-3 ); algorithm.set( INTEGRATOR_TOLERANCE, 1e-4 ); algorithm.set( ABSOLUTE_TOLERANCE , 1e-6 ); algorithm.set( PRINT_SCP_METHOD_PROFILE, YES ); algorithm.set( LINEAR_ALGEBRA_SOLVER, SPARSE_LU ); algorithm.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); //algorithm.set( LEVENBERG_MARQUARDT, 1e-3 ); algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY_LIFTED ); //algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY ); //algorithm.set( CONSTRAINT_SENSITIVITY, FORWARD_SENSITIVITY ); //algorithm.set( ALGEBRAIC_RELAXATION,ART_EXPONENTIAL ); //results in an extra step but steps are quicker algorithm.solve(); double clock2 = clock(); printf("total computation time = %.16e \n", (clock2-clock1)/CLOCKS_PER_SEC ); // PLOT RESULTS: // --------------------------------------------------- VariablesGrid out_states; algorithm.getDifferentialStates( out_states ); out_states.print( "OUT_states.m","STATES",PS_MATLAB ); VariablesGrid out_controls; algorithm.getControls( out_controls ); out_controls.print( "OUT_controls.m","CONTROLS",PS_MATLAB ); VariablesGrid out_algstates; algorithm.getAlgebraicStates( out_algstates ); out_algstates.print( "OUT_algstates.m","ALGSTATES",PS_MATLAB ); GnuplotWindow window; window.addSubplot( out_algstates(94), "Temperature tray 14" ); window.addSubplot( out_algstates(108), "Temperature tray 28" ); window.addSubplot( out_controls(0), "L_vol" ); window.addSubplot( out_controls(1), "Q" ); window.plot( ); return 0; }
int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4; // x, y, z : position // vx, vy, vz : linear velocity // phi, theta, psi : orientation (Yaw-Pitch-Roll = Euler(3,2,1)) // p, q, r : angular velocity // u1, u2, u3, u4 : velocity of the propellers Control vu1,vu2,vu3,vu4; // vu1, vu2, vu3, vu4 : derivative of u1, u2, u3, u4 DifferentialEquation f; // Quad constants const double c = 0.00001; const double Cf = 0.00065; const double d = 0.250; const double Jx = 0.018; const double Jy = 0.018; const double Jz = 0.026; const double Im = 0.0001; const double m = 0.9; const double g = 9.81; const double Cx = 0.1; // Minimization Weights double coeffX = .00001; double coeffU = coeffX*0.0001;//0.000000000000001; double coeffX2 = coeffX * 100.; double coeffX3 = coeffX * 0.00001; double coeffO = -coeffX * 0.1; // final position (used in the cost function) double xf = 0., yf = 0., zf = 20.; // double T = 8.; //length (in second) of the trajectory predicted in the MPC int nb_nodes = 20.; //number of nodes used in the Optimal Control Problem // 20 nodes means that the algorithm will discretized the trajectory equally into 20 pieces // If you increase the number of node, the solution will be more precise but calculation will take longer (~nb_nodes^2) // In ACADO, the commands are piecewise constant functions, constant between each node. double tmpc = 0.2; //time (in second) between two activation of the MPC algorithm // DEFINE A OPTIMAL CONTROL PROBLEM // ------------------------------- OCP ocp( 0.0, T, nb_nodes ); // DEFINE THE COST FUNCTION // ------------------------------- Function h, hf; h << x; h << y; h << z; h << vu1; h << vu2; h << vu3; h << vu4; h << p; h << q; h << r; hf << x; hf << y; hf << z; DMatrix Q(10,10), Qf(3,3); Q(0,0) = coeffX; Q(1,1) = coeffX; Q(2,2) = coeffX; Q(3,3) = coeffU; Q(4,4) = coeffU; Q(5,5) = coeffU; Q(6,6) = coeffU; Q(7,7) = coeffX2; Q(8,8) = coeffX2; Q(9,9) = coeffX2; Qf(0,0) = 1.; Qf(1,1) = 1.; Qf(2,2) = 1.; DVector reff(3), ref(10); ref(0) = xf; ref(1) = yf; ref(2) = zf; ref(3) = 58.; ref(4) = 58.; ref(5) = 58.; ref(6) = 58.; ref(7) = 0.; ref(8) = 0.; ref(9) = 0.; reff(0) = xf; reff(1) = yf; reff(2) = zf; // The cost function is define as : integrale from 0 to T { transpose(h(x,u,t)-ref)*Q*(h(x,u,t)-ref) } + transpose(hf(x,u,T))*Qf*hf(x,u,T) // == integrale cost (also called running cost or Lagrange Term) + final cost (Mayer Term) ocp.minimizeLSQ ( Q, h, ref); // ocp.minimizeLSQEndTerm(Qf, hf, reff); // When doing MPC, you need terms in the cost function to stabilised the system => p, q, r and vu1, vu2, vu3, vu4. You can check that if you reduce their weights "coeffX2" or "coeffU" too low, the optimization will crashed. // DEFINE THE DYNAMIC EQUATION OF THE SYSTEM: // ---------------------------------- f << dot(x) == vx; f << dot(y) == vy; f << dot(z) == vz; f << dot(vx) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m; f << dot(vy) == -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(psi)*cos(theta)/m; f << dot(vz) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(psi)*cos(theta)/m - g; f << dot(phi) == -cos(phi)*tan(theta)*p+sin(phi)*tan(theta)*q+r; f << dot(theta) == sin(phi)*p+cos(phi)*q; f << dot(psi) == cos(phi)/cos(theta)*p-sin(phi)/cos(theta)*q; f << dot(p) == (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx; f << dot(q) == (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy; f << dot(r) == (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz; f << dot(u1) == vu1; f << dot(u2) == vu2; f << dot(u3) == vu3; f << dot(u4) == vu4; // ---------------------------- DEFINE CONTRAINTES --------------------------------- // //Dynamic ocp.subjectTo( f ); //State constraints //Constraints on the velocity of each propeller ocp.subjectTo( 16 <= u1 <= 95 ); ocp.subjectTo( 16 <= u2 <= 95 ); ocp.subjectTo( 16 <= u3 <= 95 ); ocp.subjectTo( 16 <= u4 <= 95 ); //Command constraints //Constraints on the acceleration of each propeller ocp.subjectTo( -100 <= vu1 <= 100 ); ocp.subjectTo( -100 <= vu2 <= 100 ); ocp.subjectTo( -100 <= vu3 <= 100 ); ocp.subjectTo( -100 <= vu4 <= 100 ); #if AVOID_SINGULARITIES == 1 //Constraint to avoid singularity // In this example I used Yaw-Pitch-Roll convention to describe orientation of the quadrotor // when using Euler Angles representation, you always have a singularity, and we need to // avoid it otherwise the algorithm will crashed. ocp.subjectTo( -1. <= theta <= 1.); #endif //Example of Eliptic obstacle constraints (here, cylinders with eliptic basis) ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-5)*(z-5)) ); ocp.subjectTo( 16 <= ((x-3)*(x-3)+2*(z-9)*(z-9)) ); ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-15)*(z-15)) ); // SETTING UP THE (SIMULATED) PROCESS: // ----------------------------------- OutputFcn identity; // The next line define the equation used to simulate the system : // here "f" is used (=the same as the one used for the ocp) so the trajectory simulated // from the commands will be exactly the same as the one calculated by the MPC. // If for instance you want to test robusness, you can define an other dynamic equation "f2" // with changed parameters and put it here. DynamicSystem dynamicSystem( f,identity ); Process process( dynamicSystem,INT_RK45 ); // SETTING UP THE MPC CONTROLLER: // ------------------------------ RealTimeAlgorithm alg( ocp, tmpc ); //Usually, you do only one step of the optimisation algorithm (~Gauss-Newton here) //at each activation of the MPC, that way the delay between getting the state and //sending a command is as quick as possible. alg.set( MAX_NUM_ITERATIONS, 1 ); StaticReferenceTrajectory zeroReference; Controller controller( alg,zeroReference ); // SET AN INITIAL GUESS FOR THE FIRST MPC LOOP (NEXT LOOPS WILL USE AS INITIAL GUESS THE SOLUTION FOUND AT THE PREVIOUS MPC LOOP) Grid timeGrid(0.0,T,nb_nodes+1); VariablesGrid x_init(16, timeGrid); // init with static for (int i = 0 ; i<nb_nodes+1 ; i++ ) { x_init(i,0) = 0.; x_init(i,1) = 0.; x_init(i,2) = 0.; x_init(i,3) = 0.; x_init(i,4) = 0.; x_init(i,5) = 0.; x_init(i,6) = 0.; x_init(i,7) = 0.; x_init(i,8) = 0.; x_init(i,9) = 0.; x_init(i,10) = 0.; x_init(i,11) = 0.; x_init(i,12) = 58.; //58. is the propeller rotation speed so the total thrust balance the weight of the quad x_init(i,13) = 58.; x_init(i,14) = 58.; x_init(i,15) = 58.; } alg.initializeDifferentialStates(x_init); // SET OPTION AND PLOTS WINDOW // --------------------------- // Linesearch is an algorithm which will try several points along the descent direction to choose a better step length. // It looks like activating this option produice more stable trajectories.198 alg.set( GLOBALIZATION_STRATEGY, GS_LINESEARCH ); alg.set(INTEGRATOR_TYPE, INT_RK45); // You can uncomment those lines to see how the predicted trajectory involve along time // (but be carefull because you will have 1 ploting window per MPC loop) // GnuplotWindow window1(PLOT_AT_EACH_ITERATION); // window1.addSubplot( z,"DifferentialState z" ); // window1.addSubplot( x,"DifferentialState x" ); // window1.addSubplot( theta,"DifferentialState theta" ); // window1.addSubplot( 16./((x+3)*(x+3)+4*(z-5)*(z-5)),"Dist obs1" ); // window1.addSubplot( 16./((x-3)*(x-3)+4*(z-9)*(z-9)),"Dist obs2" ); // window1.addSubplot( 16./((x+2)*(x+2)+4*(z-15)*(z-15)),"Dist obs3" ); // alg<<window1; // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE... // ---------------------------------------------------------- // The first argument is the starting time, the second the end time. SimulationEnvironment sim( 0.0,10.,process,controller ); //Setting the state of the sytem at the beginning of the simulation. DVector x0(16); x0.setZero(); x0(0) = 0.; x0(12) = 58.; x0(13) = 58.; x0(14) = 58.; x0(15) = 58.; t = clock(); if (sim.init( x0 ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); if (sim.run( ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); t = clock() - t; std::cout << "total time : " << (((float)t)/CLOCKS_PER_SEC)<<std::endl; // ...SAVE THE RESULTS IN FILES // ---------------------------------------------------------- std::ofstream file; file.open("/tmp/log_state.txt",std::ios::out); std::ofstream file2; file2.open("/tmp/log_control.txt",std::ios::out); VariablesGrid sampledProcessOutput; sim.getSampledProcessOutput( sampledProcessOutput ); sampledProcessOutput.print(file); VariablesGrid feedbackControl; sim.getFeedbackControl( feedbackControl ); feedbackControl.print(file2); // ...AND PLOT THE RESULTS // ---------------------------------------------------------- GnuplotWindow window; window.addSubplot( sampledProcessOutput(0), "x " ); window.addSubplot( sampledProcessOutput(1), "y " ); window.addSubplot( sampledProcessOutput(2), "z " ); window.addSubplot( sampledProcessOutput(6),"phi" ); window.addSubplot( sampledProcessOutput(7),"theta" ); window.addSubplot( sampledProcessOutput(8),"psi" ); window.plot( ); graphics::corbaServer::ClientCpp client = graphics::corbaServer::ClientCpp(); client.createWindow("window"); return 0; }
/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // Define a Right-Hand-Side: // ------------------------- DifferentialState x; DifferentialEquation f; TIME t; f << dot(x) == -x + sin(0.01*t); // Define an initial value: // ------------------------ Vector xStart( 1 ); xStart(0) = 1.0; double tStart = 0.0; double tEnd = 1000.0; Grid timeHorizon( tStart,tEnd,2 ); Grid timeGrid( tStart,tEnd,20 ); // Define an integration algorithm: // -------------------------------- IntegrationAlgorithm intAlg; intAlg.addStage( f, timeHorizon ); intAlg.set( INTEGRATOR_TYPE, INT_BDF ); intAlg.set( INTEGRATOR_PRINTLEVEL, MEDIUM ); intAlg.set( INTEGRATOR_TOLERANCE, 1.0e-3 ); intAlg.set( PRINT_INTEGRATOR_PROFILE, YES ); intAlg.set( PLOT_RESOLUTION, HIGH ); GnuplotWindow window; window.addSubplot( x,"x" ); intAlg << window; // START THE INTEGRATION // ---------------------- intAlg.integrate( timeHorizon, xStart ); // GET THE RESULTS // --------------- VariablesGrid differentialStates; intAlg.getX( differentialStates ); differentialStates.print( "x" ); Vector xEnd; intAlg.getX( xEnd ); xEnd.print( "xEnd" ); return 0; }
int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------ DifferentialState x; Control u; Disturbance w; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- DifferentialEquation f, f2; f << dot(x) == -2.0*x + u; f2 << dot(x) == -2.0*x + u + 0.1*w; //- 0.000000000001*x*x // DEFINE LEAST SQUARE FUNCTION: // ----------------------------- Function h; h << x; h << u; Matrix Q(2,2); Q.setIdentity(); Vector r(2); r.setAll( 0.0 ); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- const double t_start = 0.0; const double t_end = 7.0; OCP ocp ( t_start, t_end, 14 ); ocp.minimizeLSQ( Q, h, r ); ocp.subjectTo ( f ); ocp.subjectTo( -1.0 <= u <= 2.0 ); //ocp.subjectTo( w == 0.0 ); // SETTING UP THE (SIMULATED) PROCESS: // ----------------------------------- OutputFcn identity; DynamicSystem dynamicSystem( f2,identity ); Process process( dynamicSystem,INT_RK45 ); VariablesGrid disturbance = fopen( "my_disturbance.txt", "r" ); // GnuplotWindow window2; // window2.addSubplot( disturbance, "my disturbance" ); // window2.plot(); process.setProcessDisturbance( disturbance ); // SETUP OF THE ALGORITHM AND THE TUNING OPTIONS: // ---------------------------------------------- double samplingTime = 0.5; RealTimeAlgorithm algorithm( ocp,samplingTime ); // // algorithm.set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE ); algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); // // // algorithm.set( ABSOLUTE_TOLERANCE , 1e-7 ); // // algorithm.set( INTEGRATOR_TOLERANCE, 1e-9 ); // // algorithm.set( KKT_TOLERANCE, 1e-4 ); algorithm.set( MAX_NUM_ITERATIONS,1 ); algorithm.set( USE_REALTIME_SHIFTS, YES ); // algorithm.set( USE_REALTIME_ITERATIONS,NO ); // algorithm.set( TERMINATE_AT_CONVERGENCE,YES ); // algorithm.set( PRINTLEVEL,HIGH ); Vector x0(1); x0(0) = 1.0; // // algorithm.solve( x0 ); // // GnuplotWindow window1; // window1.addSubplot( x, "DIFFERENTIAL STATE: x" ); // window1.addSubplot( u, "CONTROL: u" ); // window1.plot(); // // return 0; // SETTING UP THE NMPC CONTROLLER: // ------------------------------- VariablesGrid myReference = fopen( "my_reference.txt", "r" ); PeriodicReferenceTrajectory reference( myReference ); // GnuplotWindow window3; // window3.addSubplot( myReference(1), "my reference" ); // window3.plot(); Controller controller( algorithm,reference ); // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE... // ---------------------------------------------------------- double simulationStart = 0.0; double simulationEnd = 15.0; SimulationEnvironment sim( simulationStart, simulationEnd, process, controller ); sim.init( x0 ); sim.run( ); // ...AND PLOT THE RESULTS // ---------------------------------------------------------- VariablesGrid sampledProcessOutput; sim.getSampledProcessOutput( sampledProcessOutput ); VariablesGrid feedbackControl; sim.getFeedbackControl( feedbackControl ); GnuplotWindow window; window.addSubplot( sampledProcessOutput(0), "DIFFERENTIAL STATE: x" ); window.addSubplot( feedbackControl(0), "CONTROL: u" ); window.plot(); return 0; }
int main( ){ counter = 0; // DIFFERENTIAL STATES : // ------------------------- DifferentialState r; // the length r of the cable DifferentialState phi; // the angle phi DifferentialState theta; // the angle theta // ------------------------- // ------------------------------------------- DifferentialState dr; // first derivative of r0 with respect to t DifferentialState dphi; // first derivative of phi with respect to t DifferentialState dtheta; // first derivative of theta with respect to t // ------------------------- // ------------------------------------------- DifferentialState n; // winding number // ------------------------- // ------------------------------------------- DifferentialState Psi; // the roll angle Psi DifferentialState CL; // the aerodynamic lift coefficient // ------------------------- // ------------------------------------------- DifferentialState W; // integral over the power at the generator // ( = ENERGY ) // CONTROL : // ------------------------- Control ddr0; // second derivative of r0 with respect to t Control dPsi; // first derivative of Psi with respect to t Control dCL; // first derivative of CL with respect to t // THE "RIGHT-HAND-SIDE" OF THE ODE: // --------------------------------------------------------------- DifferentialEquation f; const int NX = 10; const int NU = 3 ; IntermediateState x( NX + NU ); x(0) = r; x(1) = phi; x(2) = theta; x(3) = dr; x(4) = dphi; x(5) = dtheta; x(6) = n; x(7) = Psi; x(8) = CL; x(9) = W; x(10) = ddr0; x(11) = dPsi; x(12) = dCL; CFunction kiteModel( 10, ffcn_model ); f << kiteModel( x ); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( 0.0, 18.0, 18 ); ocp.minimizeMayerTerm( W ); ocp.subjectTo( f ); // INITIAL VALUE CONSTRAINTS: // --------------------------------- ocp.subjectTo( AT_START, n == 0.0 ); ocp.subjectTo( AT_START, W == 0.0 ); // PERIODIC BOUNDARY CONSTRAINTS: // ---------------------------------------- ocp.subjectTo( 0.0, r , -r , 0.0 ); ocp.subjectTo( 0.0, phi , -phi , 0.0 ); ocp.subjectTo( 0.0, theta , -theta , 0.0 ); ocp.subjectTo( 0.0, dr , -dr , 0.0 ); ocp.subjectTo( 0.0, dphi , -dphi , 0.0 ); ocp.subjectTo( 0.0, dtheta, -dtheta, 0.0 ); ocp.subjectTo( 0.0, Psi , -Psi , 0.0 ); ocp.subjectTo( 0.0, CL , -CL , 0.0 ); ocp.subjectTo( -0.34 <= phi <= 0.34 ); ocp.subjectTo( 0.85 <= theta <= 1.45 ); ocp.subjectTo( -40.0 <= dr <= 10.0 ); ocp.subjectTo( -0.29 <= Psi <= 0.29 ); ocp.subjectTo( 0.1 <= CL <= 1.50 ); ocp.subjectTo( -0.7 <= n <= 0.90 ); ocp.subjectTo( -25.0 <= ddr0 <= 25.0 ); ocp.subjectTo( -0.065 <= dPsi <= 0.065 ); ocp.subjectTo( -3.5 <= dCL <= 3.5 ); // CREATE A PLOT WINDOW AND VISUALIZE THE RESULT: // ---------------------------------------------- GnuplotWindow window; window.addSubplot( r, "CABLE LENGTH r [m]" ); window.addSubplot( phi, "POSITION ANGLE phi [rad]" ); window.addSubplot( theta,"POSITION ANGLE theta [rad]" ); window.addSubplot( Psi, "ROLL ANGLE psi [rad]" ); window.addSubplot( CL, "LIFT COEFFICIENT CL" ); window.addSubplot( W, "ENERGY W [MJ]" ); window.addSubplot( phi,theta, "Kite Orbit","theta [rad]","phi [rad]" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.initializeDifferentialStates("powerkite_states.txt" ); algorithm.initializeControls ("powerkite_controls.txt" ); algorithm.set ( MAX_NUM_ITERATIONS, 100 ); algorithm.set ( KKT_TOLERANCE , 1e-2 ); algorithm << window; algorithm.set( PRINT_SCP_METHOD_PROFILE, YES ); algorithm.solve(); return 0; }
/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ---------------------------- DifferentialState x(10); // a differential state vector with dimension 10. (vector) DifferentialState y ; // another differential state y (scalar) Control u(2 ); // a control input with dimension 2. (vector) Parameter p ; // a parameter (here a scalar). (scalar) DifferentialEquation f ; // the differential equation const double t_start = 0.0; const double t_end = 1.0; // READ A MATRIX "A" FROM A FILE: // ------------------------------ Matrix A = readFromFile( "matrix_vector_ocp_A.txt" ); Matrix B = readFromFile( "matrix_vector_ocp_B.txt" ); // READ A VECTOR "x0" FROM A FILE: // ------------------------------- Vector x0 = readFromFile( "matrix_vector_ocp_x0.txt" ); // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- f << dot(x) == -(A*x) + B*u; // matrix vector notation for a linear equation f << dot(y) == x.transpose()*x + 2.0*u.transpose()*u; // matrix vector notation: x^x = scalar product = ||x||_2^2 // u^u = scalar product = ||u||_2^2 // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( t_start, t_end, 20 ); ocp.minimizeMayerTerm( y ); ocp.subjectTo( f ); ocp.subjectTo( AT_START, x == x0 ); ocp.subjectTo( AT_START, y == 0.0 ); GnuplotWindow window; window.addSubplot( x(0),"x0" ); window.addSubplot( x(6),"x6" ); window.addSubplot( u(0),"u0" ); window.addSubplot( u(1),"u1" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.set( MAX_NUM_ITERATIONS, 20 ); algorithm.set( KKT_TOLERANCE, 1e-10 ); algorithm << window; algorithm.solve(); return 0; }
int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState s,v,m; Control u ; Parameter T ; DifferentialEquation f( 0.0, T ); // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- f << dot(s) == v; f << dot(v) == (u-0.2*v*v)/m; f << dot(m) == -0.01*u*u; // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( 0, T, 20 ); ocp.minimizeMayerTerm( T ); ocp.subjectTo( f ); ocp.subjectTo( AT_START, s == 0.0 ); ocp.subjectTo( AT_START, v == 0.0 ); ocp.subjectTo( AT_START, m == 1.0 ); ocp.subjectTo( AT_END , s == 10.0 ); ocp.subjectTo( AT_END , v == 0.0 ); ocp.subjectTo( -0.1 <= v <= 1.7 ); ocp.subjectTo( -1.1 <= u <= 1.1 ); ocp.subjectTo( 5.0 <= T <= 15.0 ); // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW: // ------------------------------------------ GnuplotWindow window; window.addSubplot( s, "THE DISTANCE s" ); window.addSubplot( v, "THE VELOCITY v" ); window.addSubplot( m, "THE MASS m" ); window.addSubplot( u, "THE CONTROL INPUT u" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.set( MAX_NUM_ITERATIONS, 20 ); algorithm << window; // algorithm.initializeDifferentialStates("tor_states.txt"); // algorithm.initializeParameters("tor_pars.txt"); // algorithm.initializeControls("tor_controls.txt"); algorithm.solve(); // algorithm.getDifferentialStates("tor_states.txt"); // algorithm.getParameters("tor_pars.txt"); // algorithm.getControls("tor_controls.txt"); return 0; }
/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // DEFINE VALRIABLES: // --------------------------- DifferentialStateVector x(3); // the position of the pendulum (x,y,alpha) DifferentialStateVector v(3); // the associated velocities AlgebraicStateVector a(3); // the associated accelerations AlgebraicStateVector lambda(2); // the constraint forces const double L = 1.00; // the length of the pendulum const double m = 1.00; // the mass of the pendulum const double g = 9.81; // the gravitational constant const double J = m*L*L; // the inertial of the pendulum IntermediateStateVector R(3); IntermediateStateVector G(2); R.setComponent( 0, m*a(0) ); // ---------------------------------------- R.setComponent( 1, m*a(1) + m*g ); // the definition of the force residuum: R.setComponent( 2, J*a(2) ); // R := m*a - F G.setComponent( 0, x(0)-L*sin(x(2)) ); // definition of the constraint manifold G G.setComponent( 1, x(1)+L*cos(x(2)) ); // --------------------------------------- // AUTOMATIC GENERATION OF AN INDEX 1 DAE SYSTEM BASES ON THE // NEWTON EULER FORMALISM: // ----------------------------------------------------------- DifferentialEquation f; NewtonEulerFormalism( f, R, G, x, v, a, lambda ); // Define an integrator: // --------------------- IntegratorBDF integrator( f ); // Define an initial value: // ------------------------ double x_start[6]; double z_start[5]; x_start[0] = 1.9866932270683099e-01; x_start[1] = -9.8006654611577315e-01; x_start[2] = 2.0000003107582773e-01; x_start[3] = -1.4519963562050693e-04; x_start[4] = 4.7104175041346282e-04; x_start[5] = 4.4177521668741377e-04; z_start[0] = -9.5504866367984165e-01; z_start[1] = -1.9359778029074531e-01; z_start[2] = -9.7447321693831934e-01; z_start[3] = -9.5504866367984165e-01; z_start[4] = 9.6164022197092560e+00; double t_start = 0.0; double t_end = 10.0; // START THE INTEGRATION // ---------------------- integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM ); // integrator.set( INTEGRATOR_TOLERANCE, 1e-12 ); integrator.freezeAll(); integrator.integrate( x_start, z_start, t_start, t_end ); VariablesGrid xres,zres; integrator.getTrajectory(&xres,&zres,NULL,NULL,NULL,NULL); GnuplotWindow window; window.addSubplot( xres(0), "The x-position of the mass m" ); window.addSubplot( xres(1), "The y-position of the mass m" ); window.addSubplot( xres(2), "The excitation angle of the pendulum" ); window.addSubplot( xres(3), "The velocity in x-direction" ); window.addSubplot( xres(4), "The velocity in y-direction" ); window.addSubplot( xres(5), "The angular velocity" ); // window.addSubplot( zres(0), "The acceleration in x-direction" ); // window.addSubplot( zres(1), "The acceleration in y-direction" ); // window.addSubplot( zres(2), "The angular acceleration" ); window.addSubplot( zres(3), "The constraint force in x-direction" ); window.addSubplot( zres(4), "The constraint force in y-direction" ); window.plot(); return 0; }
/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState v,s,m; Control u ; DifferentialEquation f ; const double t_start = 0.0; const double t_end = 10.0; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- f << dot(s) == v; f << dot(v) == (u-0.02*v*v)/m; f << dot(m) == -0.01*u*u; // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( t_start, t_end, 20 ); ocp.minimizeLagrangeTerm( u*u ); ocp.subjectTo( f ); ocp.subjectTo( AT_START, s == 0.0 ); ocp.subjectTo( AT_START, v == 0.0 ); ocp.subjectTo( AT_START, m == 1.0 ); ocp.subjectTo( AT_END , s == 10.0 ); ocp.subjectTo( AT_END , v == 0.0 ); ocp.subjectTo( -0.01 <= v <= 1.3 ); ocp.subjectTo( u*u >= -1.0 ); // DEFINE A PLOT WINDOW: // --------------------- GnuplotWindow window; window.addSubplot( s,"DifferentialState s" ); window.addSubplot( v,"DifferentialState v" ); window.addSubplot( m,"DifferentialState m" ); window.addSubplot( u,"Control u" ); window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" ); // window.addSubplot( 0.5 * m * v*v,"Kinetic Energy" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); // algorithm.set( INTEGRATOR_TYPE, INT_BDF ); // algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 ); // algorithm.set( KKT_TOLERANCE, 1e-3 ); //algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY ); algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); algorithm.set( MAX_NUM_ITERATIONS, 20 ); algorithm.set( KKT_TOLERANCE, 1e-10 ); // algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 4 ); algorithm << window; algorithm.solve(); // BlockMatrix sens; // algorithm.getSensitivitiesX( sens ); // sens.print(); return 0; }
int main( ){ USING_NAMESPACE_ACADO; // Parameters double h_hw = 10; // water level double A_hw = 1.0; // amplitude of the waves double T_hw = 5.0; // duration of a wave double h_b = 3.0; // height of the buoy double rho = 1000; // density of water double A = 1.0; // bottom area of the buoy double m = 100; // mass of the buoy double g = 9.81; // gravitational constant // Free parameter Control u; // Variables DifferentialState h; // Position of the buoy DifferentialState v; // Velocity of the buoy DifferentialState w; // Produced wave energy TIME t; // Differential equation DifferentialEquation f; // Height of the wave IntermediateState hw; hw = h_hw + A_hw*sin(2*M_PI*t/T_hw); f << dot(h) == v; f << dot(v) == rho*A*(hw-h)/m - g - u; f << dot(w) == u*v; // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- const double t_start = 0.0 ; const double t_end = 15; OCP ocp( t_start, t_end, 100 ); ocp.maximizeMayerTerm( w ); ocp.subjectTo( f ); // double x_start[3]; // x_start[0] = h_hw - 0*A_hw; // x_start[1] = 0; // x_start[2] = 0; ocp.subjectTo( AT_START, h - (h_hw-A_hw) == 0.0 ); ocp.subjectTo( AT_START, v == 0.0 ); ocp.subjectTo( AT_START, w == 0.0 ); ocp.subjectTo( -h_b <= h-hw <= 0.0 ); ocp.subjectTo( 0.0 <= u <= 100.0 ); // DEFINE A PLOT WINDOW: // --------------------- GnuplotWindow window; window.addSubplot( h,"Height of buoy" ); window.addSubplot( v,"Velocity of buoy" ); window.addSubplot( w,"Objective function " ); window.addSubplot( u,"Resistance" ); window.addSubplot( hw,"Wave height" ); // window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); algorithm.set( MAX_NUM_ITERATIONS, 100 ); // algorithm.set( KKT_TOLERANCE, 1e-10 ); algorithm << window; algorithm.solve(); return 0; }
int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState x; DifferentialState l; AlgebraicState z; Control u; DifferentialEquation f; // Disturbance R; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- f << dot(x) == -x + 0.5*x*x + u + 0.5*z ; f << dot(l) == x*x + 3.0*u*u ; f << 0 == z + exp(z) - 1.0 + x ; // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( 0.0, 5.0, 10 ); ocp.minimizeMayerTerm( l ); ocp.subjectTo( f ); // ocp.subjectTo( R == 0.0 ); // SETTING UP THE (SIMULATED) PROCESS: // ----------------------------------- OutputFcn identity; DynamicSystem dynamicSystem( f,identity ); Process process( dynamicSystem,INT_BDF ); //VariablesGrid disturbance = readFromFile( "dae_simulation_disturbance.txt" ); //process.setProcessDisturbance( disturbance ); // SETTING UP THE MPC CONTROLLER: // ------------------------------ RealTimeAlgorithm alg( ocp,0.5 ); StaticReferenceTrajectory zeroReference; Controller controller( alg,zeroReference ); // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE... // ---------------------------------------------------------- SimulationEnvironment sim( 0.0,15.0,process,controller ); Vector x0(2); x0(0) = 1; x0(1) = 0; sim.init( x0 ); sim.run( ); // ...AND PLOT THE RESULTS // ---------------------------------------------------------- VariablesGrid diffStates; sim.getProcessDifferentialStates( diffStates ); diffStates.printToFile( "diffStates.txt" ); diffStates.printToFile( "diffStates.m","DIFFSTATES",PS_MATLAB ); VariablesGrid sampledProcessOutput; sim.getSampledProcessOutput( sampledProcessOutput ); sampledProcessOutput.printToFile( "sampledOut.txt" ); sampledProcessOutput.printToFile( "sampledOut.m","OUT",PS_MATLAB ); VariablesGrid feedbackControl; sim.getFeedbackControl( feedbackControl ); feedbackControl.printToFile( "controls.txt" ); feedbackControl.printToFile( "controls.m","CONTROL",PS_MATLAB ); VariablesGrid algStates; sim.getProcessAlgebraicStates( algStates ); algStates.printToFile( "algStates.txt" ); algStates.printToFile( "algStates.m","ALGSTATES",PS_MATLAB ); GnuplotWindow window; window.addSubplot( diffStates(0), "DIFFERENTIAL STATE: x" ); window.addSubplot( diffStates(1), "DIFFERENTIAL STATE: l" ); window.addSubplot( algStates(0), "ALGEBRAIC STATE: z" ); window.addSubplot( feedbackControl(0), "CONTRUL: u" ); window.plot( ); return 0; }
int main( ){ USING_NAMESPACE_ACADO // Define a Right-Hand-Side: // ------------------------- DifferentialState x, y; DifferentialEquation f; f << dot(x) == y; f << dot(y) == -x; // Define an integrator: // --------------------- IntegratorRK45 integrator( f ); integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM ); integrator.set( PRINT_INTEGRATOR_PROFILE, YES ); // Define an initial value: // ------------------------ double x_start[2] = { 0.0, 1.0 }; Grid timeInterval( 0.0, 2.0*M_PI, 100 ); integrator.freezeAll(); integrator.integrate( timeInterval, x_start ); // GET THE RESULTS // --------------- VariablesGrid differentialStates; integrator.getX( differentialStates ); GnuplotWindow window; window.addSubplot( differentialStates(0) ); window.addSubplot( differentialStates(1) ); window.plot(); // Vector seed(2); // // seed( 0 ) = 1.0; // seed( 1 ) = 0.0; // // integrator.setForwardSeed( 1, seed ); // integrator.integrateSensitivities(); // // VariablesGrid sens; // integrator.getForwardSensitivities( sens, 1 ); // // GnuplotWindow window2; // window2.addSubplot( sens(0) ); // window2.addSubplot( sens(1) ); // window2.plot(); return 0; }
int main( ){ USING_NAMESPACE_ACADO // DEFINE A RIGHT-HAND-SIDE: // ------------------------- DifferentialState x; AlgebraicState z; Parameter p,q; IntermediateState is(4); is(0) = x; is(1) = z; is(2) = p; is(3) = q; CFunction simpledaeModel( 2, ffcn_model ); // Define a Right-Hand-Side: // ------------------------- DifferentialEquation f; f << simpledaeModel(is); // DEFINE AN INTEGRATOR: // --------------------- IntegratorBDF integrator(f); // DEFINE INITIAL VALUES: // ---------------------- double x0 = 1.0; double z0 = 1.000000; double pp[2] = { 1.0, 1.0 }; Grid interval( 0.0, 1.0, 100 ); // START THE INTEGRATION: // ---------------------- integrator.integrate( interval, &x0, &z0, pp ); VariablesGrid differentialStates; VariablesGrid algebraicStates ; VariablesGrid intermediateStates; integrator.getX ( differentialStates ); integrator.getXA( algebraicStates ); integrator.getI ( intermediateStates ); GnuplotWindow window; window.addSubplot( differentialStates(0) ); window.addSubplot( algebraicStates (0) ); window.plot(); return 0; }
int main( ) { USING_NAMESPACE_ACADO // DEFINE THE VARIABLES: // ---------------------------------------------------------- DifferentialState p ; // the trolley position DifferentialState v ; // the trolley velocity DifferentialState phi ; // the excitation angle DifferentialState omega; // the angular velocity Control a ; // the acc. of the trolley const double g = 9.81; // the gravitational constant const double b = 0.20; // the friction coefficient // ---------------------------------------------------------- // DEFINE THE MODEL EQUATIONS: // ---------------------------------------------------------- DifferentialEquation f; f << dot( p ) == v ; f << dot( v ) == a ; f << dot( phi ) == omega ; f << dot( omega ) == -g*sin(phi) - a*cos(phi) - b*omega; // ---------------------------------------------------------- // SETTING UP THE (SIMULATED) PROCESS: // ----------------------------------- OutputFcn identity; DynamicSystem dynamicSystem( f,identity ); Process process( dynamicSystem,INT_RK45 ); // SETTING UP THE MPC CONTROLLER: // ------------------------------ ExportedRTIscheme rtiScheme( 4,1, 10, 0.3 ); #ifdef USE_CVXGEN set_defaults( ); #endif Vector xuRef(5); xuRef.setZero( ); VariablesGrid reference; reference.addVector( xuRef, 0.0 ); reference.addVector( xuRef, 10.0 ); StaticReferenceTrajectory referenceTrajectory( reference ); Controller controller( rtiScheme,referenceTrajectory ); controller.set( USE_REFERENCE_PREDICTION,NO ); // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE... // ---------------------------------------------------------- SimulationEnvironment sim( 0.0,10.0, process,controller ); Vector x0(4); x0(0) = 1.0; x0(1) = 0.0; x0(2) = 0.0; x0(3) = 0.0; sim.init( x0 ); sim.run( ); // ... AND PLOT THE RESULTS // ------------------------ VariablesGrid diffStates; sim.getProcessDifferentialStates( diffStates ); VariablesGrid feedbackControl; sim.getFeedbackControl( feedbackControl ); GnuplotWindow window; window.addSubplot( diffStates(0), "p" ); window.addSubplot( diffStates(1), "v" ); window.addSubplot( diffStates(2), "phi" ); window.addSubplot( diffStates(3), "omega" ); window.addSubplot( feedbackControl(0), "a" ); window.plot( ); return 0; }
int main( ) { USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState xB; //Body Position DifferentialState xW; //Wheel Position DifferentialState vB; //Body Velocity DifferentialState vW; //Wheel Velocity Disturbance R; Control F; double mB = 350.0; double mW = 50.0; double kS = 20000.0; double kT = 200000.0; // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- DifferentialEquation f; f << dot(xB) == vB; f << dot(xW) == vW; f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB; f << dot(vW) == ( kS*xB - (kT+kS)*xW + kT*R - F ) / mW; // SETTING UP THE (SIMULATED) PROCESS: // ----------------------------------- OutputFcn identity; DynamicSystem dynamicSystem( f,identity ); Process process( dynamicSystem,INT_RK45 ); VariablesGrid disturbance = readFromFile( "road.txt" ); if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- Function h; h << xB; h << xW; h << vB; h << vW; h << F; Matrix Q = zeros(5,5); // LSQ coefficient matrix Q(0,0) = 10.0; Q(1,1) = 10.0; Q(2,2) = 1.0; Q(3,3) = 1.0; Q(4,4) = 1.0e-8; Vector r(5); // Reference r.setAll( 0.0 ); const double tStart = 0.0; const double tEnd = 1.0; OCP ocp( tStart, tEnd, 20 ); ocp.minimizeLSQ( Q, h, r ); ocp.subjectTo( f ); ocp.subjectTo( -200.0 <= F <= 200.0 ); ocp.subjectTo( R == 0.0 ); // SETTING UP THE MPC CONTROLLER: // ------------------------------ RealTimeAlgorithm alg( ocp,0.05 ); alg.set( INTEGRATOR_TYPE, INT_RK78 ); alg.set( DYNAMIC_SENSITIVITY,FORWARD_SENSITIVITY ); // alg.set( MAX_NUM_ITERATIONS, 2 ); // alg.set( USE_IMMEDIATE_FEEDBACK,YES ); StaticReferenceTrajectory zeroReference; Controller controller( alg,zeroReference ); // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE... // ---------------------------------------------------------- SimulationEnvironment sim( 0.0,2.5,process,controller ); Vector x0(4); x0.setZero(); if (sim.init( x0 ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); if (sim.run( ) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE ); // ... AND PLOT THE RESULTS // ------------------------ VariablesGrid diffStates; sim.getProcessDifferentialStates( diffStates ); VariablesGrid feedbackControl; sim.getFeedbackControl( feedbackControl ); GnuplotWindow window; window.addSubplot( diffStates(0), "Body Position [m]" ); window.addSubplot( diffStates(1), "Wheel Position [m]" ); window.addSubplot( diffStates(2), "Body Velocity [m/s]" ); window.addSubplot( diffStates(3), "Wheel Velocity [m/s]" ); window.addSubplot( feedbackControl, "Damping Force [N]" ); window.addSubplot( disturbance, "Road Excitation [m]" ); window.plot( ); return EXIT_SUCCESS; }