Fix comment
Fix GoToGoal PID
Add a check for the goal
Patch robot dynamics according to angle
Set default wheel speed to 0
Add default scene dimension - wip