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