Added OptimizeTPSProblem to trajoptpy