Simplified code for J and JDotU
(Standing works the same as before,
it holds for about 20 steps,
given that lambda is applied,
pelvis dof is used and subtalar is locked)