Velocity penalty stabilization

This still needs some work on Jacobians but should be better than nothing