Added Eq&Ineq Constraints + Changed Jacob method
(Jacobian dimension still does not match +
some more checking needed for constraints)
Added contact_state detection
(based on calcn height wrt the ground)
Added updVector routine
(required for updating vectors after optimization)
Initial basic WBO_CG functions Added
(all req headers and sources in c)