added old follow_traj to see if hybridPlanner with execute better trajectories