diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index a979d13..4501c36 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/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