Clean up
Handle and ignore duplicate objects
Changed robot diameter
known points at current position not that important