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