Finish up the AStar planner basics.  Now I just need to write the portion that moves the feet to the final goal position.  Should be relatively straight forward.