Fix: add angle wrapping to trajectory interpolation

Direct angle calculations can lead to angles that are bigger than pi. As yaw rotation is between -pi and pi, rotations above pi can lead to the entity taking the "long way around", e.g the angle -3 radians and 3 radians is less than 0.28 radians apart but has a difference of 6 radians if we calculate it directly. In this mr, we are applying the wrapping logic that is already used in "FollowTrajectoryWithSpeedControlUnit" to solve this issue.