Add assertion to detect if the user has set a skeleton to be immobile without reinitializing ContactDynamics. Plus some clean-up in ContactDynamics.