True robot drawing now using drawState function instead of geometry. Better for viz