int
main(int argc,
char *argv[])
{
GRBenv *env = NULL;
GRBmodel *model = NULL;
int error = 0;
double sol[3];
int ind[3];
double val[3];
double obj[] = {1, 0, 0};
int qrow[3];
int qcol[3];
double qval[3];
int optimstatus;
double objval;
/* Create environment */
error = GRBloadenv(&env, "qcp.log");
if (error || env == NULL) {
fprintf(stderr, "Error: could not create environment\n");
exit(1);
}
/* Create an empty model */
error = GRBnewmodel(env, &model, "qcp", 0, NULL, NULL, NULL, NULL, NULL);
if (error) goto QUIT;
/* Add variables */
error = GRBaddvars(model, 3, 0, NULL, NULL, NULL, obj, NULL, NULL, NULL,
NULL);
if (error) goto QUIT;
/* Change sense to maximization */
error = GRBsetintattr(model, GRB_INT_ATTR_MODELSENSE, GRB_MAXIMIZE);
if (error) goto QUIT;
/* Integrate new variables */
error = GRBupdatemodel(model);
if (error) goto QUIT;
/* Linear constraint: x + y + z = 1 */
ind[0] = 0; ind[1] = 1; ind[2] = 2;
val[0] = 1; val[1] = 1; val[2] = 1;
error = GRBaddconstr(model, 3, ind, val, GRB_EQUAL, 1.0, "c0");
if (error) goto QUIT;
/* Cone: x^2 + y^2 <= z^2 */
qrow[0] = 0; qcol[0] = 0; qval[0] = 1.0;
qrow[1] = 1; qcol[1] = 1; qval[1] = 1.0;
qrow[2] = 2; qcol[2] = 2; qval[2] = -1.0;
error = GRBaddqconstr(model, 0, NULL, NULL, 3, qrow, qcol, qval,
GRB_LESS_EQUAL, 0.0, "qc0");
if (error) goto QUIT;
/* Rotated cone: x^2 <= yz */
qrow[0] = 0; qcol[0] = 0; qval[0] = 1.0;
qrow[1] = 1; qcol[1] = 2; qval[1] = -1.0;
error = GRBaddqconstr(model, 0, NULL, NULL, 2, qrow, qcol, qval,
GRB_LESS_EQUAL, 0.0, "qc1");
if (error) goto QUIT;
/* Optimize model */
error = GRBoptimize(model);
if (error) goto QUIT;
/* Write model to 'qcp.lp' */
error = GRBwrite(model, "qcp.lp");
if (error) goto QUIT;
/* Capture solution information */
error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
if (error) goto QUIT;
error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
if (error) goto QUIT;
error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, 3, sol);
if (error) goto QUIT;
printf("\nOptimization complete\n");
if (optimstatus == GRB_OPTIMAL) {
printf("Optimal objective: %.4e\n", objval);
printf(" x=%.2f, y=%.2f, z=%.2f\n", sol[0], sol[1], sol[2]);
} else if (optimstatus == GRB_INF_OR_UNBD) {
printf("Model is infeasible or unbounded\n");
} else {
printf("Optimization was stopped early\n");
}
QUIT:
/* Error reporting */
if (error) {
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
return 0;
}
int engine(int numassets, int numfactors,
double *ub, double *lb, double *mu, double *sigma2,
double *V, double *F, double lambda)
{
int retcode = 0;
GRBenv *env = NULL;
GRBmodel *model = NULL;
int n, i, j, k;
double *x;
int *qrow, *qcol, Nq;
double *qval;
int *cind;
double rhs;
char sense;
double *cval;
int numnonz;
char **names, bigname[100];
double expectedreturnval;
printf("running solver engine\n");
n = numassets + numfactors;
retcode = GRBloadenv(&env, "engine.log");
if (retcode) goto BACK;
/* Create initial model */
retcode = GRBnewmodel(env, &model, "factors", n,
NULL, NULL, NULL, NULL, NULL);
if (retcode) goto BACK;
names = (char **) calloc(n, sizeof(char *));
/** next we create the remaining attributes for the n columns **/
x = (double *) calloc (n, sizeof(double));
for(j = 0; j < numassets; j++){
names[j] = (char *)calloc(3, sizeof(char));
if(names[j] == NULL){
retcode = 1; goto BACK;
}
sprintf(names[j],"x%d", j);
}
for(j = numassets; j < numassets + numfactors; j++){
names[j] = (char *)calloc(3, sizeof(char));
if(names[j] == NULL){
retcode = 1; goto BACK;
}
sprintf(names[j],"F%d", j - numassets);
}
/* initialize variables */
for(j = 0; j < n; j++){
retcode = GRBsetstrattrelement(model, "VarName", j, names[j]);
if (retcode) goto BACK;
retcode = GRBsetdblattrelement(model, "Obj", j, -mu[j]);
if (retcode) goto BACK;
retcode = GRBsetdblattrelement(model, "LB", j, lb[j]);
if (retcode) goto BACK;
retcode = GRBsetdblattrelement(model, "UB", j, ub[j]);
if (retcode) goto BACK;
}
/** next, the quadratic -- there are numassets + numfactors*numfactors nonzeroes:
numassets residual variances plus the numfactors x numfactors
factor covariance matrix**/
Nq = numassets + numfactors*numfactors;
qrow = (int *) calloc(Nq, sizeof(int)); /** row indices **/
qcol = (int *) calloc(Nq, sizeof(int)); /** column indices **/
qval = (double *) calloc(Nq, sizeof(double)); /** values **/
if( ( qrow == NULL) || ( qcol == NULL) || (qval == NULL) ){
printf("could not create quadratic\n");
retcode = 1; goto BACK;
}
for (j = 0; j < numassets; j++){
qval[j] = lambda*sigma2[j];
qrow[j] = qcol[j] = j;
}
for (i = 0; i < numfactors; i++){
for (j = 0; j < numfactors; j++){
k = i*numfactors + j;
qval[k + numassets] = lambda*F[k];
qrow[k + numassets] = numassets + i;
qcol[k + numassets] = numassets + j;
}
}
retcode = GRBaddqpterms(model, Nq, qrow, qcol, qval);
if (retcode) goto BACK;
/** now we will add one constraint at a time **/
/** we need to have a couple of auxiliary arrays **/
cind = (int *)calloc(n, sizeof(int)); /** n is over the top since no constraint is totally dense; but it's not too bad here **/
cval= (double *)calloc(n, sizeof(double));
if(!cval){
printf("cannot allocate cval\n"); retcode = 2; goto BACK;
}
for(i = 0; i < numfactors; i++){
for(j = 0; j < numassets; j++){
cval[j] = V[i*numassets + j];
cind[j] = j;
}
cind[numassets] = /* j */ numassets + i;
cval[numassets] = -1;
numnonz = numassets + 1;
rhs = 0;
sense = GRB_EQUAL;
sprintf(bigname,"factor%d",i);
retcode = GRBaddconstr(model, numnonz, cind, cval, sense, rhs, bigname);
if (retcode) goto BACK;
}
/** sum of x variables = 1 **/
for (j = 0; j < numassets; j++){
cval[j] = 1.0; cind[j] = j;
}
numnonz = numassets;
rhs = 1.0;
sense = GRB_EQUAL;
/* let's reuse some space */
sprintf(bigname, "sum");
retcode = GRBaddconstr(model, numnonz, cind, cval, sense, rhs, bigname);
if (retcode) goto BACK;
retcode = GRBupdatemodel(model);
if (retcode) goto BACK;
/** optional: write the problem **/
retcode = GRBwrite(model, "engine.lp");
if (retcode) goto BACK;
retcode = GRBoptimize(model);
if (retcode) goto BACK;
/** get solution **/
retcode = GRBgetdblattrarray(model,
GRB_DBL_ATTR_X, 0, n,
x);
if(retcode) goto BACK;
/** now let's see the values **/
expectedreturnval = 0;
for(j = 0; j < numassets; j++){
if( x[j] > 1.0e-09){
printf("%s = %g\n", names[j], x[j]);
expectedreturnval += x[j]*mu[j];
}
}
printf("\n*** expected portfolio return: %g\n", expectedreturnval);
GRBfreemodel(model);
GRBfreeenv(env);
BACK:
printf("engine exits with code %d\n", retcode);
return retcode;
}
int setupAndSolveQP(NewQPControllerData *pdata, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input, double t, Map<VectorXd> &q, Map<VectorXd> &qd, const Ref<Matrix<bool, Dynamic, 1>> &b_contact_force, QPControllerOutput *qp_output, std::shared_ptr<QPControllerDebugData> debug) {
// The primary solve loop for our controller. This constructs and solves a Quadratic Program and produces the instantaneous desired torques, along with reference positions, velocities, and accelerations. It mirrors the Matlab implementation in atlasControllers.InstantaneousQPController.setupAndSolveQP(), and more documentation can be found there.
// Note: argument `debug` MAY be set to NULL, which signals that no debug information is requested.
// look up the param set by name
AtlasParams *params;
std::map<string,AtlasParams>::iterator it;
it = pdata->param_sets.find(qp_input->param_set_name);
if (it == pdata->param_sets.end()) {
mexWarnMsgTxt("Got a param set I don't recognize! Using standing params instead");
it = pdata->param_sets.find("standing");
if (it == pdata->param_sets.end()) {
mexErrMsgTxt("Could not fall back to standing parameters either. I have to give up here.");
}
}
// cout << "using params set: " + it->first + ", ";
params = &(it->second);
// mexPrintf("Kp_accel: %f, ", params->Kp_accel);
int nu = pdata->B.cols();
int nq = pdata->r->num_positions;
// zmp_data
Map<Matrix<double, 4, 4, RowMajor>> A_ls(&qp_input->zmp_data.A[0][0]);
Map<Matrix<double, 4, 2, RowMajor>> B_ls(&qp_input->zmp_data.B[0][0]);
Map<Matrix<double, 2, 4, RowMajor>> C_ls(&qp_input->zmp_data.C[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> D_ls(&qp_input->zmp_data.D[0][0]);
Map<Matrix<double, 4, 1>> x0(&qp_input->zmp_data.x0[0][0]);
Map<Matrix<double, 2, 1>> y0(&qp_input->zmp_data.y0[0][0]);
Map<Matrix<double, 2, 1>> u0(&qp_input->zmp_data.u0[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> R_ls(&qp_input->zmp_data.R[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> Qy(&qp_input->zmp_data.Qy[0][0]);
Map<Matrix<double, 4, 4, RowMajor>> S(&qp_input->zmp_data.S[0][0]);
Map<Matrix<double, 4, 1>> s1(&qp_input->zmp_data.s1[0][0]);
Map<Matrix<double, 4, 1>> s1dot(&qp_input->zmp_data.s1dot[0][0]);
// // whole_body_data
if (qp_input->whole_body_data.num_positions != nq) mexErrMsgTxt("number of positions doesn't match num_dof for this robot");
Map<VectorXd> q_des(qp_input->whole_body_data.q_des.data(), nq);
Map<VectorXd> condof(qp_input->whole_body_data.constrained_dofs.data(), qp_input->whole_body_data.num_constrained_dofs);
PIDOutput pid_out = wholeBodyPID(pdata, t, q, qd, q_des, ¶ms->whole_body);
qp_output->q_ref = pid_out.q_ref;
// mu
// NOTE: we're using the same mu for all supports
double mu;
if (qp_input->num_support_data == 0) {
mu = 1.0;
} else {
mu = qp_input->support_data[0].mu;
for (int i=1; i < qp_input->num_support_data; i++) {
if (qp_input->support_data[i].mu != mu) {
mexWarnMsgTxt("Currently, we assume that all supports have the same value of mu");
}
}
}
const int dim = 3, // 3D
nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now
assert(nu+6 == nq);
vector<DesiredBodyAcceleration> desired_body_accelerations;
desired_body_accelerations.resize(qp_input->num_tracked_bodies);
Vector6d body_pose_des, body_v_des, body_vdot_des;
Vector6d body_vdot;
for (int i=0; i < qp_input->num_tracked_bodies; i++) {
int body_id0 = qp_input->body_motion_data[i].body_id - 1;
double weight = params->body_motion[body_id0].weight;
desired_body_accelerations[i].body_id0 = body_id0;
Map<Matrix<double, 6, 4,RowMajor>>coefs_rowmaj(&qp_input->body_motion_data[i].coefs[0][0]);
Matrix<double, 6, 4> coefs = coefs_rowmaj;
evaluateCubicSplineSegment(t - qp_input->body_motion_data[i].ts[0], coefs, body_pose_des, body_v_des, body_vdot_des);
desired_body_accelerations[i].body_vdot = bodyMotionPD(pdata->r, q, qd, body_id0, body_pose_des, body_v_des, body_vdot_des, params->body_motion[body_id0].Kp, params->body_motion[body_id0].Kd);
desired_body_accelerations[i].weight = weight;
desired_body_accelerations[i].accel_bounds = params->body_motion[body_id0].accel_bounds;
// mexPrintf("body: %d, vdot: %f %f %f %f %f %f weight: %f\n", body_id0,
// desired_body_accelerations[i].body_vdot(0),
// desired_body_accelerations[i].body_vdot(1),
// desired_body_accelerations[i].body_vdot(2),
// desired_body_accelerations[i].body_vdot(3),
// desired_body_accelerations[i].body_vdot(4),
// desired_body_accelerations[i].body_vdot(5),
// weight);
// mexPrintf("tracking body: %d, coefs[:,0]: %f %f %f %f %f %f coefs(", body_id0,
}
int n_body_accel_eq_constraints = 0;
for (int i=0; i < desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight < 0)
n_body_accel_eq_constraints++;
}
MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;
pdata->r->doKinematics(q,false,qd);
//---------------------------------------------------------------------
vector<SupportStateElement> available_supports = loadAvailableSupports(qp_input);
vector<SupportStateElement> active_supports = getActiveSupports(pdata->r, pdata->map_ptr, q, qd, available_supports, b_contact_force, params->contact_threshold, pdata->default_terrain_height);
int num_active_contact_pts=0;
for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
num_active_contact_pts += iter->contact_pts.size();
}
pdata->r->HandC(q,qd,(MatrixXd*)nullptr,pdata->H,pdata->C,(MatrixXd*)nullptr,(MatrixXd*)nullptr,(MatrixXd*)nullptr);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (params->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
MatrixXd Jcom,Jcomdot;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdot = pdata->Jdot;
}
else {
Jcom = pdata->J_xy;
Jcomdot = pdata->Jdot_xy;
}
MatrixXd B,JB,Jp,Jpdot,normals;
int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,pdata->default_terrain_height);
int neps = nc*dim;
VectorXd x_bar,xlimp;
MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
if (nc>0) {
if (x0.size()==6) {
// x,y,z com
xlimp.resize(6);
xlimp.topRows(3) = xcom;
xlimp.bottomRows(3) = Jcom*qd;
}
else {
xlimp.resize(4);
xlimp.topRows(2) = xcom.topRows(2);
xlimp.bottomRows(2) = Jcom*qd;
}
x_bar = xlimp-x0;
D_float = JB.topRows(6);
D_act = JB.bottomRows(nu);
}
int nf = nc*nd; // number of contact force variables
int nparams = nq+nf+neps;
Vector3d kdot_des;
if (include_angular_momentum) {
VectorXd k = pdata->Ak*qd;
kdot_des = -params->Kp_ang*k; // TODO: parameterize
}
//----------------------------------------------------------------------
// QP cost function ----------------------------------------------------
//
// min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) +
// w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) +
// w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd))
VectorXd f(nparams);
{
if (nc > 0) {
// NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
VectorXd tmp = C_ls*xlimp;
VectorXd tmp1 = Jcomdot*qd;
MatrixXd tmp2 = R_DQyD_ls*Jcom;
pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom;
// mexPrintf("fqp head: %f %f %f\n", pdata->fqp(0), pdata->fqp(1), pdata->fqp(2));
pdata->fqp += tmp1.transpose()*tmp2;
pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom;
pdata->fqp -= u0.transpose()*tmp2;
pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom;
pdata->fqp -= (params->whole_body.w_qdd.array()*pid_out.qddot_des.array()).matrix().transpose();
if (include_angular_momentum) {
pdata->fqp += qd.transpose()*pdata->Akdot.transpose()*params->W_kdot*pdata->Ak;
pdata->fqp -= kdot_des.transpose()*params->W_kdot*pdata->Ak;
}
f.head(nq) = pdata->fqp.transpose();
} else {
f.head(nq) = -pid_out.qddot_des;
}
}
f.tail(nf+neps) = VectorXd::Zero(nf+neps);
int neq = 6+neps+6*n_body_accel_eq_constraints+qp_input->whole_body_data.num_constrained_dofs;
MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
VectorXd beq = VectorXd::Zero(neq);
// constrained floating base dynamics
// H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
Aeq.topLeftCorner(6,nq) = pdata->H_float;
beq.topRows(6) = -pdata->C_float;
if (nc>0) {
Aeq.block(0,nq,6,nc*nd) = -D_float;
}
if (nc > 0) {
// relative acceleration constraint
Aeq.block(6,0,neps,nq) = Jp;
Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf); // note: obvious sparsity here
Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps); // note: obvious sparsity here
beq.segment(6,neps) = (-Jpdot -params->Kp_accel*Jp)*qd;
}
// add in body spatial equality constraints
// VectorXd body_vdot;
MatrixXd orig = MatrixXd::Zero(4,1);
orig(3,0) = 1;
int equality_ind = 6+neps;
MatrixXd Jb(6,nq);
MatrixXd Jbdot(6,nq);
for (int i=0; i<desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight < 0) { // negative implies constraint
if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(desired_body_accelerations[i].body_vdot(j))) {
Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
beq[equality_ind++] = -Jbdot.row(j)*qd + desired_body_accelerations[i].body_vdot(j);
}
}
}
}
}
if (qp_input->whole_body_data.num_constrained_dofs>0) {
// add joint acceleration constraints
for (int i=0; i<qp_input->whole_body_data.num_constrained_dofs; i++) {
Aeq(equality_ind,(int)condof[i]-1) = 1;
beq[equality_ind++] = pid_out.qddot_des[(int)condof[i]-1];
}
}
int n_ineq = 2*nu+2*6*desired_body_accelerations.size();
MatrixXd Ain = MatrixXd::Zero(n_ineq,nparams); // note: obvious sparsity here
VectorXd bin = VectorXd::Zero(n_ineq);
// linear input saturation constraints
// u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
// using transpose instead of inverse because B is orthogonal
Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;
Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;
int constraint_start_index = 2*nu;
for (int i=0; i<desired_body_accelerations.size(); i++) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = Jb;
bin.segment(constraint_start_index,6) = -Jbdot*qd + desired_body_accelerations[i].accel_bounds.max;
constraint_start_index += 6;
Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = -Jb;
bin.segment(constraint_start_index,6) = Jbdot*qd - desired_body_accelerations[i].accel_bounds.min;
constraint_start_index += 6;
}
for (int i=0; i<n_ineq; i++) {
// remove inf constraints---needed by gurobi
if (std::isinf(double(bin(i)))) {
Ain.row(i) = 0*Ain.row(i);
bin(i)=0;
}
}
GRBmodel * model = nullptr;
int info=-1;
// set obj,lb,up
VectorXd lb(nparams), ub(nparams);
lb.head(nq) = pdata->qdd_lb;
ub.head(nq) = pdata->qdd_ub;
lb.segment(nq,nf) = VectorXd::Zero(nf);
ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
lb.tail(neps) = -params->slack_limit*VectorXd::Ones(neps);
ub.tail(neps) = params->slack_limit*VectorXd::Ones(neps);
VectorXd alpha(nparams);
MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 ); // nq, nf, neps // this one is for gurobi
VectorXd w = (params->whole_body.w_qdd.array() + REG).matrix();
#ifdef USE_MATRIX_INVERSION_LEMMA
double max_body_accel_weight = -numeric_limits<double>::infinity();
for (int i=0; i < desired_body_accelerations.size(); i++) {
max_body_accel_weight = max(max_body_accel_weight, desired_body_accelerations[i].weight);
}
bool include_body_accel_cost_terms = desired_body_accelerations.size() > 0 && max_body_accel_weight > 1e-10;
if (pdata->use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
{
// TODO: update to include angular momentum, body accel objectives.
// We want Hqp inverse, which I can compute efficiently using the
// matrix inversion lemma (see wikipedia):
// inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
if (nc>0) {
MatrixXd Wi = ((1/(params->whole_body.w_qdd.array() + REG)).matrix()).asDiagonal();
if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi;
}
}
else {
pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
}
#ifdef TEST_FAST_QP
if (nc>0) {
MatrixXd Hqp_test(nq,nq);
MatrixXd W = w.asDiagonal();
Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse();
if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
}
}
#endif
Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha);
//if (info<0) mexPrintf("fastQP info = %d. Calling gurobi.\n", info);
}
else {
#endif
if (nc>0) {
pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom;
if (include_angular_momentum) {
pdata->Hqp += pdata->Ak.transpose()*params->W_kdot*pdata->Ak;
}
pdata->Hqp += params->whole_body.w_qdd.asDiagonal();
pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
} else {
pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq);
}
// add in body spatial acceleration cost terms
for (int i=0; i<desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight > 0) {
if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(desired_body_accelerations[i].body_vdot[j])) {
pdata->Hqp += desired_body_accelerations[i].weight*(Jb.row(j)).transpose()*Jb.row(j);
f.head(nq) += desired_body_accelerations[i].weight*(qd.transpose()*Jbdot.row(j).transpose() - desired_body_accelerations[i].body_vdot[j])*Jb.row(j).transpose();
}
}
}
}
}
Qnfdiag = MatrixXd::Constant(nf,1,params->w_grf+REG);
Qneps = MatrixXd::Constant(neps,1,params->w_slack+REG);
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
if (pdata->use_fast_qp > 0)
{ // set up and call fastqp
info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha);
//if (info<0) mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
}
else {
// use gurobi active set
model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.vbasis,pdata->state.vbasis_len,pdata->state.cbasis,pdata->state.cbasis_len,alpha);
CGE(GRBgetintattr(model,"NumVars",&(pdata->state.vbasis_len)), pdata->env);
CGE(GRBgetintattr(model,"NumConstrs",&(pdata->state.cbasis_len)), pdata->env);
info=66;
//info = -1;
}
if (info<0) {
model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.active,alpha);
int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
//if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
}
#ifdef USE_MATRIX_INVERSION_LEMMA
}
#endif
//----------------------------------------------------------------------
// Solve for inputs ----------------------------------------------------
qp_output->qdd = alpha.head(nq);
VectorXd beta = alpha.segment(nq,nc*nd);
// use transpose because B_act is orthogonal
qp_output->u = pdata->B_act.transpose()*(pdata->H_act*qp_output->qdd + pdata->C_act - D_act*beta);
//y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);
bool foot_contact[2];
foot_contact[0] = b_contact_force(pdata->rpc.body_ids.r_foot) == 1;
foot_contact[1] = b_contact_force(pdata->rpc.body_ids.l_foot) == 1;
qp_output->qd_ref = velocityReference(pdata, t, q, qd, qp_output->qdd, foot_contact, &(params->vref_integrator), &(pdata->rpc));
// Remember t for next time around
pdata->state.t_prev = t;
// If a debug pointer was passed in, fill it with useful data
if (debug) {
debug->active_supports.resize(active_supports.size());
for (int i=0; i < active_supports.size(); i++) {
debug->active_supports[i] = active_supports[i];
}
debug->nc = nc;
debug->normals = normals;
debug->B = B;
debug->alpha = alpha;
debug->f = f;
debug->Aeq = Aeq;
debug->beq = beq;
debug->Ain_lb_ub = Ain_lb_ub;
debug->bin_lb_ub = bin_lb_ub;
debug->Qnfdiag = Qnfdiag;
debug->Qneps = Qneps;
debug->x_bar = x_bar;
debug->S = S;
debug->s1 = s1;
debug->s1dot = s1dot;
debug->s2dot = qp_input->zmp_data.s2dot;
debug->A_ls = A_ls;
debug->B_ls = B_ls;
debug->Jcom = Jcom;
debug->Jcomdot = Jcomdot;
debug->beta = beta;
}
// if we used gurobi, clean up
if (model) {
GRBfreemodel(model);
}
// GRBfreeenv(env);
return info;
}
int
main(int argc,
char *argv[])
{
GRBenv *env = NULL, *modelenv = NULL;
GRBmodel *model = NULL;
int error = 0;
int j, numfractional, iter, nfix;
int numintvars;
int *intvars = NULL;
int status;
char vtype, *vname;
double sol, obj, fixval;
var_t *fractional = NULL;
if (argc < 2)
{
fprintf(stderr, "Usage: fixanddive_c filename\n");
exit(1);
}
error = GRBloadenv(&env, "fixanddive.log");
if (error || env == NULL)
{
fprintf(stderr, "Error: could not create environment\n");
exit(1);
}
/* Read model */
error = GRBreadmodel(env, argv[1], &model);
if (error) goto QUIT;
/* Collect integer variables and relax them */
error = GRBgetintattr(model, "NumIntVars", &numintvars);
if (error) goto QUIT;
intvars = malloc(sizeof(int) * numintvars);
if (!intvars) goto QUIT;
fractional = malloc(sizeof(var_t) * numintvars);
if (!fractional) goto QUIT;
numfractional = 0;
for (j = 0; j < numintvars; ++j)
{
error = GRBgetcharattrelement(model, "VType", j, &vtype);
if (error) goto QUIT;
if (vtype != GRB_CONTINUOUS)
{
intvars[numfractional++] = j;
error = GRBsetcharattrelement(model, "VType", j, GRB_CONTINUOUS);
if (error) goto QUIT;
}
}
modelenv = GRBgetenv(model);
if (!modelenv) goto QUIT;
error = GRBsetintparam(modelenv, "OutputFlag", 0);
if (error) goto QUIT;
error = GRBoptimize(model);
if (error) goto QUIT;
/* Perform multiple iterations. In each iteration, identify the first
quartile of integer variables that are closest to an integer value
in the relaxation, fix them to the nearest integer, and repeat. */
for (iter = 0; iter < 1000; ++iter)
{
/* create a list of fractional variables, sorted in order of
increasing distance from the relaxation solution to the nearest
integer value */
numfractional = 0;
for (j = 0; j < numintvars; ++j)
{
error = GRBgetdblattrelement(model, "X", intvars[j], &sol);
if (error) goto QUIT;
if (fabs(sol - floor(sol + 0.5)) > 1e-5)
{
fractional[numfractional].index = intvars[j];
fractional[numfractional++].X = sol;
}
}
error = GRBgetdblattr(model, "ObjVal", &obj);
if (error) goto QUIT;
printf("Iteration %i, obj %f, fractional %i\n",
iter, obj, numfractional);
if (numfractional == 0)
{
printf("Found feasible solution - objective %f\n", obj);
break;
}
/* Fix the first quartile to the nearest integer value */
qsort(fractional, numfractional, sizeof(var_t), vcomp);
nfix = numfractional / 4;
nfix = (nfix > 1) ? nfix : 1;
for (j = 0; j < nfix; ++j)
{
fixval = floor(fractional[j].X + 0.5);
error = GRBsetdblattrelement(model, "LB", fractional[j].index, fixval);
if (error) goto QUIT;
error = GRBsetdblattrelement(model, "UB", fractional[j].index, fixval);
if (error) goto QUIT;
error = GRBgetstrattrelement(model, "VarName",
fractional[j].index, &vname);
printf(" Fix %s to %f ( rel %f )\n", vname, fixval, fractional[j].X);
}
error = GRBoptimize(model);
if (error) goto QUIT;
/* Check optimization result */
error = GRBgetintattr(model, "Status", &status);
if (error) goto QUIT;
if (status != GRB_OPTIMAL)
{
printf("Relaxation is infeasible\n");
break;
}
}
QUIT:
/* Error reporting */
if (error)
{
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free data */
free(intvars);
free(fractional);
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
return 0;
}
int
main(int argc,
char *argv[])
{
FILE *fp = NULL;
GRBenv *env = NULL;
GRBmodel *model = NULL;
int board[DIM][DIM];
char inputline[100];
int ind[DIM];
double val[DIM];
double lb[DIM*DIM*DIM];
char vtype[DIM*DIM*DIM];
char *names[DIM*DIM*DIM];
char namestorage[10*DIM*DIM*DIM];
char *cursor;
int optimstatus;
double objval;
int zero = 0;
int i, j, v, ig, jg, count;
int error = 0;
if (argc < 2) {
fprintf(stderr, "Usage: sudoku_c datafile\n");
exit(1);
}
fp = fopen(argv[1], "r");
if (fp == NULL) {
fprintf(stderr, "Error: unable to open input file %s\n", argv[1]);
exit(1);
}
for (i = 0; i < DIM; i++) {
fgets(inputline, 100, fp);
if (strlen(inputline) < 9) {
fprintf(stderr, "Error: not enough board positions specified\n");
exit(1);
}
for (j = 0; j < DIM; j++) {
board[i][j] = (int) inputline[j] - (int) '1';
if (board[i][j] < 0 || board[i][j] >= DIM)
board[i][j] = -1;
}
}
/* Create an empty model */
cursor = namestorage;
for (i = 0; i < DIM; i++) {
for (j = 0; j < DIM; j++) {
for (v = 0; v < DIM; v++) {
if (board[i][j] == v)
lb[i*DIM*DIM+j*DIM+v] = 1;
else
lb[i*DIM*DIM+j*DIM+v] = 0;
vtype[i*DIM*DIM+j*DIM+v] = GRB_BINARY;
names[i*DIM*DIM+j*DIM+v] = cursor;
sprintf(names[i*DIM*DIM+j*DIM+v], "x[%d,%d,%d]", i, j, v+1);
cursor += strlen(names[i*DIM*DIM+j*DIM+v]) + 1;
}
}
}
/* Create environment */
error = GRBloadenv(&env, "sudoku.log");
if (error) goto QUIT;
/* Create new model */
error = GRBnewmodel(env, &model, "sudoku", DIM*DIM*DIM, NULL, lb, NULL,
vtype, names);
if (error) goto QUIT;
/* Each cell gets a value */
for (i = 0; i < DIM; i++) {
for (j = 0; j < DIM; j++) {
for (v = 0; v < DIM; v++) {
ind[v] = i*DIM*DIM + j*DIM + v;
val[v] = 1.0;
}
error = GRBaddconstr(model, DIM, ind, val, GRB_EQUAL, 1.0, NULL);
if (error) goto QUIT;
}
}
/* Each value must appear once in each row */
for (v = 0; v < DIM; v++) {
for (j = 0; j < DIM; j++) {
for (i = 0; i < DIM; i++) {
ind[i] = i*DIM*DIM + j*DIM + v;
val[i] = 1.0;
}
error = GRBaddconstr(model, DIM, ind, val, GRB_EQUAL, 1.0, NULL);
if (error) goto QUIT;
}
}
/* Each value must appear once in each column */
for (v = 0; v < DIM; v++) {
for (i = 0; i < DIM; i++) {
for (j = 0; j < DIM; j++) {
ind[j] = i*DIM*DIM + j*DIM + v;
val[j] = 1.0;
}
error = GRBaddconstr(model, DIM, ind, val, GRB_EQUAL, 1.0, NULL);
if (error) goto QUIT;
}
}
/* Each value must appear once in each subgrid */
for (v = 0; v < DIM; v++) {
for (ig = 0; ig < SUBDIM; ig++) {
for (jg = 0; jg < SUBDIM; jg++) {
count = 0;
for (i = ig*SUBDIM; i < (ig+1)*SUBDIM; i++) {
for (j = jg*SUBDIM; j < (jg+1)*SUBDIM; j++) {
ind[count] = i*DIM*DIM + j*DIM + v;
val[count] = 1.0;
count++;
}
}
error = GRBaddconstr(model, DIM, ind, val, GRB_EQUAL, 1.0, NULL);
if (error) goto QUIT;
}
}
}
/* Optimize model */
error = GRBoptimize(model);
if (error) goto QUIT;
/* Write model to 'sudoku.lp' */
error = GRBwrite(model, "sudoku.lp");
if (error) goto QUIT;
/* Capture solution information */
error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
if (error) goto QUIT;
error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
if (error) goto QUIT;
printf("\nOptimization complete\n");
if (optimstatus == GRB_OPTIMAL)
printf("Optimal objective: %.4e\n", objval);
else if (optimstatus == GRB_INF_OR_UNBD)
printf("Model is infeasible or unbounded\n");
else
printf("Optimization was stopped early\n");
printf("\n");
QUIT:
/* Error reporting */
if (error) {
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
return 0;
}
int
main(int argc,
char *argv[])
{
GRBenv *env = NULL;
GRBmodel *model = NULL;
int error = 0, status;
int s, w, col;
int *cbeg = NULL;
int *cind = NULL;
int idx;
double *cval = NULL;
char *sense = NULL;
char vname[MAXSTR];
double obj;
int i, iis, numconstrs;
char *cname;
/* Sample data */
const int nShifts = 14;
const int nWorkers = 7;
/* Sets of days and workers */
char* Shifts[] =
{ "Mon1", "Tue2", "Wed3", "Thu4", "Fri5", "Sat6",
"Sun7", "Mon8", "Tue9", "Wed10", "Thu11", "Fri12", "Sat13",
"Sun14" };
char* Workers[] =
{ "Amy", "Bob", "Cathy", "Dan", "Ed", "Fred", "Gu" };
/* Number of workers required for each shift */
double shiftRequirements[] =
{ 3, 2, 4, 4, 5, 6, 5, 2, 2, 3, 4, 6, 7, 5 };
/* Amount each worker is paid to work one shift */
double pay[] = { 10, 12, 10, 8, 8, 9, 11 };
/* Worker availability: 0 if the worker is unavailable for a shift */
double availability[][14] =
{ { 0, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1 },
{ 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 0 },
{ 0, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1 },
{ 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1 },
{ 1, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 1 },
{ 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 } };
/* Create environment */
error = GRBloadenv(&env, "workforce1.log");
if (error || env == NULL)
{
fprintf(stderr, "Error: could not create environment\n");
exit(1);
}
/* Create initial model */
error = GRBnewmodel(env, &model, "workforce1", nWorkers * nShifts,
NULL, NULL, NULL, NULL, NULL);
if (error) goto QUIT;
/* Initialize assignment decision variables:
x[w][s] == 1 if worker w is assigned
to shift s. Since an assignment model always produces integer
solutions, we use continuous variables and solve as an LP. */
for (w = 0; w < nWorkers; ++w)
{
for (s = 0; s < nShifts; ++s)
{
col = xcol(w, s);
sprintf(vname, "%s.%s", Workers[w], Shifts[s]);
error = GRBsetdblattrelement(model, "UB", col, availability[w][s]);
if (error) goto QUIT;
error = GRBsetdblattrelement(model, "Obj", col, pay[w]);
if (error) goto QUIT;
error = GRBsetstrattrelement(model, "VarName", col, vname);
if (error) goto QUIT;
}
}
/* The objective is to minimize the total pay costs */
error = GRBsetintattr(model, "ModelSense", 1);
if (error) goto QUIT;
/* Make space for constraint data */
cbeg = malloc(sizeof(int) * nShifts);
if (!cbeg) goto QUIT;
cind = malloc(sizeof(int) * nShifts * nWorkers);
if (!cind) goto QUIT;
cval = malloc(sizeof(double) * nShifts * nWorkers);
if (!cval) goto QUIT;
sense = malloc(sizeof(char) * nShifts);
if (!sense) goto QUIT;
/* Constraint: assign exactly shiftRequirements[s] workers
to each shift s */
idx = 0;
for (s = 0; s < nShifts; ++s)
{
cbeg[s] = idx;
sense[s] = GRB_EQUAL;
for (w = 0; w < nWorkers; ++w)
{
cind[idx] = xcol(w, s);
cval[idx++] = 1.0;
}
}
error = GRBaddconstrs(model, nShifts, idx, cbeg, cind, cval, sense,
shiftRequirements, Shifts);
if (error) goto QUIT;
/* Optimize */
error = GRBoptimize(model);
if (error) goto QUIT;
error = GRBgetintattr(model, "Status", &status);
if (error) goto QUIT;
if (status == GRB_UNBOUNDED)
{
printf("The model cannot be solved because it is unbounded\n");
goto QUIT;
}
if (status == GRB_OPTIMAL)
{
error = GRBgetdblattr(model, "ObjVal", &obj);
if (error) goto QUIT;
printf("The optimal objective is %f\n", obj);
goto QUIT;
}
if ((status != GRB_INF_OR_UNBD) && (status != GRB_INFEASIBLE))
{
printf("Optimization was stopped with status %i\n", status);
goto QUIT;
}
/* do IIS */
printf("The model is infeasible; computing IIS\n");
error = GRBcomputeIIS(model);
if (error) goto QUIT;
printf("\nThe following constraint(s) cannot be satisfied:\n");
error = GRBgetintattr(model, "NumConstrs", &numconstrs);
if (error) goto QUIT;
for (i = 0; i < numconstrs; ++i)
{
error = GRBgetintattrelement(model, "IISConstr", i, &iis);
if (error) goto QUIT;
if (iis)
{
error = GRBgetstrattrelement(model, "ConstrName", i, &cname);
if (error) goto QUIT;
printf("%s\n", cname);
}
}
QUIT:
/* Error reporting */
if (error)
{
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free data */
free(cbeg);
free(cind);
free(cval);
free(sense);
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
return 0;
}
int
main(int argc,
char *argv[])
{
GRBenv *env = NULL;
GRBmodel *model = NULL;
int error = 0;
int vars, optimstatus;
double objval;
struct callback_data mydata;
mydata.lastmsg = -GRB_INFINITY;
mydata.logfile = NULL;
mydata.solution = NULL;
if (argc < 2) {
fprintf(stderr, "Usage: callback_c filename\n");
goto QUIT;
}
mydata.logfile = fopen("cb.log", "w");
if (!mydata.logfile) {
fprintf(stderr, "Cannot open cb.log for callback message\n");
goto QUIT;
}
/* Create environment */
error = GRBloadenv(&env, NULL);
if (error || env == NULL) {
fprintf(stderr, "Error: could not create environment\n");
exit(1);
}
/* Turn off display */
error = GRBsetintparam(env, GRB_INT_PAR_OUTPUTFLAG, 0);
if (error) goto QUIT;
/* Read model from file */
error = GRBreadmodel(env, argv[1], &model);
if (error) goto QUIT;
/* Allocate space for solution */
error = GRBgetintattr(model, GRB_INT_ATTR_NUMVARS, &vars);
if (error) goto QUIT;
mydata.solution = malloc(vars*sizeof(double));
if (mydata.solution == NULL) {
fprintf(stderr, "Failed to allocate memory\n");
exit(1);
}
/* Set callback function */
error = GRBsetcallbackfunc(model, mycallback, (void *) &mydata);
if (error) goto QUIT;
/* Solve model */
error = GRBoptimize(model);
if (error) goto QUIT;
/* Capture solution information */
error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
if (error) goto QUIT;
error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
if (error) goto QUIT;
printf("\nOptimization complete\n");
if (optimstatus == GRB_OPTIMAL)
printf("Optimal objective: %.4e\n", objval);
else if (optimstatus == GRB_INF_OR_UNBD)
printf("Model is infeasible or unbounded\n");
else
printf("Optimization was stopped early\n");
printf("\n");
QUIT:
/* Error reporting */
if (error) {
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Close file */
if (mydata.logfile)
fclose(mydata.logfile);
/* Free solution */
if (mydata.solution)
free(mydata.solution);
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
return 0;
}
GRBmodel *fixed_model(GRBmodel *mdl0)
{
GRBenv *env;
GRBmodel *mdl;
double f, *y;
int i;
static char *statusname[] = {
"infeasible",
"infeasible or unbounded",
"unbounded",
"cutoff",
"iteration limit",
"node limit",
"time limit",
"solution limit",
"interrupted",
"numeric difficulty"
};
if (!(mdl = GRBfixedmodel(mdl0)))
return 0;
if (!(env = GRBgetenv(mdl))) {
//dpf(d, "\nGRBgetenv failed in fixed_model().");
badret:
GRBfreemodel(mdl);
return 0;
}
if (GRBsetintparam(env, "Presolve", 0)) {
//intbasis_fail(d, "setintparam(\"Presolve\")");
goto badret;
}
gurobi_set_basis(mdl);
if (GRBoptimize(mdl)) {
//intbasis_fail(d, "optimize()");
goto badret;
}
if (GRBgetintattr(mdl, GRB_INT_ATTR_STATUS, &i)) {
//intbasis_fail(d, "getintattr()");
goto badret;
}
if (i != GRB_OPTIMAL) {
// if (i >= GRB_INFEASIBLE && i <= GRB_NUMERIC)
//dpf(d, "\nGRBoptimize of fixed model: %s.",
// statusname[i-GRB_INFEASIBLE]);
// else
//dpf(d, "\nSurprise status %d after GRBoptimize of fixed model.",
//i);
goto badret;
}
/* if (d->missing & 2 && (y = d->y0)
&& !GRBgetdblattrarray(mdl, GRB_DBL_ATTR_PI, 0, n_con, y)) {
d->y = y;
d->missing &= ~2;
}
if (!GRBgetdblattr(mdl, GRB_DBL_ATTR_ITERCOUNT, &f)) {
if (f > 0.)
// dpf(d, "\nplus %.0f simplex iteration%s for intbasis",
// f, "s" + (f == 1.));
}
*/
return mdl;
}
int
main(int argc,
char *argv[])
{
GRBenv *env = NULL;
GRBmodel *model = NULL;
int error = 0;
double sol[3];
int ind[3];
double val[3];
double obj[3];
char vtype[3];
int optimstatus;
double objval;
/* Create environment */
error = GRBloadenv(&env, "mip1.log");
if (error || env == NULL) {
fprintf(stderr, "Error: could not create environment\n");
exit(1);
}
/* Create an empty model */
error = GRBnewmodel(env, &model, "mip1", 0, NULL, NULL, NULL, NULL, NULL);
if (error) goto QUIT;
/* Add variables */
obj[0] = -1; obj[1] = -1; obj[2] = -2;
vtype[0] = GRB_BINARY; vtype[1] = GRB_BINARY; vtype[2] = GRB_BINARY;
error = GRBaddvars(model, 3, 0, NULL, NULL, NULL, obj, NULL, NULL, vtype,
NULL);
if (error) goto QUIT;
/* Integrate new variables */
error = GRBupdatemodel(model);
if (error) goto QUIT;
/* First constraint: x + 2 y + 3 z <= 4 */
ind[0] = 0; ind[1] = 1; ind[2] = 2;
val[0] = 1; val[1] = 2; val[2] = 3;
error = GRBaddconstr(model, 3, ind, val, GRB_LESS_EQUAL, 4.0, "c0");
if (error) goto QUIT;
/* Second constraint: x + y >= 1 */
ind[0] = 0; ind[1] = 1;
val[0] = 1; val[1] = 1;
error = GRBaddconstr(model, 2, ind, val, GRB_GREATER_EQUAL, 1.0, "c1");
if (error) goto QUIT;
/* Optimize model */
error = GRBoptimize(model);
if (error) goto QUIT;
/* Write model to 'mip1.lp' */
error = GRBwrite(model, "mip1.lp");
if (error) goto QUIT;
/* Capture solution information */
error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
if (error) goto QUIT;
error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
if (error) goto QUIT;
error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, 3, sol);
if (error) goto QUIT;
printf("\nOptimization complete\n");
if (optimstatus == GRB_OPTIMAL) {
printf("Optimal objective: %.4e\n", objval);
printf(" x=%.0f, y=%.0f, z=%.0f\n", sol[0], sol[1], sol[2]);
} else if (optimstatus == GRB_INF_OR_UNBD) {
printf("Model is infeasible or unbounded\n");
} else {
printf("Optimization was stopped early\n");
}
QUIT:
/* Error reporting */
if (error) {
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
return 0;
}
int
main(int argc,
char *argv[])
{
GRBenv *env = NULL;
GRBmodel *model = NULL;
int i, j, len, n, solcount;
int error = 0;
char name[100];
double *x = NULL;
double *y = NULL;
int *ind = NULL;
double *val = NULL;
struct callback_data mydata;
if (argc < 2) {
fprintf(stderr, "Usage: tsp_c size\n");
exit(1);
}
n = atoi(argv[1]);
if (n == 0) {
fprintf(stderr, "Argument must be a positive integer.\n");
} else if (n > 30) {
printf("It will be a challenge to solve a TSP this large.\n");
}
x = (double *) malloc(n*sizeof(double));
y = (double *) malloc(n*sizeof(double));
ind = (int *) malloc(n*sizeof(int));
val = (double *) malloc(n*sizeof(double));
if (x == NULL || y == NULL || ind == NULL || val == NULL) {
fprintf(stderr, "Out of memory\n");
exit(1);
}
/* Create random points */
for (i = 0; i < n; i++) {
x[i] = ((double) rand())/RAND_MAX;
y[i] = ((double) rand())/RAND_MAX;
}
/* Create environment */
error = GRBloadenv(&env, "tsp.log");
if (error || env == NULL) {
fprintf(stderr, "Error: could not create environment\n");
exit(1);
}
/* Create an empty model */
error = GRBnewmodel(env, &model, "tsp", 0, NULL, NULL, NULL, NULL, NULL);
if (error) goto QUIT;
/* Add variables - one for every pair of nodes */
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
sprintf(name, "x_%d_%d", i, j);
error = GRBaddvar(model, 0, NULL, NULL, distance(x, y, i, j),
0.0, 1.0, GRB_BINARY, name);
if (error) goto QUIT;
}
}
/* Integrate new variables */
error = GRBupdatemodel(model);
if (error) goto QUIT;
/* Degree-2 constraints */
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
ind[j] = i*n+j;
val[j] = 1.0;
}
sprintf(name, "deg2_%d", i);
error = GRBaddconstr(model, n, ind, val, GRB_EQUAL, 2, name);
if (error) goto QUIT;
}
/* Forbid edge from node back to itself */
for (i = 0; i < n; i++) {
error = GRBsetdblattrelement(model, GRB_DBL_ATTR_UB, i*n+i, 0);
if (error) goto QUIT;
}
/* Symmetric TSP */
for (i = 0; i < n; i++) {
for (j = 0; j < i; j++) {
ind[0] = i*n+j;
ind[1] = i+j*n;
val[0] = 1;
val[1] = -1;
error = GRBaddconstr(model, 2, ind, val, GRB_EQUAL, 0, NULL);
if (error) goto QUIT;
}
}
/* Set callback function */
mydata.n = n;
error = GRBsetcallbackfunc(model, subtourelim, (void *) &mydata);
if (error) goto QUIT;
/* Turn off dual reductions - required when using lazy constraints */
error = GRBsetintparam(GRBgetenv(model), GRB_INT_PAR_DUALREDUCTIONS, 0);
if (error) goto QUIT;
/* Optimize model */
error = GRBoptimize(model);
if (error) goto QUIT;
/* Extract solution */
error = GRBgetintattr(model, GRB_INT_ATTR_SOLCOUNT, &solcount);
if (error) goto QUIT;
if (solcount > 0) {
int *tour = NULL;
double *sol = NULL;
sol = (double *) malloc(n*n*sizeof(double));
tour = (int *) malloc(n*sizeof(int));
if (sol == NULL || tour == NULL) {
fprintf(stderr, "Out of memory\n");
exit(1);
}
error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, n*n, sol);
if (error) goto QUIT;
/* Print tour */
findsubtour(n, sol, &len, tour);
printf("Tour: ");
for (i = 0; i < len; i++)
printf("%d ", tour[i]);
printf("\n");
free(tour);
free(sol);
}
QUIT:
/* Free data */
free(x);
free(y);
free(ind);
free(val);
/* Error reporting */
if (error) {
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
return 0;
}
int
main(int argc,
char *argv[])
{
GRBenv *env = NULL;
GRBmodel *model = NULL;
int error = 0, status;
int s, w, col;
int *cbeg = NULL;
int *cind = NULL;
int idx;
double *cval = NULL;
char *sense = NULL;
char vname[MAXSTR];
double obj;
int i, j, numvars, numconstrs;
int *vbeg = NULL;
int *vind = NULL;
double *vval = NULL;
double *vobj = NULL;
double sol;
char *cname, *sname;
int varnamesct = 0;
char **varnames = NULL;
/* Sample data */
const int nShifts = 14;
const int nWorkers = 7;
/* Sets of days and workers */
char* Shifts[] =
{ "Mon1", "Tue2", "Wed3", "Thu4", "Fri5", "Sat6",
"Sun7", "Mon8", "Tue9", "Wed10", "Thu11", "Fri12", "Sat13",
"Sun14" };
char* Workers[] =
{ "Amy", "Bob", "Cathy", "Dan", "Ed", "Fred", "Gu" };
/* Number of workers required for each shift */
double shiftRequirements[] =
{ 3, 2, 4, 4, 5, 6, 5, 2, 2, 3, 4, 6, 7, 5 };
/* Amount each worker is paid to work one shift */
double pay[] = { 10, 12, 10, 8, 8, 9, 11 };
/* Worker availability: 0 if the worker is unavailable for a shift */
double availability[][14] =
{ { 0, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1 },
{ 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 0 },
{ 0, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1 },
{ 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1 },
{ 1, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 1 },
{ 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 } };
/* Create environment */
error = GRBloadenv(&env, "workforce3.log");
if (error || env == NULL)
{
fprintf(stderr, "Error: could not create environment\n");
exit(1);
}
/* Create initial model */
error = GRBnewmodel(env, &model, "workforce3", nWorkers * nShifts,
NULL, NULL, NULL, NULL, NULL);
if (error) goto QUIT;
/* Initialize assignment decision variables:
x[w][s] == 1 if worker w is assigned
to shift s. Since an assignment model always produces integer
solutions, we use continuous variables and solve as an LP. */
for (w = 0; w < nWorkers; ++w)
{
for (s = 0; s < nShifts; ++s)
{
col = xcol(w, s);
sprintf(vname, "%s.%s", Workers[w], Shifts[s]);
error = GRBsetdblattrelement(model, "UB", col, availability[w][s]);
if (error) goto QUIT;
error = GRBsetdblattrelement(model, "Obj", col, pay[w]);
if (error) goto QUIT;
error = GRBsetstrattrelement(model, "VarName", col, vname);
if (error) goto QUIT;
}
}
/* The objective is to minimize the total pay costs */
error = GRBsetintattr(model, "ModelSense", 1);
if (error) goto QUIT;
/* Make space for constraint data */
cbeg = malloc(sizeof(int) * nShifts);
if (!cbeg) goto QUIT;
cind = malloc(sizeof(int) * nShifts * nWorkers);
if (!cind) goto QUIT;
cval = malloc(sizeof(double) * nShifts * nWorkers);
if (!cval) goto QUIT;
sense = malloc(sizeof(char) * nShifts);
if (!sense) goto QUIT;
/* Constraint: assign exactly shiftRequirements[s] workers
to each shift s */
idx = 0;
for (s = 0; s < nShifts; ++s)
{
cbeg[s] = idx;
sense[s] = GRB_EQUAL;
for (w = 0; w < nWorkers; ++w)
{
cind[idx] = xcol(w, s);
cval[idx++] = 1.0;
}
}
error = GRBaddconstrs(model, nShifts, idx, cbeg, cind, cval, sense,
shiftRequirements, Shifts);
if (error) goto QUIT;
/* Optimize */
error = GRBoptimize(model);
if (error) goto QUIT;
error = GRBgetintattr(model, "Status", &status);
if (error) goto QUIT;
if (status == GRB_UNBOUNDED)
{
printf("The model cannot be solved because it is unbounded\n");
goto QUIT;
}
if (status == GRB_OPTIMAL)
{
error = GRBgetdblattr(model, "ObjVal", &obj);
if (error) goto QUIT;
printf("The optimal objective is %f\n", obj);
goto QUIT;
}
if ((status != GRB_INF_OR_UNBD) && (status != GRB_INFEASIBLE))
{
printf("Optimization was stopped with status %i\n", status);
goto QUIT;
}
/* Add slack variables to make the model feasible */
printf("The model is infeasible; adding slack variables\n");
/* Determine the matrix size before adding the slacks */
error = GRBgetintattr(model, "NumVars", &numvars);
if (error) goto QUIT;
error = GRBgetintattr(model, "NumConstrs", &numconstrs);
if (error) goto QUIT;
/* Set original objective coefficients to zero */
for (j = 0; j < numvars; ++j)
{
error = GRBsetdblattrelement(model, "Obj", j, 0.0);
if (error) goto QUIT;
}
/* Add a new slack variable to each shift constraint so that the shifts
can be satisfied */
vbeg = malloc(sizeof(int) * numconstrs);
if (!vbeg) goto QUIT;
vind = malloc(sizeof(int) * numconstrs);
if (!vind) goto QUIT;
vval = malloc(sizeof(double) * numconstrs);
if (!vval) goto QUIT;
vobj = malloc(sizeof(double) * numconstrs);
if (!vobj) goto QUIT;
varnames = calloc(numconstrs, sizeof(char*));
if (!varnames) goto QUIT;
for (i = 0; i < numconstrs; ++i)
{
vbeg[i] = i;
vind[i] = i;
vval[i] = 1.0;
vobj[i] = 1.0;
error = GRBgetstrattrelement(model, "ConstrName", i, &cname);
if (error) goto QUIT;
varnames[i] = malloc(sizeof(char*) * (6 + strlen(cname)));
if (!varnames[i]) goto QUIT;
varnamesct++;
strcpy(varnames[i], cname);
strcat(varnames[i], "Slack");
}
error = GRBaddvars(model, numconstrs, numconstrs,
vbeg, vind, vval, vobj, NULL, NULL, NULL, varnames);
if (error) goto QUIT;
error = GRBupdatemodel(model);
if (error) goto QUIT;
/* Solve the model with slacks */
error = GRBoptimize(model);
if (error) goto QUIT;
error = GRBgetintattr(model, "Status", &status);
if (error) goto QUIT;
if ((status == GRB_INF_OR_UNBD) || (status == GRB_INFEASIBLE) ||
(status == GRB_UNBOUNDED))
{
printf("The model with slacks cannot be solved "
"because it is infeasible or unbounded\n");
goto QUIT;
}
if (status != GRB_OPTIMAL)
{
printf("Optimization was stopped with status %i\n", status);
goto QUIT;
}
printf("\nSlack values:\n");
for (j = numvars; j < numvars + numconstrs; ++j)
{
error = GRBgetdblattrelement(model, "X", j, &sol);
if (error) goto QUIT;
if (sol > 1e-6)
{
error = GRBgetstrattrelement(model, "VarName", j, &sname);
if (error) goto QUIT;
printf("%s = %f\n", sname, sol);
}
}
QUIT:
/* Error reporting */
if (error)
{
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free data */
free(cbeg);
free(cind);
free(cval);
free(sense);
free(vbeg);
free(vind);
free(vval);
free(vobj);
for (i = 0; i < varnamesct; ++i)
{
free(varnames[i]);
}
free(varnames);
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
return 0;
}
int main2(int argc, char *argv[]){
char * file_model;
char * file_data;
if (argc < 5){
usage();
exit(1);
}
file_model = file_data = NULL;
GRB_out = GLPK_out = verbose = 1;
for(int i=1 ; i<=argc-1 ; i++){
if (strcmp(argv[i],"-m")==0) file_model = argv[i+1];
if (strcmp(argv[i],"-d")==0) file_data = argv[i+1];
if (strcmp(argv[i],"-v")==0) verbose = 1;
if (strcmp(argv[i],"--glpk_out")==0) GLPK_out = 1;
if (strcmp(argv[i],"--grb_out")==0) GRB_out = 1;
if (strcmp(argv[i],"--glpk_mip_gap")==0) glpk_iparm_mip_gap = atof(argv[i+1]);
if (strcmp(argv[i],"--glpk_tol_int")==0) glpk_iparm_tol_int = atof(argv[i+1]);
if (strcmp(argv[i],"--glpk_tol_obj")==0) glpk_iparm_tol_obj = atof(argv[i+1]);
}
if ((file_model==NULL) || (file_data == NULL)){
usage();
fprintf(stderr, "Error no model or data files provided\n");
freeMem();
}
/** GLPK: Open environment **/
mip = glp_create_prob();
tran = glp_mpl_alloc_wksp();
glp_term_out(GLPK_out?GLP_ON:GLP_OFF);
/** GLPK: Read model written in MathProg **/
ret = glp_mpl_read_model(tran, file_model, 1);
if (ret){
fprintf(stderr, "Error on translating model\n");
freeMem();
}
/** GLPK: Read data for MathProg **/
ret = glp_mpl_read_data(tran, file_data);
if (ret){
fprintf(stderr, "Error on translating data\n");
freeMem();
}
/** GLPK: Generate model (merge data an model) **/
ret = glp_mpl_generate(tran, NULL);
if (ret){
fprintf(stderr, "Error on generating model\n");
freeMem();
}
/** GLPK: Generate Build Model **/
glp_mpl_build_prob(tran, mip);
wrapper_params wpar;
wpar.grb_out = GRB_out;
wpar.glp_out = GLPK_out;
solve_glp_grb(mip,&wpar);
/** GLPK: Perform postprocessing **/
ret = glp_mpl_postsolve(tran, mip, GLP_MIP);
if (ret != 0) fprintf(stderr, "Error on postsolving model\n");
/** GLPK: free structures **/
if (tran) glp_mpl_free_wksp(tran);
if (mip) glp_delete_prob(mip);
if (retGRB) printf("ERROR: %s\n", GRBgeterrormsg(env));
/** GUROBI: free structures **/
if (model) GRBfreemodel(model);
if (env) GRBfreeenv(env);
printf("Done.\nGLPK -> GUROBI -> GLPK wrapper v0.1 (2010)\n");
exit(0);
}
double solve_glp_grb(glp_prob *mip, wrapper_params *par){
GLPK_out = par->glp_out;
GRB_out = par->grb_out;
double obj_val;
/** GLPK: Generate Variable indexing **/
glp_create_index(mip);
/** GLPK: Generate LP **/
glp_write_mps(mip, GLP_MPS_FILE, NULL, "tmp.mps");
/************/
/** GUROBI **/
/************/
retGRB = GRBloadenv(&env, NULL);
if (retGRB || env == NULL)
{
fprintf(stderr, "Error: could not create environment\n");
exit(1);
}
retGRB = GRBsetintparam(env, "OutputFlag", GRB_out?1:0);
if (retGRB) freeMem();
//retGRB = GRBsetintparam(env, "Sensitivity", 1);
//if (retGRB) freeMem();
/** GUROBI: Read model **/
retGRB = GRBreadmodel(env, "tmp.mps", &model);
if (retGRB) freeMem();
/** Remove utility files from disk **/
//remove("tmp.mps");
/** GUROBI: Get environment **/
mipenv = GRBgetenv(model);
if (!mipenv) freeMem();
/** GUROBI: Set parameters **/
/** GUROBI: Ask for more precision **/
retGRB = GRBsetdblparam(mipenv, "FeasibilityTol", 10E-6);
if (retGRB) freeMem();
retGRB = GRBsetdblparam(mipenv, "IntFeasTol", 10E-5);
if (retGRB) freeMem();
retGRB = GRBsetdblparam(mipenv, "MIPgap", 10E-6);
if (retGRB) freeMem();
/* * Playing with gurobi parameters and attr*/
//gurobi_set_basis();
retGRB = GRBsetintparam(mipenv, "Cuts", 3);
if (retGRB) freeMem();
retGRB = GRBsetintparam(mipenv, "RootMethod", 1);
if (retGRB) freeMem();
retGRB = GRBsetintparam(mipenv, "Symmetry", -1);
if (retGRB) freeMem();
/** GUROBI: get numvars and numrows **/
retGRB = GRBgetintattr(model, "NumVars", &numvars);
if (retGRB) freeMem();
/** Test variable names */
for(int j=0;j<numvars;j++){
retGRB = GRBgetstrattrelement(model, "VarName", j, &nameGRB);
printf("GRB Var %d Name %s\n",j,nameGRB);
}
/** GUROBI: get model type **/
retGRB = GRBgetintattr(model, "IsMIP", &GRB_IsMIP);
if (retGRB) freeMem();
/** GUROBI: Optimize model **/
retGRB = GRBoptimize(model);
if (retGRB) freeMem();
/** GUROBI: Retreive the optimization status **/
GRBgetintattr(model, "Status", &retGRB);
switch(retGRB){
case GRB_OPTIMAL:
break;
case GRB_INFEASIBLE :
fprintf(stderr, "Error GRB optimization failed with code GRB_INFEASIBLE\n");
case GRB_INF_OR_UNBD :
fprintf(stderr, "Error GRB optimization failed with code GRB_INF_OR_UNBD \n");
case GRB_UNBOUNDED :
fprintf(stderr, "Error GRB optimization failed with code GRB_UNBOUNDED \n");
case GRB_CUTOFF :
fprintf(stderr, "Error GRB optimization failed with code GRB_CUTOFF \n");
case GRB_ITERATION_LIMIT :
fprintf(stderr, "Error GRB optimization failed with code GRB_ITERATION_LIMIT \n");
case GRB_NODE_LIMIT :
fprintf(stderr, "Error GRB optimization failed with code GRB_NODE_LIMIT \n");
case GRB_TIME_LIMIT :
fprintf(stderr, "Error GRB optimization failed with code GRB_TIME_LIMIT \n");
case GRB_SOLUTION_LIMIT :
fprintf(stderr, "Error GRB optimization failed with code GRB_SOLUTION_LIMIT \n");
case GRB_INTERRUPTED :
fprintf(stderr, "Error GRB optimization failed with code GRB_INTERRUPTED \n");
case GRB_SUBOPTIMAL :
fprintf(stderr, "Error GRB optimization failed with code GRB_SUBOPTIMAL \n");
case GRB_NUMERIC :
fprintf(stderr, "Error GRB optimization failed with code GRB_NUMERIC \n");
/** GUROBI: Quit in any case non optimal **/
freeMem();
}
/** GUROBI: Get obj function value **/
retGRB = GRBgetdblattr(model, "IntVio", &tmp);
if (retGRB) freeMem();
retGRB = GRBgetdblattr(model, "ObjBound", &bound);
if (retGRB) freeMem();
retGRB = GRBgetdblattr(model, "ObjVal", &tmp);
if (retGRB) freeMem();
/* ********************** */
obj_val = tmp;
/* ************ */
if (verbose) printf ("Objective %lf\n", tmp);
if (verbose) printf ("Best bound %lf\n", bound);
if (verbose) printf ("Absolute gap %lf\n", fabs(tmp - bound));
/** GUROBI: Get variable values **/
for (j = 0; j < numvars; ++j){
retGRB = GRBgetdblattrelement(model, "X", j, &tmp);
if (retGRB) freeMem();
retGRB = GRBgetstrattrelement(model, "VarName", j, &nameGRB);
printf("GRB Var %d Name %s\n",j,nameGRB);
if (retGRB) freeMem();
retGRB = GRBgetcharattrelement(model, "VType", j, &type);
if (retGRB) freeMem();
/** GLPK search variable index by name **/
col_index = glp_find_col(mip, nameGRB);
if (col_index != 0){
/** GLPK set variable bounds **/
if ((type == 'B') || (type == 'I')){
if (verbose) printf ("Variable %s is of type %c value %lf fixed to %lf\n", nameGRB, type, tmp, round(tmp));
glp_set_col_bnds(mip, col_index, GLP_FX, round(tmp), round(tmp));
}
else{
if (verbose) printf ("Variable %s is of type %c value %lf fixed to %lf\n", nameGRB, type, tmp, tmp);
glp_set_col_bnds(mip, col_index, GLP_FX, tmp, tmp);
}
}
}
if (GRB_IsMIP){
/** GLPK initialize parameters **/
iparm = (glp_iocp*) malloc(sizeof(glp_iocp));
glp_init_iocp(iparm);
iparm->presolve = GLP_ON;
iparm->mip_gap = glpk_iparm_mip_gap;
iparm->tol_int = glpk_iparm_tol_int;
iparm->tol_obj = glpk_iparm_tol_obj;
/** GLPK get the optimal integer solution **/
ret = glp_intopt(mip, iparm);
if (ret){
fprintf(stderr, "glp_intopt, Error on optimizing the model : %d \n", ret);
freeMem();
}
ret = glp_mip_status(mip);
switch (ret){
case GLP_OPT:
break;
case GLP_FEAS:
fprintf(stderr, "Error GLPK simplex is not optimal, GLP_FEAS, code %d\n", ret);
freeMem();
case GLP_NOFEAS:
fprintf(stderr, "Error GLPK simplex is not optimal, GLP_NOFEAS, code %d\n", ret);
freeMem();
case GLP_UNDEF:
fprintf(stderr, "Error GLPK simplex is not optimal, GLP_UNDEF, code %d\n", ret);
freeMem();
}
}
else{
/*GLPK initialize parameters */
parm = (glp_smcp*) malloc(sizeof(glp_smcp));
glp_init_smcp(parm);
parm->meth = GLP_DUALP;
parm->tol_bnd = 10E-4;
parm->tol_dj = 10E-4;
/* GLPK get the optimal basis */
//ret = glp_simplex(mip, parm);
if (ret){
fprintf(stderr, "glp_simplex, Error on optimizing the model : %d \n", ret);
freeMem();
}
ret = glp_get_status(mip);
switch (ret){
case GLP_OPT:
break;
case GLP_FEAS:
fprintf(stderr, "Error GLPK simplex is not optimal, GLP_FEAS, code %d\n", ret);
freeMem();
case GLP_INFEAS:
fprintf(stderr, "Error GLPK simplex is not optimal, GLP_INFEAS, code %d\n", ret);
freeMem();
case GLP_NOFEAS:
fprintf(stderr, "Error GLPK simplex is not optimal, GLP_NOFEAS, code %d\n", ret);
freeMem();
case GLP_UNBND:
fprintf(stderr, "Error GLPK simplex is not optimal, GLP_UNBND, code %d\n", ret);
freeMem();
case GLP_UNDEF:
fprintf(stderr, "Error GLPK simplex is not optimal, GLP_UNDEF, code %d\n", ret);
freeMem();
}
}
//GRBmodel *fmod = fixed_model(model);
//gurobi_sens_output(fmod, "/tmp/sens.sol");
GRBwrite(model, "/tmp/model.sol");
/** GUROBI: free structures **/
if (model) GRBfreemodel(model);
if (env) GRBfreeenv(env);
return obj_val;
}
int
main(int argc,
char *argv[])
{
GRBenv *masterenv = NULL;
GRBmodel *model = NULL;
GRBenv *modelenv = NULL;
int error = 0;
int optimstatus;
double objval;
if (argc < 2) {
fprintf(stderr, "Usage: lp_c filename\n");
exit(1);
}
/* Create environment */
error = GRBloadenv(&masterenv, "lp.log");
if (error) goto QUIT;
/* Read model from file */
error = GRBreadmodel(masterenv, argv[1], &model);
if (error) goto QUIT;
/* Solve model */
error = GRBoptimize(model);
if (error) goto QUIT;
/* Capture solution information */
error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
if (error) goto QUIT;
/* If model is infeasible or unbounded, turn off presolve and resolve */
if (optimstatus == GRB_INF_OR_UNBD) {
modelenv = GRBgetenv(model);
if (!modelenv) {
fprintf(stderr, "Error: could not get model environment\n");
goto QUIT;
}
/* Change parameter on model environment. The model now has
a copy of the master environment, so changing the master will
no longer affect the model. */
error = GRBsetintparam(modelenv, "PRESOLVE", 0);
if (error) goto QUIT;
error = GRBoptimize(model);
if (error) goto QUIT;
error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
if (error) goto QUIT;
}
if (optimstatus == GRB_OPTIMAL) {
error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
if (error) goto QUIT;
printf("Optimal objective: %.4e\n\n", objval);
} else if (optimstatus == GRB_INFEASIBLE) {
printf("Model is infeasible\n\n");
error = GRBcomputeIIS(model);
if (error) goto QUIT;
error = GRBwrite(model, "model.ilp");
if (error) goto QUIT;
} else if (optimstatus == GRB_UNBOUNDED) {
printf("Model is unbounded\n\n");
} else {
printf("Optimization was stopped with status = %d\n\n", optimstatus);
}
QUIT:
/* Error reporting */
if (error) {
printf("ERROR: %s\n", GRBgeterrormsg(masterenv));
exit(1);
}
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(masterenv);
return 0;
}
int
main(int argc,
char *argv[])
{
GRBenv *env = NULL, *aenv;
GRBmodel *a = NULL, *b = NULL;
int error = 0;
int i, numvars, status;
char vtype, *vname;
double x, bnd, aobj, bobj, objchg;
if (argc < 2)
{
fprintf(stderr, "Usage: sensitivity_c filename\n");
exit(1);
}
error = GRBloadenv(&env, "sensitivity.log");
if (error) goto QUIT;
/* Read model */
error = GRBreadmodel(env, argv[1], &a);
if (error) goto QUIT;
error = GRBoptimize(a);
if (error) goto QUIT;
error = GRBgetdblattr(a, "ObjVal", &aobj);
if (error) goto QUIT;
aenv = GRBgetenv(a);
if (!aenv) goto QUIT;
error = GRBsetintparam(aenv, "OutputFlag", 0);
if (error) goto QUIT;
/* Iterate over all variables */
error = GRBgetintattr(a, "NumVars", &numvars);
if (error) goto QUIT;
for (i = 0; i < numvars; ++i)
{
error = GRBgetcharattrelement(a, "VType", i, &vtype);
if (error) goto QUIT;
if (vtype == GRB_BINARY)
{
/* Create clone and fix variable */
b = GRBcopymodel(a);
if (!b) goto QUIT;
error = GRBgetstrattrelement(a, "VarName", i, &vname);
if (error) goto QUIT;
error = GRBgetdblattrelement(a, "X", i, &x);
if (error) goto QUIT;
error = GRBgetdblattrelement(a, "LB", i, &bnd);
if (error) goto QUIT;
if (x - bnd < 0.5)
{
error = GRBgetdblattrelement(b, "UB", i, &bnd);
if (error) goto QUIT;
error = GRBsetdblattrelement(b, "LB", i, bnd);
if (error) goto QUIT;
}
else
{
error = GRBgetdblattrelement(b, "LB", i, &bnd);
if (error) goto QUIT;
error = GRBsetdblattrelement(b, "UB", i, bnd);
if (error) goto QUIT;
}
error = GRBoptimize(b);
if (error) goto QUIT;
error = GRBgetintattr(b, "Status", &status);
if (error) goto QUIT;
if (status == GRB_OPTIMAL)
{
error = GRBgetdblattr(b, "ObjVal", &bobj);
if (error) goto QUIT;
objchg = bobj - aobj;
if (objchg < 0)
{
objchg = 0;
}
printf("Objective sensitivity for variable %s is %f\n", vname, objchg);
}
else
{
printf("Objective sensitivity for variable %s is infinite\n", vname);
}
GRBfreemodel(b);
b = NULL;
}
}
QUIT:
/* Error reporting */
if (error)
{
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free models */
GRBfreemodel(a);
GRBfreemodel(b);
/* Free environment */
GRBfreeenv(env);
return 0;
}
void GurobiInterface::
eval(void* mem, const double** arg, double** res, int* iw, double* w) const {
auto m = static_cast<GurobiMemory*>(mem);
// Inputs
const double *h=arg[CONIC_H],
*g=arg[CONIC_G],
*a=arg[CONIC_A],
*lba=arg[CONIC_LBA],
*uba=arg[CONIC_UBA],
*lbx=arg[CONIC_LBX],
*ubx=arg[CONIC_UBX],
*x0=arg[CONIC_X0],
*lam_x0=arg[CONIC_LAM_X0];
// Outputs
double *x=res[CONIC_X],
*cost=res[CONIC_COST],
*lam_a=res[CONIC_LAM_A],
*lam_x=res[CONIC_LAM_X];
// Temporary memory
double *val=w; w+=nx_;
int *ind=iw; iw+=nx_;
int *ind2=iw; iw+=nx_;
int *tr_ind=iw; iw+=nx_;
// Greate an empty model
GRBmodel *model = 0;
try {
int flag = GRBnewmodel(m->env, &model, name_.c_str(), 0, 0, 0, 0, 0, 0);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
// Add variables
for (int i=0; i<nx_; ++i) {
// Get bounds
double lb = lbx ? lbx[i] : 0., ub = ubx ? ubx[i] : 0.;
if (isinf(lb)) lb = -GRB_INFINITY;
if (isinf(ub)) ub = GRB_INFINITY;
// Get variable type
char vtype;
if (!vtype_.empty()) {
// Explicitly set 'vtype' takes precedence
vtype = vtype_.at(i);
} else if (!discrete_.empty() && discrete_.at(i)) {
// Variable marked as discrete (integer or binary)
vtype = lb==0 && ub==1 ? GRB_BINARY : GRB_INTEGER;
} else {
// Continious variable
vtype = GRB_CONTINUOUS;
}
// Pass to model
flag = GRBaddvar(model, 0, 0, 0, g ? g[i] : 0., lb, ub, vtype, 0);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
}
flag = GRBupdatemodel(model);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
// Add quadratic terms
const int *H_colind=sparsity_in(CONIC_H).colind(), *H_row=sparsity_in(CONIC_H).row();
for (int i=0; i<nx_; ++i) {
// Quadratic term nonzero indices
int numqnz = H_colind[1]-H_colind[0];
casadi_copy(H_row, numqnz, ind);
H_colind++;
H_row += numqnz;
// Corresponding column
casadi_fill(ind2, numqnz, i);
// Quadratic term nonzeros
if (h) {
casadi_copy(h, numqnz, val);
casadi_scal(numqnz, 0.5, val);
h += numqnz;
} else {
casadi_fill(val, numqnz, 0.);
}
// Pass to model
flag = GRBaddqpterms(model, numqnz, ind, ind2, val);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
}
// Add constraints
const int *A_colind=sparsity_in(CONIC_A).colind(), *A_row=sparsity_in(CONIC_A).row();
casadi_copy(A_colind, nx_, tr_ind);
for (int i=0; i<na_; ++i) {
// Get bounds
double lb = lba ? lba[i] : 0., ub = uba ? uba[i] : 0.;
// if (isinf(lb)) lb = -GRB_INFINITY;
// if (isinf(ub)) ub = GRB_INFINITY;
// Constraint nonzeros
int numnz = 0;
for (int j=0; j<nx_; ++j) {
if (tr_ind[j]<A_colind[j+1] && A_row[tr_ind[j]]==i) {
ind[numnz] = j;
val[numnz] = a ? a[tr_ind[j]] : 0;
numnz++;
tr_ind[j]++;
}
}
// Pass to model
if (isinf(lb)) {
if (isinf(ub)) {
// Neither upper or lower bounds, skip
} else {
// Only upper bound
flag = GRBaddconstr(model, numnz, ind, val, GRB_LESS_EQUAL, ub, 0);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
}
} else {
if (isinf(ub)) {
// Only lower bound
flag = GRBaddconstr(model, numnz, ind, val, GRB_GREATER_EQUAL, lb, 0);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
} else if (lb==ub) {
// Upper and lower bounds equal
flag = GRBaddconstr(model, numnz, ind, val, GRB_EQUAL, lb, 0);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
} else {
// Both upper and lower bounds
flag = GRBaddrangeconstr(model, numnz, ind, val, lb, ub, 0);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
}
}
}
// Solve the optimization problem
flag = GRBoptimize(model);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
int optimstatus;
flag = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
// Get the objective value, if requested
if (cost) {
flag = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, cost);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
}
// Get the optimal solution, if requested
if (x) {
flag = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, nx_, x);
casadi_assert_message(!flag, GRBgeterrormsg(m->env));
}
// Free memory
GRBfreemodel(model);
} catch (...) {
// Free memory
if (model) GRBfreemodel(model);
throw;
}
}
int main(void)
{
int retcode = 0;
GRBenv *env = NULL;
GRBmodel *model = NULL;
int n, j;
double *obj = NULL;
double *lb = NULL;
double *ub = NULL;
double *x;
int *qrow, *qcol, Nq;
double *qval;
int *cind;
double rhs;
char sense;
double *cval;
int numnonz;
char **names;
n = 9; /** 7 'x' variables, 2 factor variables **/
retcode = GRBloadenv(&env, "factormodel.log");
if (retcode) goto BACK;
/* Create initial model */
retcode = GRBnewmodel(env, &model, "second", n,
NULL, NULL, NULL, NULL, NULL);
if (retcode) goto BACK;
names = (char **) calloc(n, sizeof(char *));
/** next we create the remaining attributes for the n columns **/
obj = (double *) calloc (n, sizeof(double));
ub = (double *) calloc (n, sizeof(double));
lb = (double *) calloc (n, sizeof(double));
x = (double *) calloc (n, sizeof(double));
for(j = 0; j < 7; j++){
names[j] = (char *)calloc(3, sizeof(char));
if(names[j] == NULL){
retcode = 1; goto BACK;
}
sprintf(names[j],"x%d", j);
}
for(j = 7; j < 9; j++){
names[j] = (char *)calloc(3, sizeof(char));
if(names[j] == NULL){
retcode = 1; goto BACK;
}
sprintf(names[j],"y%d", j - 7);
}
obj[0] = -.233; obj[1] = -3.422; obj[2] = -.1904; obj[3] = -.5411;
obj[4] = -.045; obj[5] = -1.271; obj[6] = -0.955;
/** calloc initializes memory to zero, so all other obj[j] are zero **/
/**next, the upper bounds on the x variables **/
ub[0] = 0.6; ub[1] = 0.8; ub[2] = 0.8; ub[3] = 0.5;
ub[4] = 0.5; ub[5] = 0.26; ub[6] = 0.99;
/** the upper bounds on the two factor variables -- we make them large **/
ub[7] = 100; ub[8] = 100;
/** the lower bounds on the factor variables **/
lb[7] = -100; lb[8] = -100;
/* initialize variables */
for(j = 0; j < n; j++){
retcode = GRBsetstrattrelement(model, "VarName", j, names[j]);
if (retcode) goto BACK;
retcode = GRBsetdblattrelement(model, "Obj", j, obj[j]);
if (retcode) goto BACK;
retcode = GRBsetdblattrelement(model, "LB", j, lb[j]);
if (retcode) goto BACK;
retcode = GRBsetdblattrelement(model, "UB", j, ub[j]);
if (retcode) goto BACK;
}
/** next, the quadratic -- there are 11 nonzeroes: 7 residual variances plus the 2x2
factor covariance matrix**/
Nq = 11;
qrow = (int *) calloc(Nq, sizeof(int)); /** row indices **/
qcol = (int *) calloc(Nq, sizeof(int)); /** column indices **/
qval = (double *) calloc(Nq, sizeof(double)); /** values **/
if( ( qrow == NULL) || ( qcol == NULL) || (qval == NULL) ){
printf("could not create quadratic\n");
retcode = 1; goto BACK;
}
qval[0] = 10.0; qrow[0] = 0; qcol[0] = 0;
qval[1] = 20.0; qrow[1] = 1; qcol[1] = 1;
qval[2] = 30.0; qrow[2] = 2; qcol[2] = 2;
qval[3] = 40.0; qrow[3] = 3; qcol[3] = 3;
qval[4] = 50.0; qrow[4] = 4; qcol[4] = 4;
qval[5] = 60.0; qrow[5] = 5; qcol[5] = 5;
qval[6] = 70.0; qrow[6] = 6; qcol[6] = 6;
qval[7] = 100.0; qrow[7] = 7; qcol[7] = 7;
qval[8] = 200.0; qrow[8] = 8; qcol[8] = 8;
qval[9] = 0.1; qrow[9] = 7; qcol[9] = 8;
qval[10] = 0.1; qrow[10] = 8; qcol[10] = 7;
retcode = GRBaddqpterms(model, 11, qrow, qcol, qval);
if (retcode) goto BACK;
/** now we will add one constraint at a time **/
/** we need to have a couple of auxiliary arrays **/
cind = (int *)calloc(n, sizeof(int)); /** n is over the top since no constraint is totally dense;
but it's not too bad here **/
cval= (double *)calloc(n, sizeof(double));
/** two factor constraints, first one is next**/
cval[0] = 1.508; cval[1] = .7802; cval[2] = 1.8796;
cval[3] = 4.256; cval[4] = 1.335; cval[5] = 2.026; cval[6] = 1.909;
cval[7] = -1;
for(j = 0; j < 7; j++) cind[j] = j;
cind[7] = 7;
numnonz = 8;
rhs = 0;
sense = GRB_EQUAL;
retcode = GRBaddconstr(model, numnonz, cind, cval, sense, rhs, "first_constraint");
if (retcode) goto BACK;
/** second factor constraint **/
cval[0] = 4.228; cval[1] = 1.2945; cval[2] = .827;
cval[3] = 2.149;
cval[4] = 2.353; cval[5] = 0.3026; cval[6] = 1.487;
cval[7] = -1;
for(j = 0; j < 7; j++) cind[j] = j; /** redundant! but let's keep it here so that we know it
will be used **/
cind[7] = 7;
numnonz = 8;
rhs = 0;
sense = GRB_EQUAL;
retcode = GRBaddconstr(model, numnonz, cind, cval, sense, rhs, "second_constraint");
if (retcode) goto BACK;
/** sum of x variables = 1 **/
cval[0] = 1.0; cval[1] = 1.0; cval[2] = 1.0;
cval[3] = 1.0; cval[4] = 1.0; cval[5] = 1.0; cval[6] = 1.0;
for(j = 0; j < 7; j++) cind[j] = j;
numnonz = 7;
rhs = 1.0;
sense = GRB_EQUAL;
retcode = GRBaddconstr(model, numnonz, cind, cval, sense, rhs, "convexity");
if (retcode) goto BACK;
retcode = GRBupdatemodel(model);
if (retcode) goto BACK;
/** optional: write the problem **/
retcode = GRBwrite(model, "factorqp.lp");
if (retcode) goto BACK;
retcode = GRBoptimize(model);
if (retcode) goto BACK;
/** get solution **/
retcode = GRBgetdblattrarray(model,
GRB_DBL_ATTR_X, 0, n,
x);
if(retcode) goto BACK;
/** now let's see the values **/
for(j = 0; j < n; j++){
printf("%s = %g\n", names[j], x[j]);
}
GRBfreemodel(model);
GRBfreeenv(env);
BACK:
printf("\nexiting with retcode %d\n", retcode);
return retcode;
}
