Moves subclass code to other files. Renamed Obstacle to Polygon
Fixed some problems in pose