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