00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 from __future__ import division
00041 import roslib
00042 roslib.load_manifest('pr2_gripper_grasp_planner_cluster')
00043 import rospy
00044 import scipy
00045 import pdb
00046 import random
00047 import math
00048 import scipy.linalg
00049 from geometry_msgs.msg import PoseStamped, Point, Pose, Vector3
00050 from sensor_msgs.msg import JointState
00051 from visualization_msgs.msg import Marker
00052 from interpolated_ik_motion_planner import ik_utilities
00053 import tf.transformations
00054 from object_manipulator.convert_functions import *
00055 import object_manipulator.draw_functions as draw_functions
00056 import object_manipulator.cluster_bounding_box_finder as cluster_bounding_box_finder
00057 import time
00058
00059 '''
00060 some dimensions:
00061 from r_wrist_roll_link to r_gripper_l_finger_tip_link: .154, .059 (-.059 to r_finger_tip_link)
00062 from fingertip tip link to tip of fingertip is 3.1 cm
00063 from fingertip tip link to front of fingertip is 1.5 cm
00064 '''
00065
00066
00067 class PointClusterGraspPlanner:
00068
00069 def __init__(self, tf_listener = None, tf_broadcaster = None):
00070
00071
00072 if tf_listener == None:
00073 self.tf_listener = tf.TransformListener()
00074 else:
00075 self.tf_listener = tf_listener
00076
00077
00078 if tf_broadcaster == None:
00079 self.tf_broadcaster = tf.TransformBroadcaster()
00080 else:
00081 self.tf_broadcaster = tf_broadcaster
00082
00083
00084 self.cbbf = cluster_bounding_box_finder.ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster)
00085
00086
00087 self.draw_functions = draw_functions.DrawFunctions('point_cluster_grasp_planner_markers')
00088
00089
00090
00091
00092
00093 self.point_id_start = 1000
00094 self.collision_point_id_start = 5000
00095 self.gripper_box_id_start = 1
00096
00097
00098 self._points_per_sq_cm = 5.
00099
00100
00101
00102
00103 self.gripper_boxes = rospy.get_param("~gripper_boxes", None)
00104 self.space_boxes = rospy.get_param("~space_boxes", None)
00105 if self.gripper_boxes == None or self.space_boxes == None:
00106 (self.gripper_boxes, self.space_boxes) = self.gripper_model()
00107
00108
00109 actual_wrist_frame_in_model_frame = rospy.get_param("~actual_wrist_frame_in_model_frame", None)
00110 if actual_wrist_frame_in_model_frame == None:
00111 self.actual_wrist_frame_in_model_frame = scipy.identity(4)
00112 rospy.logerr("no actual_wrist_frame_in_model_frame! Using identity!")
00113 else:
00114 self.actual_wrist_frame_in_model_frame = scipy.matrix(actual_wrist_frame_in_model_frame).transpose()
00115 self.model_frame_in_actual_wrist_frame = self.actual_wrist_frame_in_model_frame**-1
00116
00117
00118 self.pregrasp_dist = rospy.get_param("~default_pregrasp_dist", .10)
00119
00120
00121 self.min_good_grasp_points = rospy.get_param("min_good_grasp_points", 15)
00122
00123
00124 self._wrist_to_fingertip_center_dist = rospy.get_param("~wrist_to_fingertip_center_dist", .185)
00125
00126
00127 self._wrist_to_palm_dist = rospy.get_param("~wrist_to_palm_dist", .143)
00128
00129
00130 self.gripper_opening = rospy.get_param("~gripper_opening", .083)
00131
00132
00133 self.height_good_for_side_grasps = rospy.get_param("~height_good_for_side_grasps", .05)
00134
00135
00136 self.side_grasp_start_height = rospy.get_param("~side_grasp_start_height", .04)
00137
00138
00139 self.side_step = rospy.get_param("~side_step", .02)
00140
00141
00142 self.palm_step = rospy.get_param("~palm_step", .005)
00143
00144
00145 self.overhead_grasps_only = rospy.get_param("~overhead_grasps_only", False)
00146
00147
00148 self.side_grasps_only = rospy.get_param("~side_grasps_only", False)
00149
00150
00151 self.include_high_point_grasps = rospy.get_param("~include_high_point_grasps", True)
00152
00153
00154 self.pregrasp_just_outside_box = rospy.get_param("~pregrasp_just_outside_box", False)
00155
00156
00157 self.backoff_depth_steps = rospy.get_param("~backoff_depth_steps", 5)
00158 if self.backoff_depth_steps < 1:
00159 self.backoff_depth_steps = 1
00160
00161
00162 self.disable_grasp_neighbor_check = rospy.get_param("~disable_grasp_neighbor_check", False)
00163
00164
00165 self._output_features = 0
00166 if self._output_features:
00167 self._outfile = file("feature_weights.txt", 'w')
00168
00169
00170 self.debug = 0
00171 self.draw_gripper = 0
00172
00173
00174
00175 def pplist(self, list):
00176 return ' '.join(['%5.3f'%x for x in list])
00177
00178
00179
00180 def draw_gripper_model(self, pose_mat, frame = 'object_frame', pause_after_broadcast = 0):
00181
00182
00183 (pos, quat) = mat_to_pos_and_quat(pose_mat)
00184 (object_frame_pos, object_frame_quat) = mat_to_pos_and_quat(self.object_to_cluster_frame)
00185 if frame == 'object_frame':
00186 self.tf_broadcaster.sendTransform(object_frame_pos, object_frame_quat, rospy.Time.now(), "object_frame", self.cluster_frame)
00187 now = rospy.Time.now()
00188 self.tf_broadcaster.sendTransform(pos, quat, now, 'wrist_frame', frame)
00189 if pause_after_broadcast:
00190 time.sleep(.1)
00191 self.tf_listener.waitForTransform('wrist_frame', frame, now, rospy.Duration(2.0))
00192
00193 box_id = 0
00194 for i in range(len(self.gripper_boxes)):
00195 color = [0,1,0]
00196 self.draw_functions.draw_rviz_box(scipy.matrix(scipy.identity(4)), ranges = self.gripper_boxes[i], frame = 'wrist_frame', ns = "gripper_model", \
00197 id = i, color = color, duration = 30, opaque = .75)
00198 box_id += len(self.gripper_boxes)
00199 for i in range(len(self.space_boxes)):
00200 for j in range(len(self.space_boxes[i])):
00201 color = [0,.2*i,1]
00202 self.draw_functions.draw_rviz_box(scipy.matrix(scipy.identity(4)), ranges = self.space_boxes[i][j], frame = 'wrist_frame', ns = "gripper_model", \
00203 id = box_id, color=color, duration = 30, opaque = .75)
00204 box_id += 1
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 def gripper_model(self):
00216
00217
00218
00219 palm_box = [[.04, -.0755, -.026], [.138, .0755, .026]]
00220 left_finger_box = [[.138, .0425, -.0115], [.185, .0755, .0115]]
00221 right_finger_box = [[.138, -.0755, -.0115], [.185, -.0425, .0115]]
00222
00223 space_box = [[.138, -.0405, -.005], [.180, .0405, .005]]
00224 gripper_boxes = [palm_box, left_finger_box, right_finger_box]
00225 space_boxes = [[space_box,],]
00226
00227 return gripper_boxes, space_boxes
00228
00229
00230
00231 def find_points_in_bounding_box(self, wrist_frame_points, ranges, return_points = 0):
00232
00233
00234 gt = scipy.array(wrist_frame_points[0:3,:]).T > scipy.array(ranges[0])
00235 lt = scipy.array(wrist_frame_points[0:3,:]).T < scipy.array(ranges[1])
00236 fits_in_box_bool = scipy.logical_and(gt, lt)
00237 fits_in_box = scipy.all(fits_in_box_bool, axis=1)
00238
00239 if return_points:
00240 points_that_fit = wrist_frame_points[:, fits_in_box]
00241 return (fits_in_box.sum(), points_that_fit)
00242
00243
00244
00245 return fits_in_box.sum()
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255 def transform_ranges(self, transform, ranges):
00256 corners = [scipy.matrix([ranges[xind][0], ranges[yind][1], ranges[zind][2], 1]).T for \
00257 (xind, yind, zind) in scipy.ndindex(2,2,2)]
00258 corners = scipy.concatenate(corners, axis=1)
00259 transformed_corners = transform*corners
00260 transformed_corners_ranges = [transformed_corners.min(axis=1)[0:3].T.tolist()[0], transformed_corners.max(axis=1)[0:3].T.tolist()[0]]
00261 return transformed_corners_ranges
00262
00263
00264
00265
00266
00267 def check_box_plane_collisions(self, pose, ranges):
00268
00269
00270 transformed_corners = self.transform_ranges(pose, ranges)
00271 for transformed_corner in transformed_corners:
00272 if transformed_corner[2] < -0.005:
00273
00274
00275
00276
00277 return 1
00278 return 0
00279
00280
00281
00282
00283
00284
00285 def check_gripper_pose(self, points, pose):
00286
00287 if self.debug:
00288
00289 for i in range(3):
00290 self.draw_functions.clear_rviz_points(ns = 'box_point_collision_points', id = i)
00291 self.draw_functions.clear_rviz_points(ns = 'inside_gripper_points', id = 0)
00292
00293
00294 if self.draw_gripper:
00295 self.draw_gripper_model(pose)
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306 world_to_wrist = pose**-1
00307 transformed_points = world_to_wrist * points
00308
00309
00310
00311
00312 num_collision_points = 0
00313 for i in range(len(self.gripper_boxes)):
00314 if self.debug:
00315 (num_points, points) = self.find_points_in_bounding_box(transformed_points, self.gripper_boxes[i], 1)
00316 else:
00317 num_points = self.find_points_in_bounding_box(transformed_points, self.gripper_boxes[i], 0)
00318 if num_points != 0:
00319 if self.debug:
00320 print "box-point collision"
00321 self.draw_functions.draw_rviz_points(points, frame = 'wrist_frame', size = .0075, ns = 'box_point_collision_points', id = i, \
00322 duration = 20., color = [1,0,0], opaque = 1)
00323
00324 num_collision_points += num_points
00325
00326
00327
00328 debug_points = None
00329 min_space_points = 1e6
00330 current_list_space_points = 0
00331 for i in range(len(self.space_boxes)):
00332 for j in range(len(self.space_boxes[i])):
00333 if self.debug:
00334 (num_space_points, points) = self.find_points_in_bounding_box(transformed_points, self.space_boxes[i][j], 1)
00335 if debug_points == None:
00336 debug_points = points
00337 else:
00338 debug_points = scipy.hstack((debug_points, points))
00339 else:
00340 num_space_points = self.find_points_in_bounding_box(transformed_points, self.space_boxes[i][j], 0)
00341 current_list_space_points += num_space_points
00342 if current_list_space_points < min_space_points:
00343 min_space_points = current_list_space_points
00344 if min_space_points and self.debug:
00345 print "min_space_points:", min_space_points
00346 if debug_points != None:
00347 self.draw_functions.draw_rviz_points(debug_points, frame = 'wrist_frame', size = .0075, ns = 'inside_gripper_points', id = 0., \
00348 duration = 20., color = [0,0,1], opaque = 1)
00349 if self.debug:
00350 self.keypause()
00351
00352 return (min_space_points, num_collision_points)
00353
00354
00355
00356 def evaluate_arbitrary_grasp(self, points, pose):
00357
00358
00359
00360
00361 orthogonality = self.orthogonal_measure(pose)
00362
00363
00364 (x_dist, y_dist, z_dist) = self.find_fingertip_object_center_dists(pose)
00365
00366
00367 overhead_angle = self.overhead_angle(pose)
00368 overhead_grasp = max(0, 1 - overhead_angle / (math.pi/2))
00369
00370
00371 fits_in_hand = self.object_fits_in_hand(points, pose)
00372
00373
00374 not_edge = self.check_grasp_neighbors(points, pose)
00375
00376
00377 (point_count, collision_points) = self.check_gripper_pose(points, pose)
00378
00379
00380 if point_count < self.min_good_grasp_points:
00381
00382
00383
00384 return 0
00385
00386
00387 probability = self.grasp_quality(point_count, x_dist, z_dist, overhead_grasp, fits_in_hand, not_edge, orthogonality, y_dist, collision_point_count = collision_points)
00388
00389 return probability
00390
00391
00392
00393 def quality_to_probability(self, quality):
00394
00395
00396 center = 50
00397 scale = 150. / 3.6
00398 t = (quality - center) / scale
00399 if t < -50:
00400 return 0
00401 prob = 1./(1.+math.exp(-t))
00402 return prob
00403
00404
00405
00406
00407 def orthogonal_measure(self, pose):
00408
00409
00410 aligned_pose = pose.copy()
00411 for axis in (0, 1):
00412 axis_vect = scipy.array(aligned_pose[0:3, axis].T)[0]
00413 closest_axis = scipy.fabs(axis_vect).argmax()
00414 axis_dir = axis_vect[closest_axis] / abs(axis_vect[closest_axis])
00415 closest_axis_vect = scipy.zeros(3)
00416 closest_axis_vect[closest_axis] = axis_dir
00417 dot = min(1, max(0, scipy.dot(closest_axis_vect, axis_vect)))
00418 angle = math.acos(dot)
00419 if math.fabs(angle) > 1e-6:
00420 rot_vect = scipy.cross(axis_vect, closest_axis_vect)
00421 align_rot = scipy.matrix(tf.transformations.rotation_matrix(angle, rot_vect))
00422 aligned_pose = align_rot * aligned_pose
00423
00424
00425 rel_mat = aligned_pose**-1 * pose
00426 rel_quat = tf.transformations.quaternion_from_matrix(rel_mat)
00427 if rel_quat[3] > 1.0:
00428 angle = 0
00429 elif rel_quat[3] < -1.0:
00430 angle = math.pi
00431 else:
00432 angle = 2*math.acos(rel_quat[3])
00433
00434
00435 orthogonality = max(0, 1 - angle / (math.pi/6))
00436 return orthogonality
00437
00438
00439
00440 def find_fingertip_object_center_dists(self, pose):
00441
00442
00443 object_center = scipy.matrix([0, 0, self.object_bounding_box_dims[2]/2, 1]).T
00444 gripper_frame_center = pose**-1 * object_center
00445
00446
00447 x_dist = gripper_frame_center[0,0] - self._wrist_to_fingertip_center_dist
00448
00449
00450 y_dist = math.fabs(gripper_frame_center[1,0])
00451 z_dist = math.fabs(gripper_frame_center[2,0])
00452
00453 return (x_dist, y_dist, z_dist)
00454
00455
00456
00457 def object_fits_in_hand(self, points, pose):
00458
00459
00460 transformed_points = pose**-1 * points
00461
00462
00463 return not (scipy.fabs(transformed_points[1, :]) > self.gripper_opening).any()
00464
00465
00466
00467 def overhead_angle(self, pose):
00468
00469 z_comp_of_x_axis = max(0, min(1, -pose[2,0]))
00470 angle = math.acos(z_comp_of_x_axis)
00471 return angle
00472
00473
00474
00475 def check_grasp_neighbors(self, points, grasp):
00476
00477
00478
00479
00480
00481
00482 neighbor_dist = .02
00483 for sign in [-1., 1.]:
00484
00485
00486 shift_mat = scipy.matrix(tf.transformations.translation_matrix([0,0,sign*neighbor_dist]))
00487 shifted_grasp = grasp * shift_mat
00488
00489
00490
00491
00492
00493
00494 (point_count, collision_points) = self.check_gripper_pose(points, shifted_grasp)
00495 if point_count < self.min_good_grasp_points or collision_points > 0:
00496
00497
00498 return 0
00499
00500
00501
00502
00503 return 1
00504
00505
00506
00507 def grasp_quality(self, point_count, palm_dist, side_dist, overhead_grasp, fits_in_hand, not_edge, orthogonality = 1, centered_dist = 0, points = None, grasp = None, collision_point_count = 0):
00508
00509
00510
00511
00512
00513
00514
00515
00516 feature_vector = [point_count, palm_dist, side_dist, overhead_grasp, fits_in_hand, not_edge, orthogonality, centered_dist, collision_point_count]
00517 if self._output_features:
00518 self._outfile.write(str(feature_vector)+'\n')
00519
00520
00521 point_count_weight = 5./self._points_per_sq_cm
00522
00523
00524 weights = [point_count_weight, -10./0.01, -10./0.01, 10., 50., 50., 50., -10./0.01, -1000]
00525 quality = sum([w*f for (w,f) in zip(weights, feature_vector)])
00526
00527 prob = self.quality_to_probability(quality)
00528
00529
00530
00531 return prob
00532
00533
00534
00535
00536
00537
00538 def find_best_palm_dir_grasp(self, points, start_pose, palm_step, object_bounding_box):
00539
00540
00541
00542 max_dist = max([(upper-lower)/2 for (upper, lower) in zip(object_bounding_box[1], object_bounding_box[0])]) + .06
00543
00544 good_grasps = []
00545 good_grasp_point_counts = []
00546 good_grasp_dists_moved = []
00547 new_pose = start_pose.copy()
00548 dist_moved = 0.
00549 while(1):
00550
00551
00552
00553 (point_count, collision_points) = self.check_gripper_pose(points, new_pose)
00554
00555
00556
00557 if point_count < 0 or collision_points > 0:
00558 break
00559
00560
00561 if point_count > self.min_good_grasp_points:
00562 good_grasps.append(new_pose.copy())
00563 good_grasp_point_counts.append(point_count)
00564 good_grasp_dists_moved.append(dist_moved)
00565
00566
00567 new_pose[0:3, 3] += start_pose[0:3, 0] * palm_step
00568 dist_moved += palm_step
00569
00570
00571 if dist_moved > max_dist:
00572 break
00573
00574 return (good_grasps[-self.backoff_depth_steps:], good_grasp_point_counts[-self.backoff_depth_steps:], good_grasp_dists_moved[-self.backoff_depth_steps:])
00575
00576
00577
00578
00579
00580
00581
00582 def find_grasps_along_direction(self, points, start_pose, axis, spacing, palm_step, object_bounding_box, max_side_move = None, omit_center = False):
00583
00584
00585 min_edge_dist = .02
00586
00587 dist = 0.
00588 positive_done = 0
00589 negative_done = 0
00590 new_pose = start_pose.copy()
00591 good_grasps = []
00592
00593 while(not positive_done or not negative_done):
00594 if self.debug:
00595 print "dist:", dist
00596
00597
00598 new_pose[axis, 3] = dist + start_pose[axis, 3]
00599
00600
00601 if not positive_done and not (dist == 0 and omit_center):
00602 (grasps, point_counts, palm_dists_moved) = self.find_best_palm_dir_grasp(points, new_pose, palm_step, object_bounding_box)
00603 for (grasp, point_count, palm_dist_moved) in zip(grasps, point_counts, palm_dists_moved):
00604 good_grasps.append([grasp, point_count, palm_dist_moved, dist])
00605
00606
00607 if dist != 0.:
00608
00609 if not negative_done:
00610 new_pose[axis, 3] = -dist + start_pose[axis, 3]
00611 (grasps, point_counts, palm_dists_moved) = self.find_best_palm_dir_grasp(points, new_pose, palm_step, object_bounding_box)
00612 for (grasp, point_count, palm_dist_moved) in zip(grasps, point_counts, palm_dists_moved):
00613 good_grasps.append([grasp, point_count, palm_dist_moved, dist])
00614
00615
00616 dist += spacing
00617 if max_side_move != None and dist > max_side_move:
00618 break
00619 if -dist + start_pose[axis, 3] < object_bounding_box[0][axis] + min_edge_dist:
00620 negative_done = 1
00621 if dist + start_pose[axis, 3] > object_bounding_box[1][axis] - min_edge_dist:
00622 positive_done = 1
00623
00624 return good_grasps
00625
00626
00627
00628 def find_high_point_grasps(self, points, object_bounding_box, object_bounding_box_dims, palm_step, max_grasps_to_return = 40):
00629 print "starting high_point_grasps"
00630
00631 max_grasps_to_find = int(math.floor(max_grasps_to_return * 1.5))
00632
00633
00634 num_points = scipy.shape(points)[1]
00635 num_selected_points = max_grasps_to_find*2
00636 max_dist_from_top = .02
00637 step_size = 50
00638
00639 highest_z = points[2,-1]
00640 ind_of_lowest_pt = scipy.searchsorted(points[2, :].tolist()[0], highest_z - max_dist_from_top)
00641 print "ind_of_lowest_pt:", ind_of_lowest_pt
00642
00643
00644 selected_point_inds = scipy.random.randint(ind_of_lowest_pt, num_points-2, (1,num_selected_points))
00645 selected_point_inds[0,0] = num_points-1
00646 selected_points = points[:, selected_point_inds.tolist()[0]]
00647
00648
00649
00650
00651 grasps_with_features = []
00652 for point_ind in range(num_selected_points):
00653 point = selected_points[:, point_ind]
00654
00655
00656 wrist_z_pos = object_bounding_box_dims[2] + self._wrist_to_fingertip_center_dist
00657
00658
00659 start_pose = scipy.matrix(scipy.identity(4))
00660 point_xy_mag = math.sqrt(point[0:2,0].T*point[0:2,0])
00661 x_axis = scipy.matrix([0.,0.,-1.]).T
00662 y_axis = scipy.matrix([point[0,0]/point_xy_mag, point[1,0]/point_xy_mag, 0]).T
00663 z_axis = scipy.matrix(scipy.cross(x_axis.T, y_axis.T).T)
00664 start_pose[0:3, 0] = x_axis
00665 start_pose[0:3, 1] = y_axis
00666 start_pose[0:3, 2] = z_axis
00667 start_pose[0, 3] = point[0,0]
00668 start_pose[1, 3] = point[1,0]
00669 start_pose[2, 3] = wrist_z_pos
00670
00671
00672 (grasps, point_counts, palm_dists_moved) = self.find_best_palm_dir_grasp(points, start_pose, palm_step, object_bounding_box)
00673
00674 for (grasp, point_count, palm_dist_moved) in zip(grasps, point_counts, palm_dists_moved):
00675 grasps_with_features.append((grasp, point_count, palm_dist_moved))
00676
00677
00678 if len(grasps_with_features) > max_grasps_to_find:
00679 break
00680
00681
00682 grasps_with_qualities = []
00683 for (grasp, point_count, palm_dist_moved) in grasps_with_features:
00684
00685
00686 not_edge = 1
00687 if not self.disable_grasp_neighbor_check:
00688 not_edge = self.check_grasp_neighbors(points, grasp)
00689
00690
00691 orthogonality = self.orthogonal_measure(grasp)
00692
00693
00694 (x_dist, y_dist, z_dist) = self.find_fingertip_object_center_dists(grasp)
00695
00696
00697 fits_in_hand = self.object_fits_in_hand(points, grasp)
00698
00699 quality = self.grasp_quality(point_count, x_dist, y_dist, overhead_grasp = 1, fits_in_hand = fits_in_hand, not_edge = not_edge,
00700 orthogonality = orthogonality, centered_dist = z_dist, points = points, grasp = grasp)
00701 grasps_with_qualities.append([grasp, quality, palm_dist_moved])
00702
00703
00704
00705 grasps_with_qualities = sorted(grasps_with_qualities, key=lambda t:t[1], reverse=True)
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716 rospy.loginfo("high point grasp qualities: " + self.pplist([x[1] for x in grasps_with_qualities]))
00717
00718 return grasps_with_qualities[:max_grasps_to_return]
00719
00720
00721
00722 def keypause(self):
00723 print "press enter to continue"
00724 c = raw_input()
00725 return c
00726
00727
00728
00729 def init_cluster_grasper(self, cluster):
00730
00731 self.cluster_frame = cluster.header.frame_id
00732
00733
00734 (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
00735 self.object_to_base_frame, self.object_to_cluster_frame) = \
00736 self.cbbf.find_object_frame_and_bounding_box(cluster)
00737
00738
00739 gripper_space = [self.gripper_opening - self.object_bounding_box_dims[i] for i in range(3)]
00740 self._box_fits_in_hand = [gripper_space[i] > 0 for i in range(3)]
00741
00742
00743 if self._box_fits_in_hand[0] and self._box_fits_in_hand[1]:
00744 if gripper_space[0] > gripper_space[1] and self.object_bounding_box_dims[0]/self.object_bounding_box_dims[1] < .8:
00745 self._box_fits_in_hand[1] *= .5
00746 elif gripper_space[1] > gripper_space[0] and self.object_bounding_box_dims[1]/self.object_bounding_box_dims[0] < .8:
00747 self._box_fits_in_hand[0] *= .5
00748
00749
00750
00751
00752 bounding_box_surf = (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[1]*100) + \
00753 (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[2]*100) + \
00754 (self.object_bounding_box_dims[1]*100 * self.object_bounding_box_dims[2]*100)
00755 self._points_per_sq_cm = self.object_points.shape[1] / bounding_box_surf
00756
00757
00758
00759
00760
00761 def evaluate_point_cluster_grasps(self, grasps, frame):
00762
00763
00764 grasp_poses = []
00765 cluster_to_object_frame = self.object_to_cluster_frame**-1
00766 for grasp in grasps:
00767 pose_mat = pose_to_mat(grasp.grasp_pose)
00768
00769
00770 obj_frame_mat = cluster_to_object_frame * pose_mat * self.model_frame_in_actual_wrist_frame
00771 grasp_poses.append(obj_frame_mat)
00772
00773
00774 probs = [self.evaluate_arbitrary_grasp(self.object_points, pose) for pose in grasp_poses]
00775
00776 return probs
00777
00778
00779
00780 def plan_point_cluster_grasps(self):
00781
00782 if self.debug:
00783 print "about to find grasps"
00784 self.keypause()
00785
00786
00787 found_grasps = []
00788 found_grasps1 = []
00789 found_grasps2 = []
00790 num_overhead_grasps = 0
00791 num_overhead_grasps1 = 0
00792
00793
00794 top_wrist_z_pos_palm = self.object_bounding_box[1][2] + self._wrist_to_palm_dist
00795 top_y_start_pose_palm = scipy.matrix([[0.,0.,-1.,0.],
00796 [0.,-1.,0.,0.],
00797 [-1.,0.,0.,top_wrist_z_pos_palm],
00798 [0.,0.,0.,1.]])
00799 top_x_start_pose_palm = scipy.matrix([[0.,-1.,0.,0.],
00800 [0.,0.,1.,0.],
00801 [-1.,0.,0.,top_wrist_z_pos_palm],
00802 [0.,0.,0.,1.]])
00803
00804 y_center_grasps_found = 0
00805 x_center_grasps_found = 0
00806 if not self.side_grasps_only:
00807
00808 if self._box_fits_in_hand[1] > 0:
00809 grasp_list = self.find_grasps_along_direction(self.object_points, top_y_start_pose_palm, 0, \
00810 self.side_step, self.palm_step, self.object_bounding_box, max_side_move = 0.)
00811 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list):
00812 quality = self.grasp_quality(point_count, \
00813 palm_dist = top_wrist_z_pos_palm-self._wrist_to_palm_dist-palm_dist_moved-self.object_bounding_box_dims[2]/2., \
00814 side_dist = dist, overhead_grasp = 1, fits_in_hand = self._box_fits_in_hand[1], \
00815 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp)
00816 found_grasps.append([grasp, quality, palm_dist_moved])
00817 y_center_grasps_found = 1
00818
00819
00820 if self._box_fits_in_hand[0] > 0:
00821 grasp_list = self.find_grasps_along_direction(self.object_points, top_x_start_pose_palm, 1, \
00822 self.side_step, self.palm_step, self.object_bounding_box, max_side_move = 0.)
00823 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list):
00824 quality = self.grasp_quality(point_count, \
00825 palm_dist = top_wrist_z_pos_palm-self._wrist_to_palm_dist-palm_dist_moved-self.object_bounding_box_dims[2]/2., \
00826 side_dist = dist, overhead_grasp = 1, fits_in_hand = self._box_fits_in_hand[0], \
00827 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp)
00828 found_grasps1.append([grasp, quality, palm_dist_moved])
00829 x_center_grasps_found = 1
00830 rospy.loginfo("overhead grasp (x) qualities: " + self.pplist([x[1] for x in found_grasps]))
00831 rospy.loginfo("overhead grasp (y) qualities: " + self.pplist([x[1] for x in found_grasps1]))
00832 num_overhead_grasps = len(found_grasps)
00833 num_overhead_grasps1 = len(found_grasps1)
00834
00835 if not self.overhead_grasps_only:
00836
00837
00838 side_wrist_z_pos = max(self.object_bounding_box[1][2]/2., self.side_grasp_start_height)
00839 side_wrist_y_pos = self.object_bounding_box[1][1] + self._wrist_to_palm_dist
00840 side_wrist_x_pos = self.object_bounding_box[1][0] + self._wrist_to_palm_dist
00841
00842
00843 if self._box_fits_in_hand[1] > 0 and self.object_bounding_box_dims[2] > self.height_good_for_side_grasps:
00844
00845
00846 start_pose = scipy.matrix([[-1.,0.,0.,side_wrist_x_pos],
00847 [0.,-1.,0.,0],
00848 [0.,0.,1.,side_wrist_z_pos],
00849 [0.,0.,0.,1.]])
00850 grasp_list = self.find_grasps_along_direction(self.object_points, start_pose, 2, \
00851 self.side_step, self.palm_step, self.object_bounding_box)
00852 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list):
00853 quality = self.grasp_quality(point_count, \
00854 palm_dist = side_wrist_x_pos-self._wrist_to_palm_dist-palm_dist_moved, \
00855 side_dist = dist, overhead_grasp = 0, fits_in_hand = self._box_fits_in_hand[1], \
00856 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp)
00857 found_grasps.append([grasp, quality, palm_dist_moved])
00858
00859
00860 start_pose = scipy.matrix([[1.,0.,0.,-side_wrist_x_pos],
00861 [0.,-1.,0.,0],
00862 [0.,0.,-1.,side_wrist_z_pos],
00863 [0.,0.,0.,1.]])
00864 grasp_list = self.find_grasps_along_direction(self.object_points, start_pose, 2, self.side_step, \
00865 self.palm_step, self.object_bounding_box)
00866 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list):
00867 quality = self.grasp_quality(point_count, palm_dist = side_wrist_x_pos-self._wrist_to_palm_dist-palm_dist_moved, \
00868 side_dist = dist, overhead_grasp = 0, fits_in_hand = self._box_fits_in_hand[1], \
00869 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp)
00870 found_grasps.append([grasp, quality, palm_dist_moved])
00871
00872 if self._box_fits_in_hand[0] > 0 and self.object_bounding_box_dims[2] > self.height_good_for_side_grasps:
00873
00874
00875 start_pose = scipy.matrix([[0.,-1.,0.,0.],
00876 [1.,0.,0.,-side_wrist_y_pos],
00877 [0.,0.,1.,side_wrist_z_pos],
00878 [0.,0.,0.,1.]])
00879 grasp_list = self.find_grasps_along_direction(self.object_points, start_pose, 2, self.side_step, self.palm_step, self.object_bounding_box)
00880 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list):
00881 quality = self.grasp_quality(point_count, palm_dist = side_wrist_y_pos-self._wrist_to_palm_dist-palm_dist_moved, \
00882 side_dist = dist, overhead_grasp = 0, fits_in_hand = self._box_fits_in_hand[0], \
00883 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp)
00884 found_grasps1.append([grasp, quality, palm_dist_moved])
00885
00886
00887 start_pose = scipy.matrix([[0.,1.,0.,0.],
00888 [-1.,0.,0.,side_wrist_y_pos],
00889 [0.,0.,1.,side_wrist_z_pos],
00890 [0.,0.,0.,1.]])
00891 grasp_list = self.find_grasps_along_direction(self.object_points, start_pose, 2, self.side_step, self.palm_step, self.object_bounding_box)
00892 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list):
00893 quality = self.grasp_quality(point_count, palm_dist = side_wrist_y_pos-self._wrist_to_palm_dist-palm_dist_moved, \
00894 side_dist = dist, overhead_grasp = 0, fits_in_hand = self._box_fits_in_hand[0], \
00895 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp)
00896 found_grasps1.append([grasp, quality, palm_dist_moved])
00897 rospy.loginfo("side grasp (x) qualities: " + self.pplist([x[1] for x in found_grasps[num_overhead_grasps:]]))
00898 rospy.loginfo("side grasp (y) qualities: " + self.pplist([x[1] for x in found_grasps1[num_overhead_grasps1:]]))
00899
00900 if not self.side_grasps_only:
00901
00902
00903
00904
00905 top_wrist_z_pos_fingertip = self.object_bounding_box[1][2] + self._wrist_to_fingertip_center_dist
00906 top_y_start_pose_fingertip = scipy.matrix([[0.,0.,-1.,0.],
00907 [0.,-1.,0.,0.],
00908 [-1.,0.,0.,top_wrist_z_pos_fingertip],
00909 [0.,0.,0.,1.]])
00910 top_x_start_pose_fingertip = scipy.matrix([[0.,-1.,0.,0.],
00911 [0.,0.,1.,0.],
00912 [-1.,0.,0.,top_wrist_z_pos_fingertip],
00913 [0.,0.,0.,1.]])
00914
00915
00916 grasp_list = self.find_grasps_along_direction(self.object_points, top_y_start_pose_fingertip, 0, self.side_step, self.palm_step,
00917 self.object_bounding_box, omit_center = y_center_grasps_found)
00918 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list):
00919 quality = self.grasp_quality(point_count, \
00920 palm_dist = top_wrist_z_pos_fingertip-self._wrist_to_fingertip_center_dist-palm_dist_moved-self.object_bounding_box_dims[2]/2., \
00921 side_dist = dist, overhead_grasp = 1, fits_in_hand = self.object_bounding_box_dims[1] < self.gripper_opening, \
00922 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp)
00923 found_grasps2.append([grasp, quality, palm_dist_moved])
00924
00925
00926 grasp_list = self.find_grasps_along_direction(self.object_points, top_x_start_pose_fingertip, 1, self.side_step, self.palm_step,
00927 self.object_bounding_box, omit_center = x_center_grasps_found)
00928 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list):
00929 quality = self.grasp_quality(point_count, \
00930 palm_dist = top_wrist_z_pos_fingertip-self._wrist_to_fingertip_center_dist-palm_dist_moved-self.object_bounding_box_dims[2]/2., \
00931 side_dist = dist, overhead_grasp = 1, fits_in_hand = self.object_bounding_box_dims[0] < self.gripper_opening, \
00932 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp)
00933 found_grasps2.append([grasp, quality, palm_dist_moved])
00934 rospy.loginfo("along-axes overhead grasp qualities: " + self.pplist([x[1] for x in found_grasps2]))
00935 num_overhead_grasps = len(found_grasps)
00936
00937
00938 if self.include_high_point_grasps:
00939 grasps_with_qualities = self.find_high_point_grasps(self.object_points, self.object_bounding_box, self.object_bounding_box_dims, self.palm_step)
00940 found_grasps2.extend(grasps_with_qualities)
00941
00942
00943 found_grasps = sorted(found_grasps, key=lambda t:t[1], reverse=True)
00944 found_grasps1 = sorted(found_grasps1, key=lambda t:t[1], reverse=True)
00945 found_grasps2 = sorted(found_grasps2, key=lambda t:t[1], reverse=True)
00946
00947
00948 if self._box_fits_in_hand[0] < self._box_fits_in_hand[1]:
00949 found_grasps = found_grasps + found_grasps1 + found_grasps2
00950 elif self._box_fits_in_hand[1] < self._box_fits_in_hand[0]:
00951 found_grasps = found_grasps1 + found_grasps + found_grasps2
00952 else:
00953 found_grasps = sorted(found_grasps + found_grasps1, key=lambda t:t[1], reverse=True) + found_grasps2
00954
00955 rospy.loginfo("total number of grasps found:" + str(len(found_grasps)))
00956
00957
00958 grasp_poses = []
00959 pregrasp_poses = []
00960
00961 pregrasp_dists = []
00962 qualities = []
00963
00964
00965 if found_grasps and self.draw_gripper:
00966 (best_grasp_mat, best_grasp_quality, palm_dist_moved) = found_grasps[0]
00967 self.draw_gripper_model(best_grasp_mat, pause_after_broadcast = 0)
00968
00969
00970 for (grasp_mat, quality, palm_dist_moved) in found_grasps:
00971
00972
00973
00974 back_up_mat = scipy.identity(4)
00975 if not self.pregrasp_just_outside_box:
00976 back_up_mat[0,3] = -self.pregrasp_dist
00977 pregrasp_dists.append(self.pregrasp_dist)
00978 else:
00979 back_up_mat[0,3] = -palm_dist_moved-.005
00980 pregrasp_dists.append(palm_dist_moved+.005)
00981 pregrasp_mat = grasp_mat * back_up_mat
00982
00983
00984 grasp_mat = grasp_mat * self.actual_wrist_frame_in_model_frame
00985 pregrasp_mat = pregrasp_mat * self.actual_wrist_frame_in_model_frame
00986
00987
00988 pregrasp_pose = mat_to_pose(pregrasp_mat, self.object_to_cluster_frame)
00989 grasp_pose = mat_to_pose(grasp_mat, self.object_to_cluster_frame)
00990
00991 pregrasp_poses.append(pregrasp_pose)
00992 grasp_poses.append(grasp_pose)
00993 qualities.append(quality)
00994
00995
00996 gripper_openings = [.1]*len(grasp_poses)
00997
00998 return (pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists)
00999
01000
01001