bool hitTriang(Photon *photon, Triang *triang, const float distance, const Material *materials) { Shader shader = DIFFUSE; // Better idea? if (fDot(triang->direct, photon->direct) > -EPSILON) triang->direct = vNeg(triang->direct); // Update ray's position to the point where the ray hits the sphere photon->origin = fFMA(photon->direct, distance - UM(10), photon->origin); photon->lambda = materials[triang->mID].lambda; if (shader == CRAZY) { photon->direct = vNorm(vSub(newVec(0.99, 0, 0), photon->origin)); return REFLECTED; } else if (shader == SPECULAR) { // Angle between new and old vector is twice the angle between old vector and normal Vec reflect = vDot(photon->direct, triang->direct); photon->direct = vSub(photon->direct, vMul(triang->direct, vAdd(reflect, reflect))); return REFLECTED; } else if (shader == DIFFUSE) { photon->direct = randDir(); if (fDot(photon->direct, triang->direct) < EPSILON) photon->direct = vNeg(photon->direct); return REFLECTED; } else return ABSORBED; }
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; }
//=============================================== // Unit Tests //=============================================== int main() { // set for command line argument -t to run unit tests. bool test = true; if (test) { std::cout<<"\n=================\nBegin UNIT TESTS\n=================\n\n"; Light::Light n = *new Light::Light(0, 1, 1, 0, 0.5, 0.5, 0.5); n.print(); Intersect::Intersect i = *new Intersect::Intersect(); if(i.isHit()) { std::cout<<"INCORRECT! hit is true.\n\n"; } else { std::cout<<"CORRECT! hit is false.\n\n"; i.setHit(true); } if(i.isHit()) { std::cout<<"CORRECT! hit is true.\n\n"; } else { std::cout<<"INCORRECT! hit is false.\n\n"; } std::vector<float> ptest(3); ptest[0] = 0; ptest[1] = 1; ptest[2] = 2; i.setPoint(ptest); std::vector<float> p = i.getPoint(); std::cout<< "[" << p[0] << ", " << p[1] << ", " << p[2] << "]\n"; Vertex::Vertex a = *new Vertex::Vertex(0,0,0); Vertex::Vertex b = *new Vertex::Vertex(1,0,0); Vertex::Vertex c = *new Vertex::Vertex(0,1,0); a.print(); a = a.sub(b); a.print(); std::vector<float> d = c.toVec(); std::cout<<"vec ["<< d[0] << ", " << d[1] << ", " << d[2] << "]\n"; Vertex::Vertex e1 = *new Vertex::Vertex(d); e1.print(); Vertex a1 = *new Vertex::Vertex(0,1,0); Vertex b1 = *new Vertex::Vertex(1,-1,0); Vertex c1 = *new Vertex::Vertex(-1,-1,0); Triangle t = *new Triangle::Triangle(a1,b1,c1); p[0] = 0; p[1] = 0; p[2] = 0; std::vector<float> e(3); e[0] = 0; e[1] = 0; e[2] = 3; Ray::Ray r = *new Ray::Ray(e, p); i = t.intersect(r); if (i.isHit()) { std::cout<<"Triangle Intersected - OKAY\n"; } Sphere::Sphere s = *new Sphere::Sphere(1, p); if (s.intersect(r).isHit()) { std::cout<<"Sphere Intersected - OKAY\n"; } e[0] = 0; e[1] = -4; e[2] = -4; p[0] = 1; p[1] = 1; p[2] = -1; r.setEye(e); r.setPoint(p); std::cout<<"projected "; vPrint(r.project(1.5)); PPM* ppm = new PPM(640, 480, 255); int val; for (float h = 0; h<480; h++) { for (float w =0; w<640; w++) { val = h;//std::min(255, (int)((w/639)*255.0f)); ppm->addPixel(*(new Pixel::Pixel(val, val, val))); } } ppm->save("output"); /* for (float h = 0; h < 3; h++) { for (float w = 0; w< ppm->getW(); w++) { std::cout<<w<<" "; ppm->getPixel(w, h).print(); } } */ std::cout<<"End PPM tests\n"; Pixel::Pixel px = *new Pixel::Pixel(255, 255, 255); Pixel::Pixel black = *new Pixel::Pixel(0,0,0); Pixel::Pixel pfloat = *new Pixel::Pixel(1.0f, 0.9f, 0.05f); Pixel::Pixel pmax = *new Pixel::Pixel(2.0f, 1.0f, 0.5f); px.print(); black.print(); black.add(px); black.print(); pfloat.print(); pmax.print(); std::vector<float> test1(3); test1[0] = 1; test1[1] = 0; test1[2] = 0.5; std::vector<float> test2(3); test2[0] = 0; test2[1] = 1; test2[2] = 0.5; std::cout<<vDot(test1, test2)<<"\n"; vPrint(vSub(test1, test2)); vPrint(vAdd(test1, test2)); vPrint(vMult(test1, test2)); vPrint(normalize(test1)); vPrint(normalize(test2)); vPrint(vCross(test1, test2)); vPrint(vScale(-1, test1)); std::cout<<"\n=================\nEnd UNIT TESTS\n=================\n\n"; return 0; } else { return 0; } }