I am trying to set up an NMPC for obstacle avoidance, i implemented it in python and now i am trying to use it my ROS setup in c++ by using the c generated code. I am using an SQP with partial condensing HPIPM.
In the c generated code, i am bounding my initial value for my states using the following the commands:
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
However, the resulting initial values from the optimization are drifting more and more after each close loop iteration. Is there any reason why these constraints would not be respected ? Is there any reason why it might get worst across the simulation ?
Thanks for the help,
Hope this makes sense