After much tweaking and re-writing the IK and SerialPort code, the gait is much improved. I still need to figure out why the _bodyPosY or height has odd results the closer it is to the ground. However, the code is in a very usable state.
I was also able to figure out a reliable way to query the SSC-32U for the movement status, and base the gait timing on that status. Makes for a much smoother gait.
SerialPort.ReplyCallback = b =>
Task.Factory.StartNew(async () =>
if (b == 0x2e) //0x2e = .
SscCommandCompleteEvent.Wait((int)(_gaitSpeedInMs + 75)); //Timeout
There may be a better way than using the manual reset event, but I have yet to find it.