still unable to get slam-state-mpc to run. usually crashes on the upper path where approximate merit function is often < 0. not sure if tweaking parameters at this point will solve it. my gut says it fails because satisfying the loose waypoint angle constraint directly conflicts going to the landmark