Update mdp_planner.py

quick fix for current nav bugs