Safety Improvement: Check control for nans.
Problem: A case of nans might induce a safety-critical incidence in the whole system. E.g., nans in polympc got propagated into my simulator.
Solution: Check for nans before outputting any solution.
Proposition: Check for nans along with the termination criteria, e.g.,
if (termination_criteria(m_x)) {
if (nans_check)
m_info.status.value = sqp_status_t::NANS;
else
m_info.status.value = sqp_status_t::SOLVED;
break;
}
In this case, the solver should be reset as the nans will propagate through everything.
Additionally, all getters for the solution should include a check and return zeros or a safe control if any nans are detected. The safe control, (zeros by default,) can be set with the MPC setup.
I imagine it would be save to only check the control at node 0: solution_u_at(0).hasNaN()) as a) this will probably be applied to the plant or b) it will be involved in resampling, and c) if there are nans in this single control vector, there are probably nans everywhere.
Additional feature: When detecting nans, run a full test with all variables to track the nans back to the place and reason of occurrence. Basically, what a developer would do, automized, and then output to the user something like: Exception: nans detected Track back: nans detected in [variable] coming from .... coming from .... reason: [variable] ill-conditioned / user input [...]