Robustification w.r.t. state constraints

[croesmann]

It is well known in MPC that state constraints heavily affect robustness properties.

Consider the following example: the robot is not allowed to drive backwards, but sensor noise provides values around zero (also small negative values). For these negative cases, the underlying optimal control problem is infeasible.
The same might happen when the robot moves at the maximum velocity and sensor noise leads to an exceed or an external force further accelerates the robot. Another reason could be a potential collision in the computed planning scene but not in the real world.

A possible way to tackle this issue is to convert state hard-constraints to soft-constraints, however, this is not always desired (e.g. for collision avoidance). One could also convert hard-constraints to soft-constraints as a recovery strategy. Another way is to overwrite the noisy sensor data by the actual limits (but I don't like such "workarounds" in general).
Or one could rely just on the robot to brake in case of infeasibility and "hope" to recover automatically (this is the current implementation).

I would like to collect some user experiences and test results before I continue working on this topic.