cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Clean whitespace in manipulation_planning.py

feature/grasp_object_select
Binit Shah 1 year ago
parent
d170ef2945
commit
b8e081a19f
1 changed files with 102 additions and 102 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +102
    -102
      stretch_funmap/src/stretch_funmap/manipulation_planning.py

+ 102
- 102
stretch_funmap/src/stretch_funmap/manipulation_planning.py View File

@ -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

Write Preview
Loading…
Cancel
Save