Fixed bugs in URDF model and window nav
Added Sonar and IR to Magabot, tuned navigation