fixed some bugs in 2d model
Merge pull request #1388 from dima6120/many_robots

fixed some bugs in 2d model