How to add new soft constraints for velocity

[alexbuyval]

Hi @alexliniger

Thank you for your excellent work and code!

I would like to use an up limit of velocity calculated by global planner in your MPCC formulation. So now I am trying to add new soft constraints for vs state variable.
I have added new method in constraints class:

OneDConstraint Constraints::getVsConstraints(const ArcLengthSpline& track, const State& x) const
{
  // compute linearized vs constraints
  // 0 <= vs <= v_ref(s)
  const StateVector x_vec = stateToVector(x);
  const double v_ref = track.getVelocity(x.s);
  // compute the jacobean of vs
  const C_i_MPC C_vs_constraint = C_i_MPC::Zero();
  // compute the bounds given the Tylor series expansion
  const double vs_constraint_lower = 0-x.vs+ C_vs_constraint *x_vec;
  const double vs_constraint_upper = v_ref-x.vs+ C_vs_constraint *x_vec;

  return { C_vs_constraint, vs_constraint_lower, vs_constraint_upper };
}

track.getVelocity(x.s) is new method which calculates max velocity according the global plan Also I have called the new method similar to others in Constraints::getConstraints

    const OneDConstraint vs_constraints = getVsConstraints(track,x);
...
    C_constrains_matrix.row(si_index.con_vs) = vs_constraints.C_i;
    dl_constrains_matrix(si_index.con_vs) = vs_constraints.dl_i;
    du_constrains_matrix(si_index.con_vs) = vs_constraints.du_i;

Finally, I have added additional cost parameters for new slack variable

Z_cost(si_index.con_vs,si_index.con_vs) = cost_param_.sc_quad_vs;
z_cost(si_index.con_vs) = cost_param_.sc_lin_vs;

However, it does not work.On image bellow you can see that v_s state overcome reference velocity (red line). Meanwhile all residual are small: ipm residuals max: res_g = 5.263475e-09, res_b = 1.421085e-14, res_d = 2.264855e-13, res_m = 1.013018e-09

Could you give me any advice how to make it works properly?

Best Regards, Alex

[alexliniger]

Did you change in the config.h file that you now have 4 polytopic constraints (NPC 4) and that the number of soft constraints is also 4 (NS 4). https://github.com/alexliniger/MPCC/blob/302e12a0546221963dc2430477e54a279532c52d/C%2B%2B/config.h#L35 Best, Alex

[alexliniger]

Also since vs is not a physical state you could just limit the bound without worrying about soft constraints. This can also be done for each element in the horizon.

Best, Alex

[alexbuyval]

Thank you for fast reply!

Did you change in the config.h file that you now have 4 polytopic constraints (NPC 4) and that the number of soft constraints is also 4 (NS 4).

Yes, I have changed.

Also since vs is not a physical state you could just limit the bound without worrying about soft constraints. This can also be done for each element in the horizon. Thank you, I will try.

Best Regards, Alex

[alexbuyval]

Hi Alex,

Finally, I have added the following line in MPC::setStage: stages_[time_step].u_bounds_x(si_index.vs) = normalization_param_.T_x_inv(si_index.vs,si_index.vs)* (track_.getVelocity(xk.s) - xk.vs);

and it works now properly. Thank you for your advice!

Best Regards, Alex