Fixed problem with robot not properly executing a trajectory when it
has to rotate at the beginning. Now, the planner checks if the
orientation to move along the trajectory is satisfied. If it is not
satisfied, it does not plan ahead with startPlanner_ and sets the duration parameter passed
to updatePaths to 0. Also fixed bug in updatePaths to check if duration
is > 0 before inserting the start position into the paths.