basic working MIQCQP footstep planner in pure gurobi matlab