Implement InverseKinTraj with initial state always being decision variables. It does not work well when the initial state if fixed. SNOPT always returns INFO 43.