Reduce mid-walk jittering, and ensure first step happens

[ijnek]

I've tested the walk in webots after observing the jittery behavior at 0:25 and 0:36 in the youtube short that @josh posted.

I think there are two issues causing the jitter there.

1. Rapid phase switching - nothing is filtering the noise coming in from phase switching (reported from the feet pressure sensors). It is possible that rapid switching between phases can happen, and the old runswift walk prevented this by ignoring rapid phase switching. I added the same logic to the walk, and I don't see rapid phase switching. The diagram below shows the height of right foot sole, where you can see short spikes, where this rapid switching happens.

2. Incorrect interpolation of the next step. Interpolation for a new step did not consider where the last step finished. If the step did not finish, it still interpolated from the "ideal" finishing position of the previous step. The diagram below shows how large jumps were happening in the feet trajectory. I've added some changes to fix this.

---

In addition, upon the first target message reeived, force a new step to be calculated such that the walk is initiated. Otherwise, a gentle "push" of the robot is required to trigger a phase change currently.