Add cost to the nonlinear approach of CoMForcePlanning. RelativeFootPositionPolygon is a NonlinearConstraint now