Yaw will step violently in the beginning of the planning

[sytnocui]

hello! when I use Ego Planner v2 to plan a smooth path in an airsim simulator. when the UAV is stopped, and I give a target that is opposite to the direction of the UAV head. the planner will give a path in which the position and velocity are smooth, but the yaw has a violent step. yaw will violently change from the front to the behind in the first pos_cmd frame. In Rviz, the pos_2_odom node will simply set the Odom ignoring Dynamic. However, because of the limited dynamics in my UAV simulator, my UAV will be out of control.

The problem can simply reappear in your demo launch single_drone_interactive.launch

Besides, according to my test, your previous works: Ego Planner v1 and Ego Planner Swarm don't have this problem.

So I want to ask if the problem can be solved by changing some code?