Beispiel #1
0
// 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);

}
Beispiel #2
0
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();
}
}
}
}
Beispiel #3
0
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;
}