Fixing tree inertia computation
Numerous robusness updates, fixes, adding 4-DOF wam support