Add joint damping term in Featherstone algorithm.