Sgabello's update of odomi path planner the 16/7/2014