WIP on CHMC; adding a bunch of code to deal with constraint jacobians.