Updated comments, avoid obstacles now uses Kd