Implemented opt-form for 5dof per leg
(both CG codes and controller, underactuation issue)
Got back to 6dof-per-leg opt-form + couple of cleanup/generalizing
Test-13: 12 but using HuntCrossley contact Model
(ZeroInit_ModCF_dt05_t2_OrientCont_SbtlrLock_fixedDS)