InverseKinTraj can solve problem with fix_initial_state = false