Heuristical motion planning

Implement (x,y,theta)-planning for a polygonal robot moving with translation and rotation in 2D using one of:

What to turn in

Push your code in your github repo
Provide a README that describes (1) an overview of how your project works (2) how to run your code; and (3) the state of your code (examples of when it works, a list of known bugs). Basically what you want people to know about your code.
Check into the repo a few screenshots of the planner and a movie if possible (you'll enjoy looking at it later!).