Some rearrangements.
ddx -> ddq, ...
Unified swing foot trajectory generation.