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 = .
{
InverseKinematics.SscCommandCompleteEvent.Set();
return;
}

await SerialPort.Write(_querySsc);
});
};

Then:

SscCommandCompleteEvent.Wait((int)(_gaitSpeedInMs + 75)); //Timeout
SscCommandCompleteEvent.Reset();

There may be a better way than using the manual reset event, but I have yet to find it.