First version fo wholeBodyDynamics compiling