Weird constraint behavior when implementing simple bounds

[nikolaif399]

I'm seeing some odd behavior with OSQP when adding simple bounds to an MPC problem. I'm trying to add lower and upper bounds to the control input in the form of f_min <= u <= fmax. I'm adding an appropriately sized identity matrix to the constraint matrix and the vectorized version of f_min and f_max to the lower and upper bound matrices respectively. When I set these bounds to -OsqpEigen::Infty and OsqpEigen::Infty, the result is correct and the control efforts called for are all between 20 and 30. However, if I change either the lower bound to 0 or the upper bound to 50, the solver now outputs something like 0.02 for each control. Given that the bounds are still very wide and the optimal solution falls within them, I assumed that this constraint would be inactive and unable to change the output, but that doesn't seem to be the case. Is there any way this is an OSQP issue, maybe related to primal variable initialization?

The code is somewhat integrated with the rest of a project right now, but I can attempt to construct a MWE if this seems like an OSQP issue.

[S-Dafarra]

Hi @nikolaif399, thanks for reporting this. Indeed, this seems strange. A MWE would be great to debug the problem, also because the issue might be on the problem itself (the cost function might be flat so every feasible point is also optimal), on the method (e.g. related to the solver internal machinery), or on the code (some unitialized memory maybe). So basically any additional information might be useful.

[nikolaif399]

Hi @S-Dafarra, thanks for getting back to me so quickly! I've put together a MWE, but in the process of doing so I've discovered that you were right about the flatness of my cost function in the optimal region. I assume I can correct this by dramatically increasing the magnitude of my weights, but I would prefer to tweak the appropriate solver convergence settings instead, could you recommend which are relevant in the case of early termination?

I'll go ahead and attach my example here, if for nothing else then to have another MPC setup on this page. To reproduce the effects mentioned in this issue, try changing the control_lo and control_hi bounds to the ones recommended in comment in the code and observe the changed optimal control sequence output by the solver, despite the fact that neither set of bounds is tight enough to activate the constraint.

Great work on the repository, this has been a very useful tool for my research!

#include <Eigen/Dense>
#include <iostream>
#include <math.h>
#include "OsqpEigen/OsqpEigen.h"

const double INF = OsqpEigen::INFTY;
const double NINF = -INF;

int main(int argc, char **argv) {

   // Configurable parameters
  const int Nu = 2; // Appended gravity term
  const int Nx = 2; // Number of states
  const int N = 10;   // Time horizons to consider
  const double dt = 0.1;             // Time horizon
  const double g = 9.81;
  const double m = 3;

  const int num_dyn_constraints = N * Nx;
  const int num_state_vars= (N + 1) * Nx;
  const int num_control_vars= N * Nu;
  const int num_decision_vars = num_state_vars + num_control_vars;
  const int num_constraints = num_decision_vars + num_dyn_constraints;

  // Weights on state deviation and control input
  Eigen::MatrixXd Q(Nx, Nx);
  Q.setZero();
  Q.diagonal() << 1,0.000001; //dtheta

  Eigen::MatrixXd R(Nu, Nu);
  R.setZero();
  R.diagonal() << 0.0001,0;

  // State and control bounds (fixed for a given solve) 
  Eigen::VectorXd state_lo(Nx);
  state_lo << NINF,NINF;
  Eigen::VectorXd state_hi(Nx);
  state_hi << INF,INF;

  Eigen::VectorXd control_lo(Nu);
  control_lo << NINF,1; // Try changing to -10,1
  Eigen::VectorXd control_hi(Nu);
  control_hi << INF,1; //Try changing to 20,1

  // Construct reference trajectory
  Eigen::MatrixXd ref_traj(Nx,N+1);
  Eigen::VectorXd initial_state(Nx);

  for(int i = 0; i < N+1;++i) {
    ref_traj(0,i) = 0.25 + 0.1*sin(i/3.0);
  }

  // Create vectors of dynamics matrices at each step,
  // weights at each step and contact sequences at each step
  Eigen::MatrixXd Ad = Eigen::MatrixXd::Zero(Nx,Nx);
  Ad.block(0,0,2,2) = Eigen::MatrixXd::Identity(2,2);
  Ad(0,1) = dt;
  std::cout << "Ad: " << std::endl << Ad << std::endl;

  Eigen::MatrixXd Bd = Eigen::MatrixXd::Zero(Nx,Nu);
  Bd(1,0) = 1/m;
  Bd(1,1) = -g*dt; // gravity acts downwards here
  std::cout << "Bd: " << std::endl << Bd << std::endl;

  // Construct quadratic cost matrix
  Eigen::MatrixXd Hq(num_state_vars, num_state_vars);
  Hq.setZero();
  for (int i = 0; i < N+1; ++i) {
    Hq.block(i * Nx, i * Nx, Nx, Nx) = Q;
  }

  Eigen::MatrixXd Hu(num_control_vars, num_control_vars);
  Hu.setZero();
  for (int i = 0; i < N; ++i) {
    Hu.block(i*Nu,i*Nu,Nu,Nu) = R;
  }

  Eigen::MatrixXd hessian = Eigen::MatrixXd::Zero(num_decision_vars,num_decision_vars);
  hessian.block(0,0,num_state_vars,num_state_vars) = Hq;
  hessian.block(num_state_vars,num_state_vars,
          num_control_vars,num_control_vars) = Hu;

  // Construct linear cost vector
  Eigen::VectorXd gradient = Eigen::MatrixXd::Zero(num_decision_vars,1);
  Eigen::Map<Eigen::MatrixXd> y(ref_traj.data(), 1, ref_traj.size());
  Eigen::MatrixXd fx = -y * Hq;
  Eigen::MatrixXd fu = Eigen::MatrixXd::Zero(N * Nu, 1);
  gradient << fx.transpose(), fu;

  // Construct dynamics constraint matrix and vectors
  Eigen::MatrixXd A_dyn = Eigen::MatrixXd::Zero(num_dyn_constraints,num_decision_vars);
  for (int i = 0; i < N; ++i) {
    A_dyn.block(Nx*i,Nx*i,Nx,Nx) = Ad;
    A_dyn.block(Nx*i,Nx*(i+1),Nx,Nx) = -Eigen::MatrixXd::Identity(Nx,Nx);
    A_dyn.block(Nx*i,num_state_vars+Nu*i,Nu,Nu) = Bd;
  }

  Eigen::VectorXd b_dyn(num_dyn_constraints);
  b_dyn.setZero();

  // Construct state and control bounds
  Eigen::VectorXd b_state_lo = state_lo.replicate(N,1);
  Eigen::VectorXd b_state_hi = state_hi.replicate(N,1);

  Eigen::VectorXd b_control_lo = control_lo.replicate(N,1);
  Eigen::VectorXd b_control_hi = control_hi.replicate(N,1);

  // Stack constraint matrices and constraint vectors 
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_constraints,num_decision_vars);

  A.block(0,0,num_dyn_constraints,num_decision_vars) = A_dyn;
  A.block(num_dyn_constraints,0,num_decision_vars,num_decision_vars) = Eigen::MatrixXd::Identity(num_decision_vars,num_decision_vars);

  Eigen::VectorXd lower_bound(num_constraints);
  lower_bound << b_dyn, initial_state, b_state_lo, b_control_lo;

  Eigen::VectorXd upper_bound(num_constraints);
  upper_bound << b_dyn, initial_state, b_state_hi, b_control_hi;

  // Setup OsqpEigen solver

  Eigen::SparseMatrix<double> hessian_sparse = hessian.sparseView();
  Eigen::SparseMatrix<double> A_sparse = A.sparseView();

  OsqpEigen::Solver solver;
  solver.data()->setNumberOfVariables(num_decision_vars);
  solver.data()->setNumberOfConstraints(num_constraints);
  solver.data()->setHessianMatrix(hessian_sparse);
  solver.data()->setGradient(gradient);
  solver.data()->setLinearConstraintsMatrix(A_sparse);
  solver.data()->setLowerBound(lower_bound);
  solver.data()->setUpperBound(upper_bound);
  solver.settings()->setVerbosity(true);
  solver.settings()->setWarmStart(true);
  solver.settings()->setCheckTermination(10);
  solver.initSolver();

  solver.solve();
  Eigen::MatrixXd x_out = solver.getSolution();

  //std::cout << x_out << std::endl;

  Eigen::MatrixXd control_traj(Nu,N);
  // Collect control trajectory
  for (size_t i = 0; i < N; ++i) {
    for (size_t j = 0; j < Nu; ++j) {
      control_traj(j,i) = x_out((N+1)*Nx + i*Nu + j,0);
    }
  }

  std::cout << control_traj << std::endl;

  /*mpcplusplus::LinearMPC mpc(N,Nx,Nu);

  mpc.update_weights(Q_vec,U_vec);
  mpc.update_dynamics(Ad_vec,Bd_vec);

  //mpc.update_contact(contact_sequences, mu, fmin, fmax);
  mpc.update_state_bounds(state_lo, state_hi);
  mpc.update_control_bounds(control_lo, control_hi);

  // Solve, collect output and cost val
  Eigen::MatrixXd x_out;
  double f_val;

  mpc.solve(initial_state, ref_traj, x_out, f_val);
  //mpc.solve(initial_state, ref_traj, x_out, f_val);

  //std::cout << "xout: " << std::endl << x_out << std::endl;

  Eigen::MatrixXd opt_traj,control_traj;
  Eigen::VectorXd first_control;
  mpc.get_output(x_out, first_control, opt_traj, control_traj);

  //std::cout << std::endl << "First control: " << first_control << std::endl;

  //std::cout << std::endl << "z ref traj: " << ref_traj.row(2) << std::endl;
  //std::cout << std::endl << "z traj: " << opt_traj.row(2) << std::endl;
  //std::cout << std::endl << "dz traj: " << opt_traj.row(8) << std::endl;

  std::vector<double> zref(N+1);
  std::vector<double> z(N+1);
  for (int i = 0; i < N+1; ++i) {
    zref.at(i) = ref_traj(0,i);
    z.at(i) = opt_traj(0,i);
  }

  std::cout << control_traj << std::endl;
  */
  return 0;
}```

[nikolaif399]

The difference between using infinite bounds and finite, but still wide enough bounds is minimal in this example, but larger on my full order model. This may not be a problem for my application and as said above I believe I can play with the weights to work around it, but I'm struggling to understand why adding an inactive constraint has any influence at all on the optimum found. Hopefully I'm overlooking something here as my MPC setup is extremely similar to the one in the examples folder of this repository.

[S-Dafarra]

Hi @nikolaif399, thanks for the kind words! I am happy that we have found a possible culprit. In case you need some specific advice on the solver settings, I would suggest you to open an issue directly on the osqp repo. osqp-eigen is just a C++ interface for the C API of that solver.

If you encounter any missing flag or functionality, feel free to open a new issue.

[nikolaif399]

We've finally managed to figure this one out. We were setting all control bounds but one to be -inf -> inf, but when we reset this to -1000 to 1000 for each variable the problem went away. I'm guessing this has something to do with either variable initialization or some odd scaling step that happens inside OSQP. Lesson learned is to not use simple constraints with infinite bounds.