int SundialsIda::JvProd(realtype t, N_Vector yIn, N_Vector ydotIn, N_Vector resIn,
N_Vector vIn, N_Vector JvIn, realtype c_j, void* jac_data,
N_Vector tmp1, N_Vector tmp2)
{
sdVector y(yIn), ydot(ydotIn), res(resIn), v(vIn), Jv(JvIn);
return ((sdDAE*) jac_data)->JvProd(t,y , ydot, res, v, Jv , c_j);
}
void RigidResponseMethodBLCP<Scalar, Dim>::collisionResponse()
{
//initialize
unsigned int m = this->rigid_driver_->numContactPoint();//m: number of contact points
unsigned int n = this->rigid_driver_->numRigidBody();//n: number of rigid bodies
if(m == 0 || n == 0)//no collision or no rigid body
return;
unsigned int dof = n * (Dim + RotationDof<Dim>::degree);//DoF(Degree of Freedom) of a rigid-body system
unsigned int fric_sample_count = 2;//count of friction sample directions
unsigned int s = m * fric_sample_count;//s: number of friction sample. Here a square sample is adopted
CompressedJacobianMatrix<Scalar, Dim> J(m, n);//Jacobian matrix. (m, dof) dimension when uncompressed, (m, 12) dimension after compression
CompressedJacobianMatrix<Scalar, Dim> D(s, n);//Jacobian matrix of friction. (s, dof) dimension when uncompressed, (s, 12) dimension after compression
CompressedInertiaMatrix<Scalar, Dim> M(n);//inertia matrix. (dof, dof) dimension when uncompressed, (dof, 6) dimension after compression
CompressedInertiaMatrix<Scalar, Dim> M_inv(n);//inversed inertia matrix. (dof, dof) dimension when uncompressed, (dof, 6) dimension after compression
CompressedJacobianMatrix<Scalar, Dim> MJ(n, m);//M * J. (dof, m) dimension when uncompressed, (12, m) dimension after compression
CompressedJacobianMatrix<Scalar, Dim> MD(n, s);//M * D. (dof, s) dimension when uncompressed, (12, s) dimension after compression
VectorND<Scalar> v(dof, 0);//generalized velocity of the system
VectorND<Scalar> Jv(m, 0);//normal relative velocity of each contact point (for normal contact impulse calculation)
VectorND<Scalar> post_Jv(m, 0);//expected post-impact normal relative velocity of each contact point (for normal contact impulse calculation)
VectorND<Scalar> Dv(s, 0);//tangent relative velocity of each contact point (for frictional contact impulse calculation)
VectorND<Scalar> CoR(m, 0);//coefficient of restitution (for normal contact impulse calculation)
VectorND<Scalar> CoF(s, 0);//coefficient of friction (for frictional contact impulse calculation)
VectorND<Scalar> z_norm(m, 0);//normal contact impulse. The key of collision response
VectorND<Scalar> z_fric(s, 0);//frictional contact impulse. The key of collision response
RigidBodyDriverUtility<Scalar, Dim>::computeMassMatrix(this->rigid_driver_, M, M_inv);
RigidBodyDriverUtility<Scalar, Dim>::computeJacobianMatrix(this->rigid_driver_, J);
RigidBodyDriverUtility<Scalar, Dim>::computeFricJacobianMatrix(this->rigid_driver_, D);
RigidBodyDriverUtility<Scalar, Dim>::computeGeneralizedVelocity(this->rigid_driver_, v);
//compute other matrix in need
CompressedJacobianMatrix<Scalar, Dim> J_T = J;
J_T = J.transpose();
CompressedJacobianMatrix<Scalar, Dim> D_T = D;
D_T = D.transpose();
MJ = M_inv * J_T;
MD = M_inv * D_T;
Jv = J * v;
Dv = D * v;
//update CoR and CoF
RigidBodyDriverUtility<Scalar, Dim>::computeCoefficient(this->rigid_driver_, CoR, CoF);
//calculate the expected post-impact velocity
for(unsigned int i = 0; i < m; ++i)
post_Jv[i] = -Jv[i] * CoR[i];
//solve BLCP with PGS. z_norm and z_fric are the unknown variables
RigidBodyDriverUtility<Scalar, Dim>::solveBLCPWithPGS(this->rigid_driver_, J, D, MJ, MD, Jv, post_Jv, Dv, z_norm, z_fric, CoR, CoF, 20);
//apply impulse
RigidBodyDriverUtility<Scalar, Dim>::applyImpulse(this->rigid_driver_, z_norm, z_fric, J_T, D_T);
}
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;
}
void SoftConstrFunction::Evaluate(const arma::vec &x) {
// Vertex coordinates: First convert control points into vertices
// verCoords has the form of [x1 x2, x3... y1 y2 y3... z1 z2 z3]
// We will compute Jacobian w.r.t vertex variables first, then we compute
// Jacobian w.r.t actual variables using composition rules
vec vertCoords = this->P * x;
vec CPx;
CPx.set_size(this->refEdgeLens.n_elem);
int nVertVars = this->P.n_rows; // Number of vertex variables = 3 * #vertices
int nVerts = nVertVars / 3; // Number of vertices
int nEdges = this->refEdgeLens.n_elem; // Number of edges (#constraints)
// TODO: Make Jv to be attribute of class to avoid re-allocating each evaluation
// Jacobian w.r.t all vertex coordinates. Then this->J = Jv * P since x = P*c
mat Jv = zeros(nEdges, nVertVars);
// Iterate through all edges to compute function value and derivatives
for (int i = 0; i < nEdges; i++)
{
// An edge has two vertices
int vertID1 = edges(0, i);
int vertID2 = edges(1, i);
// Indices of x,y,z coordinates in the vector vertCoords [x1 x2, x3... y1 y2 y3... z1 z2 z3]
int x1Idx = vertID1;
int y1Idx = x1Idx + nVerts;
int z1Idx = y1Idx + nVerts;
int x2Idx = vertID2;
int y2Idx = x2Idx + nVerts;
int z2Idx = y2Idx + nVerts;
// Coordinates of two vertices (x1,y1,z1) & (x2,y2,z2)
double x1 = vertCoords(x1Idx);
double y1 = vertCoords(y1Idx);
double z1 = vertCoords(z1Idx);
double x2 = vertCoords(x2Idx);
double y2 = vertCoords(y2Idx);
double z2 = vertCoords(z2Idx);
// Edge length
double edgeLen = sqrt( pow(x2-x1, 2) + pow(y2-y1, 2) + pow(z2-z1, 2) );
// Compute function value = "edge length" - "reference edge length"
// 有一些存在len大于refLen的边，这不符合实际情况，因此要做constrained optimization
CPx(i) = edgeLen - refEdgeLens(i);
// if (edgeLen > refEdgeLens(i))
// {
// std::cout << "FOUND ONE" << std::endl;
// }
// Compute derivatives. Recall the derivative: sqrt'(x) = 1/(2*sqrt(x))
Jv(i, x1Idx) = (x1-x2) / edgeLen;
Jv(i, y1Idx) = (y1-y2) / edgeLen;
Jv(i, z1Idx) = (z1-z2) / edgeLen;
Jv(i, x2Idx) = (x2-x1) / edgeLen;
Jv(i, y2Idx) = (y2-y1) / edgeLen;
Jv(i, z2Idx) = (z2-z1) / edgeLen;
}
this->F = MPwAP * x;
this->C = CPx;
// Jacobian w.r.t actual variables using composition chain rule
// mat m1 = arma::join_cols(MPwAP, Jv * P);
//mat m2 = arma::join_rows(2*trans(MPwAP*x), 2 * trans(CPx));
this->J = MPwAP;
this->A = Jv * P;
//cout << this->J << endl;
vec CPxabs = arma::abs(CPx);
#if 0
cout << "Constraint function mean: " << arma::mean(CPxabs) << endl;
#endif
}
