Reorganization algorithm and implemented legs motion

Reorganization algorithm positioning pavement