Solve the dynamic planning problem (without kinematics) as a nonlinear problem rather than using bilinear alternation. Finish implementing the nonlinear constraint on the wrench