Added head and tail movement support.
Upgrade motion plans to accomodate head and tail movements.