The new InverseKinTraj works. Now the initial state is always a decision variable. Might not be the best thing to do