re-implement footstepMIQP in new form