First working version of wholeBodyDynamics with new id system