Simplify the chain building from the chest to the wrist.
Verify using an assertion that the foot is flat during the initialization of the walk.

Otherwise there will be troubles at the beginning of the walk,
and discontinuities.