// [1/5/2009 zhangxiang]
void sgMeshCone::init(void){
/* if(m_fRadius <= 0 || m_fHeight <= 0 || m_iSlices < 2){
THROW_SAGI_EXCEPT(sgException::ERR_INVALID_STATE,
"Invalid parameters for the cone",
"sgMeshCone::init");
}
*/
Vector3 *pPosData = static_cast<Vector3*>(m_pVertexData->createElement(sgVertexBufferElement::VertexAttributeName, RDT_F, 3, m_iVertexNum)->data());
size_t *pIndexData = static_cast<size_t*>(m_pIndexData->createElement(sgVertexBufferElement::ET_VERTEX)->data());
// setup vertices
pPosData[0] = Vector3::ZERO;
Quaternion Dq(Math::PI_X_2 / m_iSlices, Vector3(0.0, 1.0, 0.0));
Vector3 o(0.0, 0.0, m_fRadius);
uInt index = 1;
for(int i=0; i<m_iSlices; i++){
pPosData[index++] = o;
// m_pVertexList->push_back(o);
o = Dq * o;
}
pPosData[index] = Vector3(0.0, m_fHeight, 0.0);
// setup faces
size_t faceNum = m_iSlices + m_iSlices;
size_t lowCenter = 0;
size_t highCenter = index; //static_cast<size_t>(m_pVertexList->size() - 1);
int slice = 1;
size_t ii = 0;
for(size_t i=0; i<faceNum; ++i){
int nextSlice = slice + 1 <= m_iSlices ? slice + 1 : 1;
pIndexData[ii++] = lowCenter;
pIndexData[ii++] = nextSlice;
pIndexData[ii++] = slice;
++i;
pIndexData[ii++] = highCenter;
pIndexData[ii++] = slice;
pIndexData[ii++] = nextSlice;
++slice;
}
//setupNormals();
// setupEdgeNormal(); // for future ...
prepareGeometry();
}
// [1/5/2009 zhangxiang]
void sgMeshSphere::init(void){
/*
if(m_fRadius <= 0 || m_iSlices < 2 || m_iStacks <= 0){
THROW_SAGI_EXCEPT(sgException::ERR_INVALID_STATE,
"Invalid parameters for the sphere.",
"sgMeshSphere::init");
}
*/
Vector3 *pPosData = static_cast<Vector3*>(m_pVertexData->createElement(sgVertexBufferElement::VertexAttributeName, RDT_F, 3, m_iVertexNum)->data());
size_t *pIndexData = static_cast<size_t*>(m_pIndexData->createElement(sgVertexBufferElement::ET_VERTEX)->data());
Real RX2 = m_fRadius + m_fRadius;
Real RUp2 = m_fRadius * m_fRadius;
Real Dy = RX2 / (m_iStacks + 1);
Quaternion Dq(Math::PI_X_2 / m_iSlices, Vector3(0.0, 1.0, 0.0));
size_t index = 0;
std::vector< std::vector<size_t> > indexedStacks;
std::vector<size_t> indexedStack;
// vertices
pPosData[0] = Vector3(0.0, -m_fRadius, 0.0);
indexedStack.clear();
for(int i=0; i<m_iSlices; i++){
indexedStack.push_back(index);
}
indexedStacks.push_back(indexedStack);
index++;
Real y = -m_fRadius + Dy;
for(int k=1; k<=m_iStacks; ++k, y+=Dy){
indexedStack.clear();
Real SectionRUp2 = RUp2 - y * y;
Vector3 o(0.0, y, Math::Sqrt(SectionRUp2));
for(int i=0; i<m_iSlices; ++i, ++index){
indexedStack.push_back(index);
pPosData[index] = o;
o = Dq * o;
}
indexedStacks.push_back(indexedStack);
}
pPosData[index] = Vector3(0.0, m_fRadius, 0.0);
indexedStack.clear();
for(int i=0; i<m_iSlices; ++i){
indexedStack.push_back(index);
}
indexedStacks.push_back(indexedStack);
// faces
int faceNum = (m_iStacks + 1) * m_iSlices;
uInt ii = 0;
for(int i=0; i<=m_iStacks; ++i){
int stack = i;
int nextStack = i+1;
for(int j=0; j<m_iSlices; ++j){
int slice = j;
int nextSlice = j + 1 < m_iSlices ? j + 1: 0;
pIndexData[ii++] = indexedStacks[stack][slice];
pIndexData[ii++] = indexedStacks[stack][nextSlice];
pIndexData[ii++] = indexedStacks[nextStack][nextSlice];
pIndexData[ii++] = indexedStacks[nextStack][slice];
}
}
m_Center = Vector3::ZERO;
m_fAverageRadius = m_fMaxRadius = m_fRadius;
// will calculate normals
//trianglate();
prepareGeometry();
}
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;
}
