diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
index 75b1f9d2..e377897e 100644
--- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
+++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
@@ -203,10 +203,10 @@ def calc_heuristic_map(M, goal_node):
     for i in range(heuristic_map.shape[0]):
         for j in range(heuristic_map.shape[1]):
             heuristic_map[i, j] = min(heuristic_map[i, j],
-                                      i + 1 + heuristic_map[M - 1, j],
-                                      M - i + heuristic_map[0, j],
-                                      j + 1 + heuristic_map[i, M - 1],
-                                      M - j + heuristic_map[i, 0]
+                                      M - i - 1 + heuristic_map[M - 1, j],
+                                      i + heuristic_map[0, j],
+                                      M - j - 1 + heuristic_map[i, M - 1],
+                                      j + heuristic_map[i, 0]
                                       )
 
     return heuristic_map
diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
index 591cd401..10f4615c 100644
--- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
+++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
@@ -105,7 +105,7 @@ def get_occupancy_grid(arm, obstacles):
     Args:
         arm: An instance of NLinkArm
         obstacles: A list of obstacles, with each obstacle defined as a list
-                   of xy coordinates and a radius. 
+                   of xy coordinates and a radius.
 
     Returns:
         Occupancy grid in joint space
@@ -234,10 +234,10 @@ def calc_heuristic_map(M, goal_node):
     for i in range(heuristic_map.shape[0]):
         for j in range(heuristic_map.shape[1]):
             heuristic_map[i, j] = min(heuristic_map[i, j],
-                                      i + 1 + heuristic_map[M - 1, j],
-                                      M - i + heuristic_map[0, j],
-                                      j + 1 + heuristic_map[i, M - 1],
-                                      M - j + heuristic_map[i, 0]
+                                      M - i - 1 + heuristic_map[M - 1, j],
+                                      i + heuristic_map[0, j],
+                                      M - j - 1 + heuristic_map[i, M - 1],
+                                      j + heuristic_map[i, 0]
                                       )
 
     return heuristic_map