Fix variability in footstep plans based on world pose of the bot

The problem was that as the robot and the goal were moved farther from the origin, a larger constant term was added to the objective (since the objective involves quadratic terms in the goal pose). This would ordinarily not be a problem, but the MIP solver terminates when the optimality gap is less than a certain /fraction/ of the best objective, so as the objective got larger (by a constant factor), the solutions became less optimal. Correctly accounting for this constant (by setting objcon in gurobi) fixes the problem.