# Invalid solutions to Piecewise Mccormick Envelope Implementation

I am currently trying to implement a piecewise McCormick envelope in Drake (c++). The current issue I am having is that the solution produced by the optimization does not produce a valid $$x$$ and $$y$$ values (outside my specified bounds or infeasible) and "$$w$$" is not close to $$x*y$$

I have used several references such as A Global Quasi-Dynamic Model for Contact-Trajectory Optimization, McCormick envelopes Northwestern Wiki, and more, but I have based off the code below from Mixed-Integer Convex Optimization for Planning Aggressive Motions of Legged Robots Over Rough Terrain.

A few things that I felt were missing from this particular paper was the sum of the binary decision variable $$z$$ equal to 1. If I'm not mistaken, one piece of the piecewise McCormick envelope gets chosen by the optimization, thus, the sum of $$z$$ should be equal to 1.

Gurobi was used to solve this problem.

I would like to know if I am missing any major components to the piecewise McCormick envelope.

    solvers::MathematicalProgram prog = solvers::MathematicalProgram();

// For this problem, M < 4 gives a solution. However, it may be incorrect.
// M >= 4 makes the problem infeasible
const int M = 6; //number of partitions

solvers::VectorDecisionVariable<1> x = prog.NewContinuousVariables<1>("x");
solvers::VectorDecisionVariable<1> y = prog.NewContinuousVariables<1>("y");
solvers::VectorXDecisionVariable x_hat = prog.NewContinuousVariables(M, "x_hat");
solvers::VectorXDecisionVariable y_hat = prog.NewContinuousVariables(M, "y_hat");
solvers::VectorDecisionVariable<1> w = prog.NewContinuousVariables<1>("w");
solvers::VectorXDecisionVariable z = prog.NewBinaryVariables(M, "z");
//solvers::VectorDecisionVariable<1> lorentz = prog.NewContinuousVariables<1>("lorentz");

const double x_upper = 1;
const double x_lower = 0;

// Replacing quadratic cost with rotated lorentz cone in the future. (developer's suggesting for better results)
//   symbolic::Expression(lorentz), symbolic::Expression(1), (0.4-w)*2*(0.4-w) + (0.5-x)*10*(0.5-x)
// );

//Min 2*(0.4-w)^2 + 10*(0.5-x)^2   --> solution should be x:0.5, y:0.8, w:0.4

VectorXd x_bounds(M+1);
x_bounds = VectorXd::Zero(M+1);

// Generate evenly distributed upper and lower bounds
for(int i = 0; i <= M; i++){
x_bounds(i) = i*(x_upper-x_lower)/M + x_lower;
}

Expression e_x; //Sum of x_hat
Expression e_y; //Sum of y_hat
Expression e_z; //Sum of z

Expression e_1;
Expression e_2;
Expression e_3;
Expression e_4;

for(int k = 0; k < M; k++){

// Bound constraints for all x_hat and y_hat values
x_bounds(k)*z[k] <= x_hat[k] && x_hat[k] <= x_bounds(k+1)*z[k] &&
0 <= y_hat[k] && y_hat[k] <= z[k]
);
// The constraint below has been removed.
// y_bounds(k)*z[k] <= y_hat[k] && y_hat[k] <= y_bounds(k+1)*z[k]

// Add sum of lower and upper bounds
e_1 += x_bounds(k)*y_hat[k];
e_2 += x_bounds(k+1)*y_hat[k] + x_hat[k] - x_bounds(k+1);
e_3 += x_bounds(k)*y_hat[k] + x_hat[k] - x_bounds(k);
e_4 += x_bounds(k+1)*y_hat[k];

e_x += x_hat[k];  // e_x += x_hat[M-1]; The paper has capital M but I assumed that this is a mistake.
e_y += y_hat[k];  // e_y += y_hat[M-1];
e_z += z[k];      //Sum of binary decision variable z
}

w >= e_1 &&
w >= e_2 &&
w <= e_3 &&
w <= e_4 &&

x == e_x &&
y == e_y &&
e_z == 1
);

• Gurobi supports quadratic terms in the objective and in the constraints. I would try that first. Apr 1, 2021 at 10:43
• My goal is to approximate bilinear constraints as linear ones. So I would like to make this work instead of using a quadratic constraint.
– Yuki
Apr 1, 2021 at 22:02

I agree that the e_z == 1 constraint belongs in the model. Your constraints y_bounds(k)*z[k] <= y_hat[k] && y_hat[k] <= y_bounds(k+1)*z[k] do not appear in the Valenzuela dissertation, though, and I suspect they are the source of your troubles. Note, for example, that if $$M=10$$ and $$z = 1$$, you are forcing $$x\in [0.3, 0.4]$$ and $$y\in [0.3,0.4]$$, the latter restriction for no apparent reason.

• Thank you for your help. I have made changes to my program, however, it still gives me the same problems such as infeasible solution when M is a high number (e.g. 4,5,6...) and the solutions to x and y do not multiply to w. I have also added in the constraint 0 <= y_hat[k] <= z[k]
– Yuki
Apr 5, 2021 at 14:40
• I suggest you edit the question to show the updated code, so that we can see what is going on.
– prubin
Apr 5, 2021 at 22:12
• Ok, I have edited the code. I have also looked at the code again and I feel the problem has to do with these lines: e_2 += x_bounds(k+1)*y_hat[k] + x_hat[k] - x_bounds(k+1); e_3 += x_bounds(k)*y_hat[k] + x_hat[k] - x_bounds(k); I noticed that the last -x_bounds(k) terms get summed up even though this particular bound may not be chosen by z. However, this is what is written in the reference.
– Yuki
Apr 5, 2021 at 23:08
• I think (4.22b) and (4.22c) in the dissertation are wrong. Besides leaving out parentheses, the summands should be multiplied by $z_m$, so that only one summand is nonzero. $z_m = 0 \implies \hat{u}_m = 0 = \hat{v}_m$, so only the last term in each of those constraints needs to be multiplied by $z_m$ to fix it.
– prubin
Apr 6, 2021 at 16:28
• I think this solved the problem. Thank you very much. Is there a specific place I should place the final code?
– Yuki
Apr 7, 2021 at 19:08