Modified MobilizedBody::lock() and lockAt() methods to immediately modify state.

This satisfies Kevin He's request in Simbody issue #20.