Merge branch 'master' into arm-manipulation

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,26 +141,7 @@ 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:
@@ -177,19 +149,13 @@ def astar_torus(grid, start_node, goal_node):
                 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.
@@ -235,7 +241,7 @@ class NLinkArm(object):
 
         self.end_effector = np.array(self.points[self.n_links]).T
 
-    def plot(self, obstacles=[]):
+    def plot(self, obstacles=[]):  # pragma: no cover
         plt.cla()
 
         for obstacle in obstacles:

ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py

@@ -53,7 +53,7 @@ def animate(grid, arm, route):
         theta2 = 2 * pi * node[1] / M - pi
         arm.update_joints([theta1, theta2])
         plt.subplot(1, 2, 2)
-        arm.plot(plt, obstacles=obstacles)
+        arm.plot_arm(plt, obstacles=obstacles)
         plt.xlim(-2.0, 2.0)
         plt.ylim(-3.0, 3.0)
         plt.show()
@@ -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,26 +172,7 @@ 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:
@@ -205,25 +180,66 @@ def astar_torus(grid, start_node, goal_node):
                 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.
@@ -256,7 +272,7 @@ class NLinkArm(object):
 
         self.end_effector = np.array(self.points[self.n_links]).T
 
-    def plot(self, myplt, obstacles=[]):
+    def plot_arm(self, myplt, obstacles=[]):  # pragma: no cover
         myplt.cla()
 
         for obstacle in obstacles:

ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py

@@ -22,7 +22,7 @@ class NLinkArm(object):
         self.lim = sum(link_lengths)
         self.goal = np.array(goal).T
 
-        if show_animation:
+        if show_animation:  # pragma: no cover
             self.fig = plt.figure()
             self.fig.canvas.mpl_connect('button_press_event', self.click)
 
@@ -46,10 +46,10 @@ class NLinkArm(object):
                 np.sin(np.sum(self.joint_angles[:i]))
 
         self.end_effector = np.array(self.points[self.n_links]).T
-        if self.show_animation:
+        if self.show_animation:  # pragma: no cover
             self.plot()
 
-    def plot(self):
+    def plot(self):  # pragma: no cover
         plt.cla()
 
         for i in range(self.n_links + 1):

ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py

@@ -21,7 +21,7 @@ MOVING_TO_GOAL = 2
 show_animation = True
 
 
-def main():
+def main():  # pragma: no cover
     """
     Creates an arm using the NLinkArm class and uses its inverse kinematics
     to move it to the desired position.
@@ -159,5 +159,5 @@ def ang_diff(theta1, theta2):
 
 
 if __name__ == '__main__':
-    main()
-    # animation()
+    # main()
+    animation()

ArmNavigation/two_joint_arm_to_point_control/Planar_Two_Link_IK.ipynb

@@ -6,10 +6,7 @@
    "source": [
     "## Two joint arm to point control\n",
     "\n",
-    "![TwoJointArmToPointControl](https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/two_joint_arm_to_point_control/animation.gif)\n",
-    "\n",
-    "\n",
-    "\n",
+    "![TwoJointArmToPointControl](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/two_joint_arm_to_point_control/animation.gif)\n",
     "\n",
     "This is two joint arm to a point control simulation.\n",
     "\n",
@@ -418,7 +415,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.6.7"
+   "version": "3.6.8"
   }
  },
  "nbformat": 4,

ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py

@@ -61,7 +61,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
             return theta1, theta2
 
 
-def plot_arm(theta1, theta2, x, y):
+def plot_arm(theta1, theta2, x, y):  # pragma: no cover
     shoulder = np.array([0, 0])
     elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
     wrist = elbow + \
@@ -94,7 +94,7 @@ def ang_diff(theta1, theta2):
     return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
 
 
-def click(event):
+def click(event):  # pragma: no cover
     global x, y
     x = event.xdata
     y = event.ydata
@@ -111,7 +111,7 @@ def animation():
             GOAL_TH=0.01, theta1=theta1, theta2=theta2)
 
 
-def main():
+def main():  # pragma: no cover
     fig = plt.figure()
     fig.canvas.mpl_connect("button_press_event", click)
     two_joint_arm()