Modified the subtree center of mass
Modified the main algo. Added unittest to validate it against the classical Jcom and by finite differencing.
It seems to work fine, except for a minor problem with the finite-diff, probably coming from Pinocchio itself, that will be investigated later.