fix initialization order
simplified working...
complementarity hinge cost works, non-trivial ground constraint doesnt