Featherstone algorithm now be able to handle constraint dynamics such as contact or joint limit.