// luminosity distance
static PyObject*
PyCosmoObject_Dl(struct PyCosmoObject* self, PyObject* args) {
double zmin, zmax;
double d;
if (!PyArg_ParseTuple(args, (char*)"dd", &zmin, &zmax)) {
return NULL;
}
d = Dl(self->cosmo, zmin, zmax);
return PyFloat_FromDouble(d);
}
void main()
{
DDRA=255;
DDRB=255;
DDRD=0xf0;
while(1)
{
PORTD=0xef;
_delay_ms(10);
if(((PIND&0x01)==0x00)||c==1)
{
while((PIND&0x0f)==0x0f)
{
c=1;
Al();
}
}
else if(((PIND&0x02)==0x00)||c==2)
{
while((PIND&0x0f)==0x0f)
{
c=2;
Bl();
}
}
else if(((PIND&0x04)==0x00)||c==3)
{
while((PIND&0x0f)==0x0f)
{
c=3;
Cl();
}
}
else if(((PIND&0x08)==0x00)||(c==4))
{
while((PIND&0x0f)==0x0f)
{
c=4;
Dl();
}
}
}
}
static PyObject*
PyCosmoObject_Dl_vec2(struct PyCosmoObject* self, PyObject* args) {
PyObject* zmaxObj=NULL, *resObj=NULL;;
double zmin, *zmax, *res;
npy_intp n, i;
if (!PyArg_ParseTuple(args, (char*)"dO", &zmin, &zmaxObj)) {
return NULL;
}
n = PyArray_SIZE(zmaxObj);
zmax = (double* )PyArray_DATA(zmaxObj);
resObj = PyArray_ZEROS(1, &n, NPY_FLOAT64, 0);
res = (double* )PyArray_DATA(resObj);
for (i=0; i<n; i++) {
res[i] = Dl(self->cosmo, zmin, zmax[i]);
}
return resObj;
}
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;
}
REAL DistMod(REAL x){
REAL res;
res = 5.0*log10(Dl(x)*(Mpc/h)/pc/10.0);
return res;
}
