Edited and tested 6*n Frame J and Jdqd
(replace with 3*n station version,
compiled well for use of CG codes,
ref_pos etc. are missing to run)
Edited subVector and updVector
(error messages adjusted)
Integrated optimizer in the loop
(number of actuated coords not match yet,
lambda is computed, not applied,
it's a good testing version)
Changed Actuated Coords to FLB(6) + Legs(12)
(removing lumbar and upper body coords,
adjusting tau vector to this change)