Optimal control constant depends on previous optimal control constant.
Use of a state machine to take joint limits into account only if they come too close.
Interesting example: with the finite state machine, the mpc controller starts to compensate for the joint limits 1 s in advance (2.5 s)
without the finite state machine, the joint limits are exceeded at about 3.5 s.