slam-state runs. fast, but sometimes reaches max iter or line search fails
slam-state sometimes crashes/doesn't reach goal waypoint. related to penalty coeff (also related to linearization, initial trajectory, and input bounds)
things to try:
- increase time steps
- trust box size
- slam-traj input initializations