I am trying to implement a NMPC for a robot manipulator arm with the Adacos Python interface. So far SQP RTI has worked great, but once I started adding collision avoidance constraints, the QP solver(PARTIAL_CONDENSING_HPIPM) sometimes crashes with error code 3. It seems like the combination of bounds on the state variables and the highly nonlinear constraints for collision avoidance makes the QP problem infeasible. Adding slack has not helped, but from testing the solver is very sensitive to the tuning of the slack weights.
Has anyone experienced anything similar or have any tips on how to troubleshoot?