[TODO] Do correct angle planning

[kylestach]

Description

In the current iteration of the motion planning rewrite PR set, angle planning (in #1496) creates angular paths that are inconsistent with our current heading and angular velocity. Ideally, we should revamp this method.

Requirements

• Angular profiles are completely smooth
• Angular profiles are consistent with current velocity

Design

• Trapezoidally profile the current smooth plan from the beginning, until the plan's target velocity/position:

        ---------------------------
-----*--                             -----------------------
    /    * is merge point 
   /
__/