Contact reduction basically working; need to shift active contact handling to solve_qp_work(.) and solve_nqp_work(.)