int main () { double A[DIM][DIM], B[DIM][DIM]; Vfrandom(DIM*DIM, A[0]); Minv (DIM, A[0], B[0]); S3pr("\nA = %M\n ", A[0]); S3pr("B = %M\n ", B[0]); return(0); } /* end main() */
/*! solve */ int32_t gmres ( const CPPL::dgsmatrix& A, const CPPL::dcovector& b, CPPL::dcovector& x, const double& eps ) { /////////////////////////////////////////////// //////////////// preconditioner /////////////// /////////////////////////////////////////////// CPPL::dgbmatrix Minv(x.l, x.l, 0, 0); //////// no precondition //////// Minv.identity(); /////////////////////////////////////////////// ///////////////// mid values ////////////////// /////////////////////////////////////////////// long m(10);//restart number CPPL::dcovector r(b-A*x); CPPL::dcovector s(m+1), co(m+1), si(m+1), w; std::vector<CPPL::dcovector> v(m+1); CPPL::dgematrix H(m+1,m); //H.zero(); //co.zero(); //si.zero(); //s.zero(); //////// norm //////// double norm_r, norm_r_min(DBL_MAX); const double norm_r_ini(fabs(damax(r))); std::cerr << "[NOTE]@gmres: norm_r_ini=" << norm_r_ini << ", eps=" << eps<< std::endl; if( norm_r_ini<DBL_MIN ){ std::cerr << "[NOTE]@gmres: already converged. v(^^)" << std::endl; return 0; } /////////////////////////////////////////////// //////////////////// loop ///////////////////// /////////////////////////////////////////////// int itc(1); //int itmax(int(2.1*x.l)); int itmax(int(1.1*x.l)); //int itmax(int(0.6*x.l)); do{ std::cerr << "** itc=" << itc << " ********************************************" << std::endl; //////// 0 //////// v[0] =r/nrm2(r); s.zero(); s(0) =nrm2(r); for(long i=0; i<m; i++){ //std::cerr << "++++ i=" << i << " ++++" << std::endl; w =A*v[i]; w =Minv*w; for(long k=0; k<i+1; k++){ H(k,i) =w%v[k]; w -=H(k,i)*v[k]; } H(i+1,i) =nrm2(w); v[i+1] =w/H(i+1,i); //// J,s //// for(long k=0; k<i; k++){ rotate(H(k,i), H(k+1,i), co(k), si(k)); } make_rotator( H(i,i), H(i+1,i), co(i), si(i) ); //std::cerr << "co = " << t(co) << std::endl; std::cerr << "si = " << t(si) << std::endl; rotate( H(i,i), H(i+1,i), co(i), si(i) );//necessary //std::cerr << "H =\n" << H << std::endl; rotate( s(i), s(i+1), co(i), si(i) ); //std::cerr << "s = " << t(s) << std::endl; } //for(long i=0; i<m+1; i++){ for(long j=i+1; j<m+1; j++){ std::cerr << "vv = " << v[i]%v[j] << std::endl; } }// v check //std::cerr << "H =\n" << H << std::endl; //std::cerr << "s =" << t(s) << std::endl; //for(long i=0; i<m+1; i++){ std::cerr << "v["<<i<<"] =" << t(v[i]) << std::flush; } //////// y //////// CPPL::dcovector y(s); for(long i=m-1; i>=0; i--){ y(i) /= H(i,i); for(long j=i-1; j>=0; j--){ y(j) -= H(j,i) * y(i); } } //std::cerr << "H*y = " << t(H*y) << std::endl; //std::cerr << "s = " << t(s) << std::endl; //std::cerr << "y = " << t(s) << std::endl; //////// update //////// for(long i=0; i<m; i++){ x += v[i] * y(i); } //std::cerr << "x = " << t(x) << std::endl; //////// residual //////// r =b-A*x; r =Minv*r; //std::cerr << "r = " << t(r) << std::endl; //////// convergence check //////// norm_r =fabs(damax(r)); std::cerr << "norm_r = " << norm_r << std::endl; if( isnan(norm_r) ){ break; }//failed if( !std::isnormal(norm_r) ){ break; }//failed if( !std::isfinite(norm_r) ){ break; }//failed if( norm_r>1e3*norm_r_ini ){ break; }//failed (getting so worse) if( norm_r<=eps ){//r satistied std::cerr << "[NOTE]@gmres: converged. v(^^) itc=" << itc << "/" << itmax << ", norm=" << norm_r << std::endl; return 0; } }while(++itc<itmax); //////// failed //////// std::cerr << "[NOTE]@gmres: itc=" << itc << ", norm=" << norm_r << ", r_satisfied=" << (norm_r<=eps) << std::endl; std::cerr << "[NOTE]@gmres: failed to converge. orz" << std::endl; return 1; }
int main(int argc, char* argv[]) { if(argc < 2) { std::cerr << "You should provide a system as argument, either gazebo or flat_fish." << std::endl; return 1; } //======================================================================================== // SETTING VARIABLES //======================================================================================== std::string system = argv[1]; double weight; double buoyancy; std::string dataFolder; if(system == "gazebo") { weight = 4600; buoyancy = 4618; dataFolder = "./config/gazebo/"; } else if(system == "flatfish" || system == "flat_fish") { weight = 2800; buoyancy = 2812; dataFolder = "./config/flatfish/"; } else { std::cerr << "Unknown system " << system << "." << std::endl; return 1; } double uncertainty = 0; /* true - Export the code to the specified folder * false - Simulates the controller inside Acado's environment */ std::string rockFolder = "/home/rafaelsaback/rock/1_dev/control/uwv_model_pred_control/src"; /*********************** * CONTROLLER SETTINGS ***********************/ // Prediction and control horizon double horizon = 2; double sampleTime = 0.1; int iteractions = horizon / sampleTime; bool positionControl = false; //======================================================================================== // DEFINING VARIABLES //======================================================================================== DifferentialEquation f; // Variable that will carry 12 ODEs (6 for position and 6 for velocity) DifferentialState v("Velocity", 6, 1); // Velocity States DifferentialState n("Pose", 6, 1); // Pose States DifferentialState tau("Efforts", 6, 1); // Effort Control tauDot("Efforts rate", 6, 1); // Effort rate of change DVector rg(3); // Center of gravity DVector rb(3); // Center of buoyancy DMatrix M(6, 6); // Inertia matrix DMatrix Minv(6, 6); // Inverse of inertia matrix DMatrix Dl(6, 6); // Linear damping matrix DMatrix Dq(6, 6); // Quadratic damping matrix // H-representation of the output domain's polyhedron DMatrix AHRep(12,6); DMatrix BHRep(12,1); Expression linearDamping(6, 6); // Dl*v Expression quadraticDamping(6, 6); // Dq*|v|*v Expression gravityBuoyancy(6); // g(n) Expression absV(6, 6); // |v| Expression Jv(6); // J(n)*v Expression vDot(6); // AUV Fossen's equation Function J; // Cost function Function Jn; // Terminal cost function AHRep = readVariable("./config/", "a_h_rep.dat"); BHRep = readVariable("./config/", "b_h_rep.dat"); M = readVariable(dataFolder, "m_matrix.dat"); Dl = readVariable(dataFolder, "dl_matrix.dat"); Dq = readVariable(dataFolder, "dq_matrix.dat"); /*********************** * LEAST-SQUARES PROBLEM ***********************/ // Cost Functions if (positionControl) { J << n; Jn << n; } else { J << v; Jn << v; } J << tauDot; // Weighting Matrices for simulational controller DMatrix Q = eye<double>(J.getDim()); DMatrix QN = eye<double>(Jn.getDim()); Q = readVariable("./config/", "q_matrix.dat"); //======================================================================================== // MOTION MODEL EQUATIONS //======================================================================================== rg(0) = 0; rg(1) = 0; rg(2) = 0; rb(0) = 0; rb(1) = 0; rb(2) = 0; M *= (2 -(1 + uncertainty)); Dl *= 1 + uncertainty; Dq *= 1 + uncertainty; SetAbsV(absV, v); Minv = M.inverse(); linearDamping = Dl * v; quadraticDamping = Dq * absV * v; SetGravityBuoyancy(gravityBuoyancy, n, rg, rb, weight, buoyancy); SetJv(Jv, v, n); // Dynamic Equation vDot = Minv * (tau - linearDamping - quadraticDamping - gravityBuoyancy); // Differential Equations f << dot(v) == vDot; f << dot(n) == Jv; f << dot(tau) == tauDot; //======================================================================================== // OPTIMAL CONTROL PROBLEM (OCP) //======================================================================================== OCP ocp(0, horizon, iteractions); // Weighting Matrices for exported controller BMatrix Qexp = eye<bool>(J.getDim()); BMatrix QNexp = eye<bool>(Jn.getDim()); ocp.minimizeLSQ(Qexp, J); ocp.minimizeLSQEndTerm(QNexp, Jn); ocp.subjectTo(f); // Individual degrees of freedom constraints ocp.subjectTo(tau(3) == 0); ocp.subjectTo(tau(4) == 0); /* Note: If the constraint for Roll is not defined the MPC does not * work since there's no possible actuation in that DOF. * Always consider Roll equal to zero. */ // Polyhedron set of inequalities ocp.subjectTo(AHRep*tau - BHRep <= 0); //======================================================================================== // CODE GENERATION //======================================================================================== // Export code to ROCK OCPexport mpc(ocp); int Ni = 1; mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON); mpc.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING); mpc.set(INTEGRATOR_TYPE, INT_RK4); mpc.set(NUM_INTEGRATOR_STEPS, iteractions * Ni); mpc.set(HOTSTART_QP, YES); mpc.set(QP_SOLVER, QP_QPOASES); mpc.set(GENERATE_TEST_FILE, NO); mpc.set(GENERATE_MAKE_FILE, NO); mpc.set(GENERATE_MATLAB_INTERFACE, NO); mpc.set(GENERATE_SIMULINK_INTERFACE, NO); mpc.set(CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); mpc.set(CG_HARDCODE_CONSTRAINT_VALUES, YES); if (mpc.exportCode(rockFolder.c_str()) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE); mpc.printDimensionsQP(); return EXIT_SUCCESS; }