0
$\begingroup$

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)
    // prog.AddLinearCost(lorentz[0]);
    // prog.AddRotatedLorentzConeConstraint(
    //   symbolic::Expression(lorentz[0]), symbolic::Expression(1), (0.4-w[0])*2*(0.4-w[0]) + (0.5-x[0])*10*(0.5-x[0])
    // );

    //Min 2*(0.4-w)^2 + 10*(0.5-x)^2   --> solution should be x:0.5, y:0.8, w:0.4
    prog.AddCost((0.4-w[0])*2*(0.4-w[0]) + (0.5-x[0])*10*(0.5-x[0]));  

    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
      prog.AddLinearConstraint(
        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 
    }

    prog.AddLinearConstraint(
      w[0] >= e_1 &&
      w[0] >= e_2 &&
      w[0] <= e_3 &&
      w[0] <= e_4 &&

      x[0] == e_x &&
      y[0] == e_y &&
      e_z == 1 
    );
Improve this question
$\endgroup$
2
  • $\begingroup$ Gurobi supports quadratic terms in the objective and in the constraints. I would try that first. $\endgroup$
    – Erwin Kalvelagen
    Apr 1, 2021 at 10:43
  • $\begingroup$ My goal is to approximate bilinear constraints as linear ones. So I would like to make this work instead of using a quadratic constraint. $\endgroup$
    – Yuki
    Apr 1, 2021 at 22:02

1 Answer 1

Reset to default
3
$\begingroup$

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[3] = 1$, you are forcing $x\in [0.3, 0.4]$ and $y\in [0.3,0.4]$, the latter restriction for no apparent reason.

Improve this answer
$\endgroup$
6
  • $\begingroup$ 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] $\endgroup$
    – Yuki
    Apr 5, 2021 at 14:40
  • $\begingroup$ I suggest you edit the question to show the updated code, so that we can see what is going on. $\endgroup$
    – prubin ♦
    Apr 5, 2021 at 22:12
  • $\begingroup$ 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. $\endgroup$
    – Yuki
    Apr 5, 2021 at 23:08
  • $\begingroup$ 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. $\endgroup$
    – prubin ♦
    Apr 6, 2021 at 16:28
  • $\begingroup$ I think this solved the problem. Thank you very much. Is there a specific place I should place the final code? $\endgroup$
    – Yuki
    Apr 7, 2021 at 19:08

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.