[BTMan] Improve fluidity of movement when using NavigateTo where robot is allowed to rotate

Feature Request

Summary

When RotationSpec is 2 or 3, which allow rotation then the movement becomes very stuttered.

I think this might be because of the way that the tree in NavigateTo is structured. Can simplify if necessary.

Acceptance Criteria

• Robot moves fluidly similar to when rotation is disabled

Design / Technical Approach

Dependencies

Testing Plan

• Write unit tests for feat