search

tesseract-robotics / trajopt

Trajectory Optimization Motion Planner for ROS
379 stars 102 forks source link

trajopt_ifopt does succeed if max_merit_coeff_increases is exceeded #340

Closed rjoomen closed 1 year ago

rjoomen commented 1 year ago

If during the penalty iteration loop the status_ is set to NLP_CONVERGED, but verifySQPSolverConvergence() still detects constraint violations, the loop continues, until max_merit_coeff_increases is reached. But because verifySQPSolverConvergence() does not set the status back to RUNNING, it can happen that solve() returns with status_ == NLP_CONVERGED although constraints are still violated.

I suggest to either add status_ = SQPStatus::RUNNING; here, or add a new SQPStatus::PENALTY_ITERATION_LIMIT with a matching check.

Levi-Armstrong commented 1 year ago

Great find. I went through the legacy trajopt and found a few other behaviors which are different. I will have a PR soon.

Levi-Armstrong commented 1 year ago

This PR should address this issue.

rjoomen commented 1 year ago

Solved by #341, thanks!