[wholeBodyDynamicsTree] first version working