Included all DoF (37) in torque compuation after Opt
(unnecessary computations!
could be reduced to only actuated torques later)
Added SpatialVec2Vector for Jdqd
(a function to change 2*3 to 1*6 vectors,
required for Jdqd)
restructured updVector + more clean-up/comments
(to act similar to subVector)
First opt-working version
(lambda, qdd, and xddp computed by opt,
tau_total (37) generated by EOM,
tau_act (12) selected as applied torque
missing: Jdqd zero, all 37 coords used, can be reduced,
check left nad right orders, contact not detected, ..)