Featherstone equals previous version: still to do direct dynamics
restore only one virtual link (3R + 3T)
still problem in the direct model