motion_plan useful demos or utils for motion plan, like graph search, path interpolation/simplification, traj generation and so on