Modified MobilizedBody::lock() and lockAt() methods to immediately modify q and/or u in the state to satisfy the requested lock, rather than waiting until the next prescribe() call.

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