Improvement for robots 2D model

... and some bugfixes