Penalty Force and Step and project Working.. Work on Lagrange next