Fixed collision avoidance.
Fixed robot theta bug, when moving robot
Removed trailing whitespaces from stdr_robot.cpp file