cut the initial pose to make it more efficient
sort the trajs
set NKY=-pi/2; NK1 = 0.2
switch sideleft and sideright for easier operation
the cutting of the previous data is wrong
the tele_quad_maomao is a re-organized version with attempts to update NKY and NK1 real time