remove error on codefactor

Author: AtsushiSakai

ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py

@@ -60,8 +60,8 @@ def detect_collision(line_seg, circle):
         closest_point = a_vec + line_vec * proj / line_mag
     if np.linalg.norm(closest_point - c_vec) > radius:
         return False
-    else:
-        return True
+
+    return True
 
 
 def get_occupancy_grid(arm, obstacles):
@@ -74,7 +74,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
@@ -120,16 +120,7 @@ def astar_torus(grid, start_node, goal_node):
 
     parent_map = [[() for _ in range(M)] for _ in range(M)]
 
-    X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
-    heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
-    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]
-                                      )
+    heuristic_map = calc_heuristic_map(M, goal_node)
 
     explored_heuristic_map = np.full((M, M), np.inf)
     distance_map = np.full((M, M), np.inf)
@@ -150,46 +141,21 @@ def astar_torus(grid, start_node, goal_node):
 
         i, j = current_node[0], current_node[1]
 
-        neighbors = []
-        if i - 1 >= 0:
-            neighbors.append((i - 1, j))
-        else:
-            neighbors.append((M - 1, j))
-
-        if i + 1 < M:
-            neighbors.append((i + 1, j))
-        else:
-            neighbors.append((0, j))
-
-        if j - 1 >= 0:
-            neighbors.append((i, j - 1))
-        else:
-            neighbors.append((i, M - 1))
-
-        if j + 1 < M:
-            neighbors.append((i, j + 1))
-        else:
-            neighbors.append((i, 0))
+        neighbors = find_neighbors(i, j)
 
         for neighbor in neighbors:
             if grid[neighbor] == 0 or grid[neighbor] == 5:
                 distance_map[neighbor] = distance_map[current_node] + 1
                 explored_heuristic_map[neighbor] = heuristic_map[neighbor]
                 parent_map[neighbor[0]][neighbor[1]] = current_node
                 grid[neighbor] = 3
-        '''
-        plt.cla()
-        plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
-        plt.show()
-        plt.pause(1e-5)
-        '''
 
     if np.isinf(explored_heuristic_map[goal_node]):
         route = []
         print("No route found.")
     else:
         route = [goal_node]
-        while parent_map[route[0][0]][route[0][1]] is not ():
+        while parent_map[route[0][0]][route[0][1]] != ():
             route.insert(0, parent_map[route[0][0]][route[0][1]])
 
         print("The route found covers %d grid cells." % len(route))
@@ -203,6 +169,46 @@ def astar_torus(grid, start_node, goal_node):
     return route
 
 
+def find_neighbors(i, j):
+    neighbors = []
+    if i - 1 >= 0:
+        neighbors.append((i - 1, j))
+    else:
+        neighbors.append((M - 1, j))
+
+    if i + 1 < M:
+        neighbors.append((i + 1, j))
+    else:
+        neighbors.append((0, j))
+
+    if j - 1 >= 0:
+        neighbors.append((i, j - 1))
+    else:
+        neighbors.append((i, M - 1))
+
+    if j + 1 < M:
+        neighbors.append((i, j + 1))
+    else:
+        neighbors.append((i, 0))
+
+    return neighbors
+
+
+def calc_heuristic_map(M, goal_node):
+    X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
+    heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
+    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]
+                                      )
+
+    return heuristic_map
+
+
 class NLinkArm(object):
     """
     Class for controlling and plotting a planar arm with an arbitrary number of links.

ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py

@@ -92,8 +92,7 @@ def detect_collision(line_seg, circle):
         closest_point = a_vec + line_vec * proj / line_mag
     if np.linalg.norm(closest_point - c_vec) > radius:
         return False
-    else:
-        return True
+    return True
 
 
 def get_occupancy_grid(arm, obstacles):
@@ -143,21 +142,16 @@ def astar_torus(grid, start_node, goal_node):
     Returns:
         Obstacle-free route in joint space from start_node to goal_node
     """
+    colors = ['white', 'black', 'red', 'pink', 'yellow', 'green', 'orange']
+    levels = [0, 1, 2, 3, 4, 5, 6, 7]
+    cmap, norm = from_levels_and_colors(levels, colors)
+
     grid[start_node] = 4
     grid[goal_node] = 5
 
     parent_map = [[() for _ in range(M)] for _ in range(M)]
 
-    X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
-    heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
-    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]
-                                      )
+    heuristic_map = calc_heuristic_map(M, goal_node)
 
     explored_heuristic_map = np.full((M, M), np.inf)
     distance_map = np.full((M, M), np.inf)
@@ -178,52 +172,74 @@ def astar_torus(grid, start_node, goal_node):
 
         i, j = current_node[0], current_node[1]
 
-        neighbors = []
-        if i - 1 >= 0:
-            neighbors.append((i - 1, j))
-        else:
-            neighbors.append((M - 1, j))
-
-        if i + 1 < M:
-            neighbors.append((i + 1, j))
-        else:
-            neighbors.append((0, j))
-
-        if j - 1 >= 0:
-            neighbors.append((i, j - 1))
-        else:
-            neighbors.append((i, M - 1))
-
-        if j + 1 < M:
-            neighbors.append((i, j + 1))
-        else:
-            neighbors.append((i, 0))
+        neighbors = find_neighbors(i, j)
 
         for neighbor in neighbors:
             if grid[neighbor] == 0 or grid[neighbor] == 5:
                 distance_map[neighbor] = distance_map[current_node] + 1
                 explored_heuristic_map[neighbor] = heuristic_map[neighbor]
                 parent_map[neighbor[0]][neighbor[1]] = current_node
                 grid[neighbor] = 3
-        '''
-        plt.cla()
-        plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
-        plt.show()
-        plt.pause(1e-5)
-        '''
 
     if np.isinf(explored_heuristic_map[goal_node]):
         route = []
         print("No route found.")
     else:
         route = [goal_node]
-        while parent_map[route[0][0]][route[0][1]] is not ():
+        while parent_map[route[0][0]][route[0][1]] != ():
             route.insert(0, parent_map[route[0][0]][route[0][1]])
 
         print("The route found covers %d grid cells." % len(route))
+        for i in range(1, len(route)):
+            grid[route[i]] = 6
+            plt.cla()
+            plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
+            plt.show()
+            plt.pause(1e-2)
+
     return route
 
 
+def find_neighbors(i, j):
+    neighbors = []
+    if i - 1 >= 0:
+        neighbors.append((i - 1, j))
+    else:
+        neighbors.append((M - 1, j))
+
+    if i + 1 < M:
+        neighbors.append((i + 1, j))
+    else:
+        neighbors.append((0, j))
+
+    if j - 1 >= 0:
+        neighbors.append((i, j - 1))
+    else:
+        neighbors.append((i, M - 1))
+
+    if j + 1 < M:
+        neighbors.append((i, j + 1))
+    else:
+        neighbors.append((i, 0))
+
+    return neighbors
+
+
+def calc_heuristic_map(M, goal_node):
+    X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
+    heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
+    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]
+                                      )
+
+    return heuristic_map
+
+
 class NLinkArm(object):
     """
     Class for controlling and plotting a planar arm with an arbitrary number of links.

PathPlanning/DubinsPath/__init__.py

@@ -36,16 +36,17 @@ def __init__(self, segments):
         for r, s in zip(segments[:-1], segments[1:]):
             assert(np.array_equal(r.end_pose, s.start_pose))
         self.segments = segments
-    """
-    eta3_path::calc_path_point
-
-    input
-        normalized interpolation point along path object, 0 <= u <= len(self.segments)
-    returns
-        2d (x,y) position vector
-    """
 
     def calc_path_point(self, u):
+        """
+        eta3_path::calc_path_point
+
+        input
+            normalized interpolation point along path object, 0 <= u <= len(self.segments)
+        returns
+            2d (x,y) position vector
+        """
+
         assert(u >= 0 and u <= len(self.segments))
         if np.isclose(u, len(self.segments)):
             segment_idx = len(self.segments) - 1
@@ -152,39 +153,41 @@ def __init__(self, start_pose, end_pose, eta=None, kappa=None):
             + (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
             - (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
                kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
-        
-        self.s_dot = lambda u : max(np.linalg.norm(self.coeffs[:, 1:].dot(np.array([1, 2.*u, 3.*u**2, 4.*u**3, 5.*u**4, 6.*u**5, 7.*u**6]))), 1e-6)
+
+        self.s_dot = lambda u: max(np.linalg.norm(self.coeffs[:, 1:].dot(np.array(
+            [1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))), 1e-6)
         self.f_length = lambda ue: quad(lambda u: self.s_dot(u), 0, ue)
         self.segment_length = self.f_length(1)[0]
 
-    """
-    eta3_path_segment::calc_point
-
-    input
-        u - parametric representation of a point along the segment, 0 <= u <= 1
-    returns
-        (x,y) of point along the segment
-    """
-
     def calc_point(self, u):
+        """
+        eta3_path_segment::calc_point
+
+        input
+            u - parametric representation of a point along the segment, 0 <= u <= 1
+        returns
+            (x,y) of point along the segment
+        """
         assert(u >= 0 and u <= 1)
         return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
 
-    """
-    eta3_path_segment::calc_deriv
-
-    input
-        u - parametric representation of a point along the segment, 0 <= u <= 1
-    returns
-        (d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
-    """
     def calc_deriv(self, u, order=1):
+        """
+        eta3_path_segment::calc_deriv
+
+        input
+            u - parametric representation of a point along the segment, 0 <= u <= 1
+        returns
+            (d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
+        """
+
         assert(u >= 0 and u <= 1)
         assert(order > 0 and order <= 2)
         if order == 1:
-            return self.coeffs[:, 1:].dot(np.array([1, 2.*u, 3.*u**2, 4.*u**3, 5.*u**4, 6.*u**5, 7.*u**6]))
+            return self.coeffs[:, 1:].dot(np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))
         else:
-            return self.coeffs[:, 2:].dot(np.array([2, 6.*u, 12.*u**2, 20.*u**3, 30.*u**4, 42.*u**5]))
+            return self.coeffs[:, 2:].dot(np.array([2, 6. * u, 12. * u**2, 20. * u**3, 30. * u**4, 42. * u**5]))
+
 
 def test1():