Clean whitespace in manipulation_planning.py

stretch_funmap/src/stretch_funmap/manipulation_planning.py

@@ -47,7 +47,7 @@ def plan_surface_coverage(tool_start_xy_pix, tool_end_xy_pix, tool_extension_dir
     # linear extension paths that overlap the surface. [[path_number,
     # retracted_tool_position, [start_of_surface_overlap,
     # end_of_surface_overlap], ...]
-    
+
     step_vector = tool_end_xy_pix - tool_start_xy_pix
     total_step_distance_pix = np.linalg.norm(step_vector)
     step_direction = step_vector / total_step_distance_pix
@@ -56,7 +56,7 @@ def plan_surface_coverage(tool_start_xy_pix, tool_end_xy_pix, tool_extension_dir
     linear_paths = []
 
     start_xy_pix = tool_start_xy_pix
-    for n in range(num_steps): 
+    for n in range(num_steps):
         end_xy_pix = np.int32(np.round(start_xy_pix + (max_extension_pix * tool_extension_direction_xy_pix)))
         first_surface_contact_xy, last_surface_contact_xy, first_obstacle_contact_xy = numba_find_line_path_on_surface(np.int32(np.round(start_xy_pix)), end_xy_pix, surface_mask_image, obstacle_mask_image)
         if first_surface_contact_xy is not None:
@@ -71,10 +71,10 @@ def plan_surface_coverage(tool_start_xy_pix, tool_end_xy_pix, tool_extension_dir
 
 def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text='', display_images=False):
     blur = True
-    if blur: 
+    if blur:
         blur_size = (7,7)
-        image = cv2.GaussianBlur(image, blur_size, 0) 
-    
+        image = cv2.GaussianBlur(image, blur_size, 0)
+
     # sobel operator does not appear to be normalized to provide a true estimate of the derivative
     # 3x3 = -1 0 +1
     #       -2 0 +2
@@ -92,8 +92,8 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text
     min_edge_height_m = 0.2
     min_edge_height_pix = min_edge_height_m / m_per_height_unit
     canny_edges[image < min_edge_height_pix] = 0
-    
-    use_dilation = True 
+
+    use_dilation = True
     if use_dilation:
         kernel_width_pix = 3
         iterations = 1
@@ -111,7 +111,7 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text
 
     min_edge_length_m = 0.1
     min_gap_m = 0.1
-    
+
     minLineLength = min_edge_length_m / m_per_pix
     print('minLineLength = {0}'.format(minLineLength))
     maxLineGap = min_gap_m / m_per_height_unit
@@ -148,14 +148,14 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text
                 line_center = np.array([(x1 + x2)/2.0, (y1 + y2)/2.0])
                 dist = np.linalg.norm(line_center - robot_loc)
                 candidates.append([dist, [x1, y1, x2, y2]])
-        
+
     if len(candidates) > 0:
         # sort by straight line distance
         def sort_by_distance(cliff):
             return cliff[0]
         candidates.sort(key=sort_by_distance)
         best_candidate = candidates[0][1]
-        if display_images: 
+        if display_images:
             width = 2
             x1, y1, x2, y2 = best_candidate
             cv2.line(color_im, (x1,y1), (x2,y2), [0,0,255], width)
@@ -163,7 +163,7 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text
         print('No viable cliff candidates found.')
         best_candidate = None
 
-    if best_candidate is not None: 
+    if best_candidate is not None:
         x0, y0, x1, y1 = best_candidate
         p0 = [x0, y0]
         p1 = [x1, y1]
@@ -189,13 +189,13 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text
         p1 = None
         normal = None
 
-    if display_images: 
+    if display_images:
         cv2.imshow('image ' + display_text, image)
         cv2.imshow('canny edges ' + display_text, canny_edges)
         cv2.imshow('depth image with detected lines ' + display_text, color_im)
-        
+
     return p0, p1, normal
-    
+
 
 class ManipulationView():
     def __init__(self, tf2_buffer, debug_directory=None, tool='tool_stretch_gripper'):
@@ -209,14 +209,14 @@ class ManipulationView():
 
         # Define the volume of interest for planning using the current
         # view.
-        
+
         # How far to look ahead.
         look_ahead_distance_m = 2.0
         # Robot's width plus a safety margin.
         look_to_side_distance_m = 1.3
 
         m_per_pix = 0.006
-        pixel_dtype = np.uint8 
+        pixel_dtype = np.uint8
 
         # stretch (based on HeadScan in mapping.py)
         robot_head_above_ground = 1.13
@@ -233,7 +233,7 @@ class ManipulationView():
         robot_right_edge_m = 0.2
         voi_side_x_m = 2.0 * look_to_side_distance_m
         voi_side_y_m = look_ahead_distance_m
-        
+
         voi_axes = np.identity(3)
         voi_origin = np.array([-(voi_side_x_m/2.0), -(voi_side_y_m + robot_right_edge_m), -lowest_distance_below_ground])
 
@@ -263,7 +263,7 @@ class ManipulationView():
         head_settle_time = 0.5
         rospy.sleep(head_settle_time)
 
-    def estimate_reach_to_contact_distance(self, tooltip_frame, tf2_buffer, save_debugging_images=True): 
+    def estimate_reach_to_contact_distance(self, tooltip_frame, tf2_buffer, save_debugging_images=True):
         h = self.max_height_im
         m_per_pix = h.m_per_pix
         tooltip_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(tooltip_frame, tf2_buffer)
@@ -271,7 +271,7 @@ class ManipulationView():
         # translational component of the transform, which is the same
         # as multiplying by [0,0,0,1]
         tooltip_x, tooltip_y, tooltip_z = tooltip_points_to_image_mat[:, 3][:3]
-        
+
         base_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat('base_link', tf2_buffer)
         # Ideal arm extension direction is in the negative y axis
         # direction of the base_link frame. This could be improved by
@@ -284,7 +284,7 @@ class ManipulationView():
         extension_xy = -base_points_to_image_mat[:, 1][:2]
         # create a unit length vector in the direction of extension in the image
         extension_xy = extension_xy / np.linalg.norm(extension_xy)
-        
+
         start_xy = np.array([tooltip_x, tooltip_y])
         max_reach = 0.5 / m_per_pix
         end_xy = (max_reach * extension_xy) + start_xy
@@ -314,7 +314,7 @@ class ManipulationView():
         rospy.loginfo('start_xy = {0}'.format(start_xy))
         rospy.loginfo('end_xy = {0}'.format(end_xy))
         rospy.loginfo('*************************************')
-            
+
         contact_found, (contact_x, contact_y) = numba_find_contact_along_line_path(start_xy, end_xy, mask_image)
         if contact_found:
             print('ManipulationView estimate_reach_to_contact_distance : contact detected!')
@@ -337,7 +337,7 @@ class ManipulationView():
             line_width = 2
             radius = 5
             p0 = tuple(np.int32(np.round(start_xy)))
-            
+
             height, width = mask_image.shape
             color_im = np.zeros((height, width, 3), np.uint8)
             color_im[:,:,0] = mask_image
@@ -345,13 +345,13 @@ class ManipulationView():
             color_im[:,:,2] = mask_image
             # always draw the start point, regardless of contact detection
             cv2.circle(color_im, p0, radius, (0,255,0), 1)
-            if contact_found:                
+            if contact_found:
                 p1 = tuple(np.int32(np.round(contact_xy)))
                 cv2.line(color_im, p0, p1, [255, 0, 0], line_width)
                 cv2.circle(color_im, p1, radius, (0,0,255), 1)
             filename = 'estimate_reach_to_contact_distance_annotated_mask_' + hm.create_time_string() + '.png'
             cv2.imwrite(dirname + filename, color_im)
-            
+
             rgb_image = self.max_height_im.rgb_image.copy()
             # always draw the start point, regardless of contact detection
             cv2.circle(rgb_image, p0, radius, (0,255,0), 1)
@@ -365,19 +365,19 @@ class ManipulationView():
         else:
             rospy.loginfo('ManipulationView estimate_reach_to_contact_distance: No debug directory provided, so debugging data will not be saved.')
 
-            
+
         return reach_m
 
     def get_grasp_target(self, tf2_buffer, max_object_planar_distance_m=1.0):
         grasp_target = sm.find_object_to_grasp(self.max_height_im, display_on=False)
         if grasp_target is None:
             return None
-        
+
         h = self.max_height_im
         m_per_pix = h.m_per_pix
-        
+
         debug = True
-        if debug and (self.debug_directory is not None): 
+        if debug and (self.debug_directory is not None):
             rgb_image = h.rgb_image.copy()
             sm.draw_grasp(rgb_image, grasp_target)
             # Save the new scan to disk.
@@ -395,16 +395,16 @@ class ManipulationView():
         print('object_planar_distance_m =', object_planar_distance_m)
         if object_planar_distance_m >= max_object_planar_distance_m:
             return None
-            
+
         return grasp_target
 
-    
+
     def get_pregrasp_lift(self, grasp_target, tf2_buffer):
         h = self.max_height_im
         m_per_unit = h.m_per_height_unit
         tooltip_frame = 'link_grasp_center'
         tooltip_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(tooltip_frame, tf2_buffer)
-        
+
         # Obtain the tooltip location in the image by obtaining the
         # translational component of the transform, which is the same
         # as multiplying by [0,0,0,1]
@@ -419,7 +419,7 @@ class ManipulationView():
             lift_to_pregrasp_m = 0.94
         return lift_to_pregrasp_m
 
-    
+
     def get_pregrasp_yaw(self, grasp_target, tf2_buffer):
         h = self.max_height_im
         # The planar component of the link gripper x_axis is parallel
@@ -434,7 +434,7 @@ class ManipulationView():
         forward_xy = gripper_points_to_image_mat[:, 0][:2]
         # create a unit length vector in the direction of extension in the image
         gripper_forward_pix = forward_xy / np.linalg.norm(forward_xy)
-        
+
         elongated_object = grasp_target['elongated']
 
         if not elongated_object:
@@ -444,33 +444,33 @@ class ManipulationView():
             yaw_angle = 0.0
         else:
             gripper_ang_rad = np.arctan2(gripper_forward_pix[1], -gripper_forward_pix[0])
-            
+
             centroid = np.array(grasp_target['location_xy_pix'])
             long_axis = np.array(grasp_target['long_axis_pix'])
-            
+
             v0 = long_axis[0] - centroid
             v0 = v0 / np.linalg.norm(v0)
             d0 = np.dot(v0, gripper_forward_pix)
-            
+
             v1 = long_axis[1] - centroid
             v1 = v1 / np.linalg.norm(v1)
             d1 = np.dot(v1, gripper_forward_pix)
 
-            if d0 > d1: 
+            if d0 > d1:
                 side_to_grasp = v0
             else:
                 side_to_grasp = v1
-                
-            object_ang_rad = np.arctan2(side_to_grasp[1], -side_to_grasp[0])            
+
+            object_ang_rad = np.arctan2(side_to_grasp[1], -side_to_grasp[0])
             yaw_angle = float(hm.angle_diff_rad(object_ang_rad, gripper_ang_rad))
 
         return yaw_angle
 
-    
+
     def get_pregrasp_planar_translation(self, grasp_target, tf2_buffer):
         h = self.max_height_im
         m_per_pix = h.m_per_pix
-        
+
         # The planar component of the link gripper x_axis is parallel
         # to the middle of the gripper, but points in the opposite
         # direction.
@@ -516,19 +516,19 @@ class ManipulationView():
         pregrasp_target_dist_m = 0.27
         pregrasp_target_dist_pix = pregrasp_target_dist_m / m_per_pix
         pregrasp_target_xy_pix = (pregrasp_target_dist_pix * gripper_forward_pix) + grasp_target['location_xy_pix']
-        
+
         translate_xy_pix = pregrasp_target_xy_pix - yaw_xy_pix
         robot_forward_m = m_per_pix * np.dot(translate_xy_pix, robot_forward_pix)
         wrist_extension_m = m_per_pix * np.dot(translate_xy_pix, tool_extension_direction_xy_pix)
 
         debug = True
-        if debug and (self.debug_directory is not None): 
+        if debug and (self.debug_directory is not None):
             rgb_image = h.rgb_image.copy()
             radius = 5
             width = 1
             line_width = 1
             line_length = 10.0
-            
+
             cv2.circle(rgb_image, tuple(np.int32(np.round(grasp_target['location_xy_pix']))), radius, [0, 0, 255], width)
             cv2.circle(rgb_image, tuple(np.int32(np.round(pregrasp_target_xy_pix))), radius, [0, 255, 0], width)
             cv2.circle(rgb_image, tuple(np.int32(np.round(yaw_xy_pix))), radius, [255, 0, 0], width)
@@ -536,11 +536,11 @@ class ManipulationView():
             x0 = np.int32(np.round(yaw_xy_pix))
             x1 = np.int32(np.round((line_length * np.array(tool_extension_direction_xy_pix)) + np.array(yaw_xy_pix)))
             cv2.line(rgb_image, tuple(x0), tuple(x1), [255, 255, 255], line_width)
-            
+
             x0 = np.int32(np.round(yaw_xy_pix))
             x1 = np.int32(np.round((line_length * np.array(robot_forward_pix)) + np.array(yaw_xy_pix)))
             cv2.line(rgb_image, tuple(x0), tuple(x1), [255, 255, 255], line_width)
-            
+
             x0 = np.int32(np.round(yaw_xy_pix))
             x1 = np.int32(np.round((line_length * np.array(gripper_forward_pix)) + np.array(yaw_xy_pix)))
             cv2.line(rgb_image, tuple(x0), tuple(x1), [0, 255, 255], line_width)
@@ -558,22 +558,22 @@ class ManipulationView():
             if not os.path.exists(dirname):
                 os.makedirs(dirname)
             cv2.imwrite(dirname + filename, rgb_image)
-            
+
         pregrasp_mobile_base_m = robot_forward_m
         pregrasp_wrist_extension_m = wrist_extension_m
-        
+
         return pregrasp_mobile_base_m, pregrasp_wrist_extension_m
 
-    
+
     def get_grasp_from_pregrasp(self, grasp_target, tf2_buffer):
 
         h = self.max_height_im
         m_per_unit = h.m_per_height_unit
         m_per_pix = h.m_per_pix
-        
+
         fingertip_frame = 'link_gripper_fingertip_left'
         fingertip_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(fingertip_frame, tf2_buffer)
-        
+
         # Obtain the fingertip location in the image by obtaining the
         # translational component of the transform, which is the same
         # as multiplying by [0,0,0,1]
@@ -583,7 +583,7 @@ class ManipulationView():
         grasp_lift_m = m_per_unit * (grasp_target['location_z_pix'] - fingertip_z)
         # lower to account for compliant fingers and finger raising when closing
         grasp_lift_m = grasp_lift_m + 0.01
-        
+
         # The planar component of the link gripper x_axis is parallel
         # to the middle of the gripper, but points in the opposite
         # direction.
@@ -628,19 +628,19 @@ class ManipulationView():
         grasp_target_dist_m = 0.21
         grasp_target_dist_pix = grasp_target_dist_m / m_per_pix
         grasp_target_xy_pix = (grasp_target_dist_pix * gripper_forward_pix) + grasp_target['location_xy_pix']
-        
+
         translate_xy_pix = grasp_target_xy_pix - yaw_xy_pix
         robot_forward_m = m_per_pix * np.dot(translate_xy_pix, robot_forward_pix)
         wrist_extension_m = m_per_pix * np.dot(translate_xy_pix, tool_extension_direction_xy_pix)
 
         debug = True
-        if debug and (self.debug_directory is not None): 
+        if debug and (self.debug_directory is not None):
             rgb_image = h.rgb_image.copy()
             radius = 5
             width = 1
             line_width = 1
             line_length = 10.0
-            
+
             cv2.circle(rgb_image, tuple(np.int32(np.round(grasp_target['location_xy_pix']))), radius, [0, 0, 255], width)
             cv2.circle(rgb_image, tuple(np.int32(np.round(grasp_target_xy_pix))), radius, [0, 255, 0], width)
             cv2.circle(rgb_image, tuple(np.int32(np.round(yaw_xy_pix))), radius, [255, 0, 0], width)
@@ -648,7 +648,7 @@ class ManipulationView():
             x0 = np.int32(np.round(yaw_xy_pix))
             x1 = np.int32(np.round((line_length * np.array(tool_extension_direction_xy_pix)) + np.array(yaw_xy_pix)))
             cv2.line(rgb_image, tuple(x0), tuple(x1), [255, 255, 255], line_width)
-            
+
             x0 = np.int32(np.round(yaw_xy_pix))
             x1 = np.int32(np.round((line_length * np.array(robot_forward_pix)) + np.array(yaw_xy_pix)))
             cv2.line(rgb_image, tuple(x0), tuple(x1), [255, 255, 255], line_width)
@@ -670,13 +670,13 @@ class ManipulationView():
             if not os.path.exists(dirname):
                 os.makedirs(dirname)
             cv2.imwrite(dirname + filename, rgb_image)
-  
+
         grasp_mobile_base_m = robot_forward_m
         grasp_wrist_extension_m = wrist_extension_m
-        
+
         return grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m
-    
-    
+
+
     def get_surface_wiping_plan(self, tf2_buffer, tool_width_m, tool_length_m, step_size_m):
         strokes = None
         movements = None
@@ -689,21 +689,21 @@ class ManipulationView():
 
             tool_width_pix = tool_width_m / m_per_pix
             tool_length_pix = tool_length_m / m_per_pix
-            
+
             wrist_frame = 'link_aruco_top_wrist'
             wrist_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(wrist_frame, tf2_buffer)
-            
+
             # Obtain the wrist location in the image by obtaining the
             # translational component of the transform, which is the same
             # as multiplying by [0,0,0,1]
             wrist_x, wrist_y, wrist_z = wrist_points_to_image_mat[:, 3][:3]
             wrist_current_xy_pix = np.array([wrist_x, wrist_y])
-            
+
             # Find the flat surface closest to the wrist.
             surface_mask, plane_parameters = sm.find_closest_flat_surface(h, wrist_current_xy_pix, display_on=False)
 
             if surface_mask is not None:
-                
+
                 # Use the maximum height on the segmented surface as a
                 # conservative height for the plane.
                 surface_height_pix = np.max(h_image[surface_mask > 0])
@@ -715,7 +715,7 @@ class ManipulationView():
                 min_obstacle_height_pix = min_obstacle_height_m / m_per_unit
                 obstacle_selector = h_image > min_obstacle_height_pix
                 obstacle_mask = np.uint8(obstacle_selector)
-                
+
                 # Dilate the obstacles to create a safety margin.
                 dilate_obstacles = True
                 if dilate_obstacles:
@@ -739,7 +739,7 @@ class ManipulationView():
                 # make a surface cleaning plan from the right to the
                 # left side of the surface
                 base_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat('base_link', tf2_buffer)
-                
+
                 # Ideal arm extension direction is in the negative y axis
                 # direction of the base_link frame. This could be improved by
                 # using the calibrated URDF to find the Jacobian for
@@ -760,7 +760,7 @@ class ManipulationView():
                 forward_xy = base_points_to_image_mat[:, 0][:2]
                 # create a unit length vector in the direction of extension in the image
                 robot_forward_pix = forward_xy / np.linalg.norm(forward_xy)
-                
+
                 max_drive_forward_m = 0.4 #0.25
                 max_drive_backward_m = 0.4 #0.25
                 max_drive_forward_pix = max_drive_forward_m / m_per_pix
@@ -768,13 +768,13 @@ class ManipulationView():
 
                 tooltip_frame = 'link_grasp_center'
                 tooltip_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(tooltip_frame, tf2_buffer)
-                
+
                 # Obtain the tooltip location in the image by obtaining the
                 # translational component of the transform, which is the same
                 # as multiplying by [0,0,0,1]
                 tooltip_x, tooltip_y, tooltip_z = tooltip_points_to_image_mat[:, 3][:3]
                 tool_current_xy_pix = np.array([tooltip_x, tooltip_y])
-            
+
                 tool_start_xy_pix = tool_current_xy_pix - (max_drive_backward_pix * robot_forward_pix)
                 tool_end_xy_pix = tool_current_xy_pix + (max_drive_forward_pix * robot_forward_pix)
 
@@ -799,7 +799,7 @@ class ManipulationView():
                                         'end_wrist_extension_m': end_wrist_extension_m})
 
                 lift_to_surface_m = m_per_unit * (surface_height_pix - tooltip_z)
-                    
+
                 debug = True
                 if debug and (self.debug_directory is not None):
                     rgb_image = h.rgb_image.copy()
@@ -827,10 +827,10 @@ class ManipulationView():
                     cv2.imwrite(dirname + filename, rgb_image)
             else:
                 rospy.loginfo('No elevated surface found.')
-            
+
         return strokes, simple_plan, lift_to_surface_m
 
-    
+
     def get_nearest_cliff(self, frame_id, tf2_buffer):
         p0 = None
         p1 = None
@@ -842,15 +842,15 @@ class ManipulationView():
 
             wrist_frame = 'link_aruco_top_wrist'
             wrist_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(wrist_frame, tf2_buffer)
-            
+
             # Obtain the wrist location in the image by obtaining the
             # translational component of the transform, which is the same
             # as multiplying by [0,0,0,1]
             wrist_x, wrist_y, wrist_z = wrist_points_to_image_mat[:, 3][:3]
             wrist_xy_pix = np.array([wrist_x, wrist_y])
-                        
+
             p0, p1, normal = detect_cliff(h.image, h.m_per_pix, h.m_per_height_unit, wrist_xy_pix)
-            if normal is not None: 
+            if normal is not None:
                 image_to_points_mat, ip_timestamp = h.get_image_to_points_mat(frame_id, tf2_buffer)
                 p0 = np.array([p0[0], p0[1], 0.0, 1.0])
                 p0 = np.matmul(image_to_points_mat, p0)[:2]
@@ -858,9 +858,9 @@ class ManipulationView():
                 p1 = np.matmul(image_to_points_mat, p1)[:2]
                 normal = np.array([normal[0], normal[1], 0.0, 0.0])
                 normal = np.matmul(image_to_points_mat, normal)[:2]
-                
+
         return p0, p1, normal
-        
+
     def update(self, point_cloud_msg, tf2_buffer):
         self.max_height_im.clear()
         cloud_time = point_cloud_msg.header.stamp
@@ -870,7 +870,7 @@ class ManipulationView():
         if only_xyz:
             xyz = rn.point_cloud2.get_xyz_points(point_cloud)
             self.max_height_im.from_points_with_tf2(xyz, cloud_frame, tf2_buffer)
-        else: 
+        else:
             rgb_points = rn.point_cloud2.split_rgb_field(point_cloud)
             self.max_height_im.from_rgb_points_with_tf2(rgb_points, cloud_frame, tf2_buffer)
         obstacle_im = self.max_height_im.image == 0
@@ -885,11 +885,11 @@ class ManipulationView():
         voi_marker_pub.publish(marker)
         point_cloud = self.max_height_im.to_point_cloud()
         point_cloud_pub.publish(point_cloud)
-        
+
 
 
 class PlanarRobotModel:
-    def __init__(self):        
+    def __init__(self):
         ##################################################################
         # PLANAR MODEL OF THE ROBOT
 
@@ -929,7 +929,7 @@ class PlanarRobotModel:
         # distance forward from the center of the wrist yaw cylinder to
         # the center of the fingertips
         self.yaw_to_fingertips_m = 0.22 # USED FOR PLANNING: 22cm for Guthrie 1
-        
+
         # -----
         # GRIPPER
         #
@@ -942,7 +942,7 @@ class PlanarRobotModel:
         # respect to the yaw axis cylinder and hence doesn't represent the
         # assymetry due to the servo being on one side of the gripper
         # thereby increasing the width substantially)
-        self.max_gripper_width_at_wrist_m = 0.1 
+        self.max_gripper_width_at_wrist_m = 0.1
         # maximum gripper width along the fingers when it is closed so the
         # fingertips are just touching each other, the measurement is made
         # where the metal bows out
@@ -956,7 +956,7 @@ class PlanarRobotModel:
         # extended without a payload
         #self.max_gripper_height_at_fingers_m = 0.9 # USED FOR PLANNING
         self.max_gripper_height_at_fingers_m = 1.09 # USED FOR PLANNING: 1.09 with tape measure for Guthrie 1 (safety margin for other robots? what if the arm or mast are tilted?
-        
+
         # distance from the ground to the bottom of the gripper's
         # fingertips when the arm is lowered and extended without a
         # payload
@@ -976,7 +976,7 @@ class PlanarRobotModel:
         # Ella: actually measured 0.51, but want to be conservative
         self.max_arm_travel_m = 0.5 # USED FOR PLANNING: about 51.25cm with Guthrie 1 (so using 0.5 for safety)
         # the height of the arm above the ground when the lift is at 0.0
-                
+
         # -----
         # MOBILE BASE
         #
@@ -1001,27 +1001,27 @@ class PlanarRobotModel:
         self.circumscribed_to_origin_m = 0.072
 
         ##################################################################
-    
+
 
 class ManipulationPlanner:
     def __init__(self):
         self.planar_model = PlanarRobotModel()
         # Region around the target over which collisions are ignored
         self.target_safe_radius_m = 0.1 # ignore 10cm radius around the target when reaching
-        
+
     def base_pose(self, max_height_image, target_xyz_pix, robot_xya_pix, image_display_on=False):
-        
+
         robot_xy_pix = np.int64(np.round(robot_xya_pix[:2]))
         robot_ang_rad = robot_xya_pix[2]
-        
+
         robot_x_pix, robot_y_pix = robot_xy_pix
         target_x, target_y, target_z = target_xyz_pix
-        
+
         image = max_height_image.image
         m_per_height_unit = max_height_image.m_per_height_unit
         m_per_pix = max_height_image.m_per_pix
         pix_per_m = 1.0 / m_per_pix
-        
+
         # The maximum height of the bottoms of the fingers at full
         # extension. This should represent the worst case for the fingers
         # moving above objects without collisions.
@@ -1040,7 +1040,7 @@ class ManipulationPlanner:
         # could be tried.
         finger_obstacle_image = np.zeros_like(image)
         finger_obstacle_image[image > target_z_pix] = 255
-        
+
         # Remove obstacles over a small area surrounding the target. For
         # example, the target might be a switch on a wall or a tall can
         # with which contact is allowed. The target location may not be
@@ -1070,7 +1070,7 @@ class ManipulationPlanner:
 
         # Find base poses that the model predicts will be able reach
         # the target.
-        
+
         # Should be greater than the distance from the mobile base
         # center to the fingertips when the arm is extended and the
         # wrist is out.
@@ -1102,7 +1102,7 @@ class ManipulationPlanner:
         close_ray_plan = False
 
         num_angles = int(np.ceil(num_angles))
-        
+
         print('num_angles =', num_angles)
         obstacle_image = dilated_finger_obstacle_image[:]
         print('pix_per_m =', pix_per_m)
@@ -1114,7 +1114,7 @@ class ManipulationPlanner:
         yaw_offset_length_m = self.planar_model.yaw_to_fingertips_m - start_distance_m
         origin_offset_left_m = yaw_offset_left_m + self.planar_model.yaw_axis_to_origin_left_m
         origin_offset_length_m = yaw_offset_length_m + self.planar_model.yaw_axis_to_origin_length_m
-        if show_all_rays: 
+        if show_all_rays:
             obstacle_image = np.zeros_like(obstacle_image)
             distance_map = np.ones_like(distance_map)
         if navigate_everywhere:
@@ -1125,7 +1125,7 @@ class ManipulationPlanner:
                                                                                                  start_distance_m, self.planar_model.max_arm_travel_m,
                                                                                                  origin_offset_left_m, origin_offset_length_m,
                                                                                                  obstacle_image)
-        if close_ray_plan: 
+        if close_ray_plan:
             # Morphologically close the rays to account for angular subsampling.
             kernel = np.ones((3,3), np.uint8)
             base_xy_image = cv2.morphologyEx(base_xy_image, cv2.MORPH_CLOSE, kernel)
@@ -1133,7 +1133,7 @@ class ManipulationPlanner:
         s = image.shape
         color_finger_obstacle_image = np.zeros([s[0], s[1], 3], np.uint8)
         color_finger_obstacle_image[:,:,0] = finger_obstacle_image
-        color_finger_obstacle_image[base_xy_image > 0] = [0, 255, 0] 
+        color_finger_obstacle_image[base_xy_image > 0] = [0, 255, 0]
         color_finger_obstacle_image[(base_xy_image == 255) & (distance_map > 0.0)] = [255, 255, 0]
         cv2.circle(color_finger_obstacle_image, (target_x, target_y), 3, [0,0,255], -1)
 
@@ -1141,7 +1141,7 @@ class ManipulationPlanner:
         # navigate and from which it can reach the target.
         navigate_and_reach_base_xy_selector = (base_xy_image == 255) & (distance_map > 0.0)
         navigate_and_reach_base_xy_image = np.uint8(navigate_and_reach_base_xy_selector)
-        
+
         # Find base positions from which the gripper can be deployed
         # (yaw joint).
         deploy_radius_pix = int(np.ceil(self.planar_model.yaw_to_fingertips_m * pix_per_m))
@@ -1151,14 +1151,14 @@ class ManipulationPlanner:
         dilated_finger_obstacle_image = cv2.dilate(finger_obstacle_image, kernel)
         # These planar model parameters are always positive. The
         # direction is handled in the numba code.
-        origin_to_yaw_left_m = self.planar_model.yaw_axis_to_origin_left_m 
+        origin_to_yaw_left_m = self.planar_model.yaw_axis_to_origin_left_m
         origin_to_yaw_length_m = self.planar_model.yaw_axis_to_origin_length_m
         tool_deploy_base_xy_image = numba_check_that_tool_can_deploy(navigate_and_reach_base_xy_image, base_ang_image,
                                                                      dilated_finger_obstacle_image,
                                                                      origin_to_yaw_left_m, origin_to_yaw_length_m,
                                                                      pix_per_m)
 
-        
+
         # Find the the candidate with the most distance between it and
         # any of the boundaries of the connected base pose candidates
         # that can also deploy the gripper.
@@ -1181,13 +1181,13 @@ class ManipulationPlanner:
         if max_val <= 0.0:
             print('No valid base pose candidate found.')
             found = False
-        else: 
+        else:
             print('Best best pose found: ({0}, {1}, {2:.2f}) = (x, y, theta_deg)'.format(base_x, base_y, base_ang_deg))
             print('     reach length = {0:.2f} m'.format(arm_reach_m))
             found = True
-        
+
         if image_display_on:
-            norm_dist_transform = cv2.normalize(distance_map, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8U)        
+            norm_dist_transform = cv2.normalize(distance_map, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8U)
             cv2.imshow('distance map without threshold for the robot width', norm_dist_transform)
 
             s = image.shape