Multiple debugs (mainly for double-check the ordering and indices)
(init for PELV_Y, debug checkContact conditions, index Y in _ditRaio fcn,
several indices & right/left order in RegulateCoMTransAccWithInvPendulum,
use of _distRatio in WBopt, cleaning up printing data..)
Added StancePlanner and COM Controller
(Stance Planner determines the desired pos, vel and acc for pelvis
and then based on their extracted exponential trajectories control the COM,
assuming pelvis and COM are close.)
Added Squat Planner and its sinusoidal trajectory generator
(similar structure as stance planner)