Back to track! Added InitState for ankle angles.
(This version has right-left asym solved, ready to work on acc ref/limits)