Add lock/unlock methods, unlock on double_tap, and set pose back to rest when locked