$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of the Willow Garage nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 # 00034 # author: Kaijen Hsiao 00035 00036 ## @package pr2_gripper_grasp_planner_cluster 00037 #Finds candidate grasps for point cloud clusters by finding the principal directions of the 00038 #cloud and searching for orthogonal grasps (and a few extra grasps of high points) 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 from object_manipulator.convert_functions import * 00057 import object_manipulator.draw_functions as draw_functions 00058 import object_manipulator.cluster_bounding_box_finder as cluster_bounding_box_finder 00059 import time 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 ##Class for doing grasp planning on point clusters 00069 class PointClusterGraspPlanner: 00070 00071 def __init__(self, tf_listener = None, tf_broadcaster = None): 00072 00073 #init a TF transform listener 00074 if tf_listener == None: 00075 self.tf_listener = tf.TransformListener() 00076 else: 00077 self.tf_listener = tf_listener 00078 00079 #init a TF transform broadcaster for the object frame 00080 if tf_broadcaster == None: 00081 self.tf_broadcaster = tf.TransformBroadcaster() 00082 else: 00083 self.tf_broadcaster = tf_broadcaster 00084 00085 #init a FindClusterBoundingBox object 00086 self.cbbf = cluster_bounding_box_finder.ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster) 00087 00088 #draw_functions object for drawing stuff in rviz 00089 self.draw_functions = draw_functions.DrawFunctions('point_cluster_grasp_planner_markers') 00090 00091 #publisher for displaying gripper boxes in rviz 00092 #self.marker_pub = rospy.Publisher('point_cluster_grasp_planner_markers', Marker) 00093 00094 #keep track of the ids used for various markers 00095 self.point_id_start = 1000 00096 self.collision_point_id_start = 5000 00097 self.gripper_box_id_start = 1 00098 00099 #average number of points per sq. cm of bounding box surface 00100 self._points_per_sq_cm = 5. 00101 00102 #params for the gripper model/grasp search 00103 00104 #the axis-aligned model of the gripper for checking for collisions and points in the hand 00105 self.gripper_boxes = rospy.get_param("~gripper_boxes", None) 00106 self.space_boxes = rospy.get_param("~space_boxes", None) 00107 if self.gripper_boxes == None or self.space_boxes == None: 00108 (self.gripper_boxes, self.space_boxes) = self.gripper_model() 00109 00110 #the actual desired wrist frame of the robot (4x4, column-order) in the gripper model frame 00111 actual_wrist_frame_in_model_frame = rospy.get_param("~actual_wrist_frame_in_model_frame", None) 00112 if actual_wrist_frame_in_model_frame == None: 00113 self.actual_wrist_frame_in_model_frame = scipy.identity(4) 00114 rospy.logerr("no actual_wrist_frame_in_model_frame! Using identity!") 00115 else: 00116 self.actual_wrist_frame_in_model_frame = scipy.matrix(actual_wrist_frame_in_model_frame).transpose() 00117 self.model_frame_in_actual_wrist_frame = self.actual_wrist_frame_in_model_frame**-1 00118 00119 #default pregrasp dist away from grasp (overridden if pregrasp_just_outside_box is True) 00120 self.pregrasp_dist = rospy.get_param("~default_pregrasp_dist", .10) 00121 00122 #minimum number of points to be declared a good grasp 00123 self.min_good_grasp_points = rospy.get_param("min_good_grasp_points", 15) 00124 00125 #distance from the wrist frame to the center of the fingertips (along the gripper x-axis) 00126 self._wrist_to_fingertip_center_dist = rospy.get_param("~wrist_to_fingertip_center_dist", .185) 00127 00128 #distance from the wrist frame to a safe distance barely past the surface of the palm (along the gripper x-axis) 00129 self._wrist_to_palm_dist = rospy.get_param("~wrist_to_palm_dist", .143) 00130 00131 #bounding box "fits in hand" if the relevant dimension is less than this 00132 self.gripper_opening = rospy.get_param("~gripper_opening", .083) 00133 00134 #only want side grasps if the bounding box height is greater than this 00135 self.height_good_for_side_grasps = rospy.get_param("~height_good_for_side_grasps", .05) 00136 00137 #start the side grasp search at this height or at the center of the object, whichever is higher 00138 self.side_grasp_start_height = rospy.get_param("~side_grasp_start_height", .04) 00139 00140 #how far to move the gripper to the side with each step when searching for grasps 00141 self.side_step = rospy.get_param("~side_step", .02) 00142 00143 #how far to move the palm inward with each step when searching for grasps 00144 self.palm_step = rospy.get_param("~palm_step", .005) 00145 00146 #set this to true to limit the planner to overhead grasps 00147 self.overhead_grasps_only = rospy.get_param("~overhead_grasps_only", False) 00148 00149 #set this to true to limit the planner to side grasps 00150 self.side_grasps_only = rospy.get_param("~side_grasps_only", False) 00151 00152 #set this to false to omit random high-point grasps 00153 self.include_high_point_grasps = rospy.get_param("~include_high_point_grasps", True) 00154 00155 #set this to true to make the pregrasps be just outside the bounding box instead of self.pregrasp_dist away from the grasp 00156 self.pregrasp_just_outside_box = rospy.get_param("~pregrasp_just_outside_box", False) 00157 00158 #how many backing-off-from-the-deepest-possible-grasp grasps to keep for each good grasp found 00159 self.backoff_depth_steps = rospy.get_param("~backoff_depth_steps", 5) 00160 if self.backoff_depth_steps < 1: 00161 self.backoff_depth_steps = 1 00162 00163 #don't check the neighbors for each grasp (reduces grasps checked, but makes for worse rankings) 00164 self.disable_grasp_neighbor_check = rospy.get_param("~disable_grasp_neighbor_check", False) 00165 00166 #for outputting feature weights 00167 self._output_features = 0 00168 if self._output_features: 00169 self._outfile = file("feature_weights.txt", 'w') 00170 00171 #debug drawing modes 00172 self.debug = 0 00173 self.draw_gripper = 0 00174 00175 00176 ##pretty-print list to string 00177 def pplist(self, list): 00178 return ' '.join(['%5.3f'%x for x in list]) 00179 00180 00181 ##draw the box model of the gripper, placed at pose_mat (4x4 scipy matrix) in frame (defaults to the object frame) 00182 def draw_gripper_model(self, pose_mat, frame = 'object_frame', pause_after_broadcast = 0): 00183 00184 #broadcast the gripper frame to tf 00185 (pos, quat) = mat_to_pos_and_quat(pose_mat) 00186 (object_frame_pos, object_frame_quat) = mat_to_pos_and_quat(self.object_to_cluster_frame) 00187 if frame == 'object_frame': 00188 self.tf_broadcaster.sendTransform(object_frame_pos, object_frame_quat, rospy.Time.now(), "object_frame", self.cluster_frame) 00189 now = rospy.Time.now() 00190 self.tf_broadcaster.sendTransform(pos, quat, now, 'wrist_frame', frame) 00191 if pause_after_broadcast: 00192 time.sleep(.1) 00193 self.tf_listener.waitForTransform('wrist_frame', frame, now, rospy.Duration(2.0)) 00194 00195 box_id = 0 00196 for i in range(len(self.gripper_boxes)): 00197 color = [0,1,0] 00198 self.draw_functions.draw_rviz_box(scipy.matrix(scipy.identity(4)), ranges = self.gripper_boxes[i], frame = 'wrist_frame', ns = "gripper_model", \ 00199 id = i, color = color, duration = 30, opaque = .75) 00200 box_id += len(self.gripper_boxes) 00201 for i in range(len(self.space_boxes)): 00202 for j in range(len(self.space_boxes[i])): 00203 color = [0,.2*i,1] 00204 self.draw_functions.draw_rviz_box(scipy.matrix(scipy.identity(4)), ranges = self.space_boxes[i][j], frame = 'wrist_frame', ns = "gripper_model", \ 00205 id = box_id, color=color, duration = 30, opaque = .75) 00206 box_id += 1 00207 00208 #if self.debug: 00209 # self.keypause() 00210 00211 00212 ##return the xyz bounding box ranges for the parts of the gripper model placed at 3-list pos and 3x3 scipy matrix rot 00213 #uses a really simple model of the fully-open gripper as three boxes (2 fingers and a palm), with a box for the empty space in-between 00214 #gripper_boxes is a list of boxes, each of which is a 2-list (min, max) of 3-lists (x,y,z) of corner coords 00215 #space boxes is a list of lists of box ranges; there must be points contained in each box-list for the grasp to be valid (points are summed within lists) 00216 #origin of gripper is at the ?_wrist_roll_link 00217 def gripper_model(self): 00218 #palm_box = [[.04, -.0755, -.026], [.128, .0755, .026]] 00219 #left_finger_box = [[.128, .0405, -.0115], [.185, .0755, .0115]] 00220 #right_finger_box = [[.128, -.0755, -.0115], [.185, -.0405, .0115]] 00221 palm_box = [[.04, -.0755, -.026], [.138, .0755, .026]] 00222 left_finger_box = [[.138, .0425, -.0115], [.185, .0755, .0115]] 00223 right_finger_box = [[.138, -.0755, -.0115], [.185, -.0425, .0115]] 00224 #space_box = [[.128, -.0405, -.0115], [.185, .0405, .0115]] 00225 space_box = [[.138, -.0405, -.005], [.180, .0405, .005]] 00226 gripper_boxes = [palm_box, left_finger_box, right_finger_box] 00227 space_boxes = [[space_box,],] 00228 00229 return gripper_boxes, space_boxes 00230 00231 00232 ##return a count of points (4xn scipy mat) contained in a bounding box (ranges is a 2-list (min, max) of 3-lists (x,y,z)) 00233 def find_points_in_bounding_box(self, wrist_frame_points, ranges, return_points = 0): 00234 00235 #find the number of points that fit within the box ranges in the wrist frame 00236 gt = scipy.array(wrist_frame_points[0:3,:]).T > scipy.array(ranges[0]) 00237 lt = scipy.array(wrist_frame_points[0:3,:]).T < scipy.array(ranges[1]) 00238 fits_in_box_bool = scipy.logical_and(gt, lt) 00239 fits_in_box = scipy.all(fits_in_box_bool, axis=1) 00240 00241 if return_points: 00242 points_that_fit = wrist_frame_points[:, fits_in_box] 00243 return (fits_in_box.sum(), points_that_fit) 00244 00245 #print "points in bounding box:", fits_in_box.sum() 00246 00247 return fits_in_box.sum() 00248 00249 # fits_in_box = [pt for pt in wrist_frame_points.T if \ 00250 # pt[0,0] > ranges[0][0] and pt[0,0] < ranges[1][0] and \ 00251 # pt[0,1] > ranges[0][1] and pt[0,1] < ranges[1][1] and \ 00252 # pt[0,2] > ranges[0][2] and pt[0,2] < ranges[1][2]] 00253 # return len(fits_in_box) 00254 00255 00256 ##transform ranges for a bounding box using transform 00257 def transform_ranges(self, transform, ranges): 00258 corners = [scipy.matrix([ranges[xind][0], ranges[yind][1], ranges[zind][2], 1]).T for \ 00259 (xind, yind, zind) in scipy.ndindex(2,2,2)] 00260 corners = scipy.concatenate(corners, axis=1) 00261 transformed_corners = transform*corners 00262 transformed_corners_ranges = [transformed_corners.min(axis=1)[0:3].T.tolist()[0], transformed_corners.max(axis=1)[0:3].T.tolist()[0]] 00263 return transformed_corners_ranges 00264 00265 00266 ##check if a box goes through the x-y plane 00267 #ranges is [[xmin, ymin, zmin], [xmax, ymax, zmax]] 00268 #pose is a 4x4 scipy matrix 00269 def check_box_plane_collisions(self, pose, ranges): 00270 00271 #transform the box corners into the object frame (where the table is the x-y plane) and check that z is above the table 00272 transformed_corners = self.transform_ranges(pose, ranges) 00273 for transformed_corner in transformed_corners: 00274 if transformed_corner[2] < -0.005: 00275 # if self.debug: 00276 # self.draw_functions.draw_rviz_points(scipy.matrix(transformed_corner).T, frame = 'object_frame', size = .02, ns = "box_plane_collision_points", id=0, color = [1,1,0]) 00277 # print "transformed_corner:", transformed_corner 00278 # #self.keypause() 00279 return 1 00280 return 0 00281 00282 00283 ##check for collisions and points within the gripper when placed at a given pose (4x4 scipy mat) 00284 #returns -1 for colliding with points, -2 for colliding with the table, 00285 #and the number of points inside the gripper otherwise 00286 #table is assumed to be the xy plane 00287 def check_gripper_pose(self, points, pose): 00288 00289 if self.debug: 00290 #self.draw_functions.clear_rviz_points(ns = 'wrist_frame_points', id = 0) 00291 for i in range(3): 00292 self.draw_functions.clear_rviz_points(ns = 'box_point_collision_points', id = i) 00293 self.draw_functions.clear_rviz_points(ns = 'inside_gripper_points', id = 0) 00294 00295 #draw the gripper model we're currently checking in rviz and broadcast the current 'wrist_frame' transform 00296 if self.draw_gripper: 00297 self.draw_gripper_model(pose) 00298 00299 # #check if any of the boxes at that pose go through the table (assumed to be the xy plane) 00300 # for i in range(len(self.gripper_boxes)): 00301 # if self.check_box_plane_collisions(pose, self.gripper_boxes[i]): 00302 # if self.debug: 00303 # print "box-table collision" 00304 # #self.keypause() 00305 # return -2 00306 00307 #transform the points to the wrist frame 00308 world_to_wrist = pose**-1 00309 transformed_points = world_to_wrist * points 00310 #self.draw_functions.draw_rviz_points(transformed_points, frame = 'wrist_frame', size = .005, ns = 'wrist_frame_points', id = 0, color = [.5, .5, .5], opaque = 1) 00311 #self.keypause() 00312 00313 #figure out whether the boxes collide with any of the points 00314 num_collision_points = 0 00315 for i in range(len(self.gripper_boxes)): 00316 if self.debug: 00317 (num_points, points) = self.find_points_in_bounding_box(transformed_points, self.gripper_boxes[i], 1) 00318 else: 00319 num_points = self.find_points_in_bounding_box(transformed_points, self.gripper_boxes[i], 0) 00320 if num_points != 0: 00321 if self.debug: 00322 print "box-point collision" 00323 self.draw_functions.draw_rviz_points(points, frame = 'wrist_frame', size = .0075, ns = 'box_point_collision_points', id = i, \ 00324 duration = 20., color = [1,0,0], opaque = 1) 00325 #self.keypause() 00326 num_collision_points += num_points 00327 #return -1 00328 00329 #no collisions, count the number of points in each gripper space and return the min of all the gripper-space-box lists 00330 debug_points = None 00331 min_space_points = 1e6 00332 current_list_space_points = 0 00333 for i in range(len(self.space_boxes)): 00334 for j in range(len(self.space_boxes[i])): 00335 if self.debug: 00336 (num_space_points, points) = self.find_points_in_bounding_box(transformed_points, self.space_boxes[i][j], 1) 00337 if debug_points == None: 00338 debug_points = points 00339 else: 00340 debug_points = scipy.hstack((debug_points, points)) 00341 else: 00342 num_space_points = self.find_points_in_bounding_box(transformed_points, self.space_boxes[i][j], 0) 00343 current_list_space_points += num_space_points 00344 if current_list_space_points < min_space_points: 00345 min_space_points = current_list_space_points 00346 if min_space_points and self.debug: 00347 print "min_space_points:", min_space_points 00348 if debug_points != None: 00349 self.draw_functions.draw_rviz_points(debug_points, frame = 'wrist_frame', size = .0075, ns = 'inside_gripper_points', id = 0., \ 00350 duration = 20., color = [0,0,1], opaque = 1) 00351 if self.debug: 00352 self.keypause() 00353 00354 return (min_space_points, num_collision_points) 00355 00356 00357 ##given a cluster and a grasp pose (4x4 scipy mat in object frame), guess at the probability of success for the grasp 00358 def evaluate_arbitrary_grasp(self, points, pose): 00359 00360 #print ppmat(pose) 00361 00362 #how close to orthogonal is the pose? 00363 orthogonality = self.orthogonal_measure(pose) 00364 00365 #find the distances from the object box center to the fingertip center when projected on the gripper x-, y-, and z-axes 00366 (x_dist, y_dist, z_dist) = self.find_fingertip_object_center_dists(pose) 00367 00368 #how close to overhead is the pose? Turn the angle into a value from 0 (sideways or worse) to 1 (exactly overhead). 00369 overhead_angle = self.overhead_angle(pose) 00370 overhead_grasp = max(0, 1 - overhead_angle / (math.pi/2)) 00371 00372 #figure out whether all the points fit within the gripper (all points no more than gripper width/2 away from gripper x-z plane) 00373 fits_in_hand = self.object_fits_in_hand(points, pose) 00374 00375 #check the grasp's neighbors to see if it's at the edge of the object 00376 not_edge = self.check_grasp_neighbors(points, pose) 00377 00378 #how many points in the gripper space box? (-1 if gripper is colliding) 00379 (point_count, collision_points) = self.check_gripper_pose(points, pose) 00380 00381 #don't like grasps that don't have at least some number of points in the gripper 00382 if point_count < self.min_good_grasp_points: 00383 #rospy.loginfo("in collision or not enough points") 00384 #raw_input() 00385 #print "not enough points, returning 0" 00386 return 0 00387 00388 #get the estimated probability of success 00389 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) 00390 00391 return probability 00392 00393 00394 ##turn a quality value into a 'probability' value (fairly arbitrary, not based on real data) 00395 def quality_to_probability(self, quality): 00396 00397 #sigmoid around center 00398 center = 50 00399 scale = 150. / 3.6 #150 + center -> ~ 95% 00400 t = (quality - center) / scale 00401 if t < -50: #avoiding math range error 00402 return 0 00403 prob = 1./(1.+math.exp(-t)) 00404 return prob 00405 00406 00407 ##how far is the pose from being orthogonal to the object frame? 00408 #returns a measure of angle of the closest rotation that would make the pose orthogonal to the object frame 00409 def orthogonal_measure(self, pose): 00410 00411 #align the gripper x-axis and then y-axis with the nearest object frame axis 00412 aligned_pose = pose.copy() 00413 for axis in (0, 1): 00414 axis_vect = scipy.array(aligned_pose[0:3, axis].T)[0] 00415 closest_axis = scipy.fabs(axis_vect).argmax() 00416 axis_dir = axis_vect[closest_axis] / abs(axis_vect[closest_axis]) 00417 closest_axis_vect = scipy.zeros(3) 00418 closest_axis_vect[closest_axis] = axis_dir 00419 dot = min(1, max(0, scipy.dot(closest_axis_vect, axis_vect))) 00420 angle = math.acos(dot) 00421 if math.fabs(angle) > 1e-6: 00422 rot_vect = scipy.cross(axis_vect, closest_axis_vect) 00423 align_rot = scipy.matrix(tf.transformations.rotation_matrix(angle, rot_vect)) 00424 aligned_pose = align_rot * aligned_pose 00425 00426 #find the angle difference between the newly-aligned rotation and the original pose 00427 rel_mat = aligned_pose**-1 * pose 00428 rel_quat = tf.transformations.quaternion_from_matrix(rel_mat) 00429 if rel_quat[3] > 1.0: 00430 angle = 0 00431 elif rel_quat[3] < -1.0: 00432 angle = math.pi 00433 else: 00434 angle = 2*math.acos(rel_quat[3]) 00435 00436 #return 1 if the pose is exactly orthogonal (angle 0), 0 if the pose is more than pi/6 away 00437 orthogonality = max(0, 1 - angle / (math.pi/6)) 00438 return orthogonality 00439 00440 00441 ##find the distances from the object box center to the fingertip center when projected on the gripper x-, y-, and z-axes 00442 def find_fingertip_object_center_dists(self, pose): 00443 00444 #object bounding box center in gripper frame 00445 object_center = scipy.matrix([0, 0, self.object_bounding_box_dims[2]/2, 1]).T 00446 gripper_frame_center = pose**-1 * object_center 00447 00448 #dist to fingertip center (fingertips past center is even better than at center) 00449 x_dist = gripper_frame_center[0,0] - self._wrist_to_fingertip_center_dist 00450 00451 #dist to either side of 0 is bad 00452 y_dist = math.fabs(gripper_frame_center[1,0]) 00453 z_dist = math.fabs(gripper_frame_center[2,0]) 00454 00455 return (x_dist, y_dist, z_dist) 00456 00457 00458 ##figure out whether all the points fit within the gripper for an arbitrary pose (no more than gripper width/2 away from gripper x-z plane) 00459 def object_fits_in_hand(self, points, pose): 00460 00461 #transform the points to the wrist frame 00462 transformed_points = pose**-1 * points 00463 00464 #check if any of the points' y-coords have a magnitude greater than the gripper width/2 00465 return not (scipy.fabs(transformed_points[1, :]) > self.gripper_opening).any() 00466 00467 00468 ##angle of gripper pose x-axis away from directly overhead (global -z) 00469 def overhead_angle(self, pose): 00470 00471 z_comp_of_x_axis = max(0, min(1, -pose[2,0])) 00472 angle = math.acos(z_comp_of_x_axis) 00473 return angle 00474 00475 00476 ##check both sides of a grasp to see if they are also viable grasps 00477 def check_grasp_neighbors(self, points, grasp): 00478 00479 # print "grasp" 00480 # self.draw_gripper_model(grasp, pause_after_broadcast = 1) 00481 # self.draw_gripper_model(grasp, pause_after_broadcast = 1) 00482 # self.keypause() 00483 00484 neighbor_dist = .02 00485 for sign in [-1., 1.]: 00486 00487 #shift the grasp by neighbor_dist in z 00488 shift_mat = scipy.matrix(tf.transformations.translation_matrix([0,0,sign*neighbor_dist])) 00489 shifted_grasp = grasp * shift_mat 00490 00491 # print "shifted grasp" 00492 # self.draw_gripper_model(shifted_grasp, pause_after_broadcast = 1) 00493 # self.draw_gripper_model(shifted_grasp, pause_after_broadcast = 1) 00494 00495 #test to see if it's any good 00496 (point_count, collision_points) = self.check_gripper_pose(points, shifted_grasp) 00497 if point_count < self.min_good_grasp_points or collision_points > 0: 00498 #print "neighbor was no good" 00499 #self.keypause() 00500 return 0 00501 #print "neighbor good" 00502 00503 #both sides were good 00504 #print "both neighbors were good" 00505 return 1 00506 00507 00508 ##assign a numerical quality value for this grasp 00509 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): 00510 00511 # if grasp != None: 00512 # print "running evaluate_arbitrary_grasp:" 00513 # self.evaluate_arbitrary_grasp(points, grasp) 00514 # print "original grasp quality params:" 00515 00516 #rospy.loginfo("pc: %d, x_d: %0.3f, z_d: %0.3f, og: %0.3f, ih: %d, ne: %d, orth: %0.3f, y_d: %0.3f\n"%(point_count, palm_dist, side_dist, overhead_grasp, fits_in_hand, not_edge, orthogonality, centered_dist)) 00517 00518 feature_vector = [point_count, palm_dist, side_dist, overhead_grasp, fits_in_hand, not_edge, orthogonality, centered_dist, collision_point_count] 00519 if self._output_features: 00520 self._outfile.write(str(feature_vector)+'\n') 00521 00522 #adjust the point_count weight depending on the density of points 00523 point_count_weight = 5./self._points_per_sq_cm 00524 00525 #hand chosen weights for now (collisions set to be unacceptable) 00526 weights = [point_count_weight, -10./0.01, -10./0.01, 10., 50., 50., 50., -10./0.01, -1000] 00527 quality = sum([w*f for (w,f) in zip(weights, feature_vector)]) 00528 00529 prob = self.quality_to_probability(quality) 00530 00531 #rospy.loginfo("quality: %0.3f, prob: %0.3f\n"%(quality, prob)) 00532 00533 return prob 00534 00535 00536 ##find the best grasp from a given start pose along the palm (gripper x) direction, in steps of size palm_step 00537 #moving the gripper inward until/as long as there are object points within the gripper, until there are collisions 00538 #checking for collisions with the table (assumed to be the xy plane) 00539 #returns the grasp pose if a good grasp is found, None if not 00540 def find_best_palm_dir_grasp(self, points, start_pose, palm_step, object_bounding_box): 00541 00542 #find a reasonable max distance to move, in case we never find collisions 00543 #using half the max bounding box dim + 6 cm 00544 max_dist = max([(upper-lower)/2 for (upper, lower) in zip(object_bounding_box[1], object_bounding_box[0])]) + .06 00545 00546 good_grasps = [] 00547 good_grasp_point_counts = [] 00548 good_grasp_dists_moved = [] 00549 new_pose = start_pose.copy() 00550 dist_moved = 0. 00551 while(1): 00552 #print "dist_moved:", dist_moved 00553 00554 #check to see if the gripper pose is good (stop if collisions are found) 00555 (point_count, collision_points) = self.check_gripper_pose(points, new_pose) 00556 # if self.debug: 00557 # time.sleep(.02) 00558 # self.keypause() 00559 if point_count < 0 or collision_points > 0: 00560 break 00561 00562 #add the grasp to the list if it is acceptable 00563 if point_count > self.min_good_grasp_points: 00564 good_grasps.append(new_pose.copy()) 00565 good_grasp_point_counts.append(point_count) 00566 good_grasp_dists_moved.append(dist_moved) 00567 00568 #direction to move is gripper's x-axis 00569 new_pose[0:3, 3] += start_pose[0:3, 0] * palm_step 00570 dist_moved += palm_step 00571 00572 #moved too far already 00573 if dist_moved > max_dist: 00574 break 00575 00576 return (good_grasps[-self.backoff_depth_steps:], good_grasp_point_counts[-self.backoff_depth_steps:], good_grasp_dists_moved[-self.backoff_depth_steps:]) 00577 00578 00579 ##for a given gripper pose and axis (0=x, 1=y, 2=z), both expressed in the points frame, 00580 #find grasps at a fixed spacing along that axis in both directions 00581 #while varying the palm dist (inward by steps of palm_step) as long as there are object points within the gripper, until there are collisions 00582 #start_pose is a 4x4 scipy mat, object_bounding_box is [[xmin, ymin, zmin], [xmax, ymax, zmax]] 00583 #min_points is the minimum number of points that need to be inside the hand for the grasp to be okay 00584 def find_grasps_along_direction(self, points, start_pose, axis, spacing, palm_step, object_bounding_box, max_side_move = None, omit_center = False): 00585 00586 #how far to keep away from the edge of the bounding box 00587 min_edge_dist = .02 00588 00589 dist = 0. 00590 positive_done = 0 00591 negative_done = 0 00592 new_pose = start_pose.copy() 00593 good_grasps = [] 00594 00595 while(not positive_done or not negative_done): 00596 if self.debug: 00597 print "dist:", dist 00598 00599 #check positive dist along direction 00600 new_pose[axis, 3] = dist + start_pose[axis, 3] 00601 #print "new_pose:\n", ppmat(new_pose) 00602 00603 if not positive_done and not (dist == 0 and omit_center): 00604 (grasps, point_counts, palm_dists_moved) = self.find_best_palm_dir_grasp(points, new_pose, palm_step, object_bounding_box) 00605 for (grasp, point_count, palm_dist_moved) in zip(grasps, point_counts, palm_dists_moved): 00606 good_grasps.append([grasp, point_count, palm_dist_moved, dist]) 00607 00608 #only check the center once 00609 if dist != 0.: 00610 #check negative dist along direction 00611 if not negative_done: 00612 new_pose[axis, 3] = -dist + start_pose[axis, 3] 00613 (grasps, point_counts, palm_dists_moved) = self.find_best_palm_dir_grasp(points, new_pose, palm_step, object_bounding_box) 00614 for (grasp, point_count, palm_dist_moved) in zip(grasps, point_counts, palm_dists_moved): 00615 good_grasps.append([grasp, point_count, palm_dist_moved, dist]) 00616 00617 #increment dist and check to see if we've gone off either end of the cluster (keeping away from the ends) 00618 dist += spacing 00619 if max_side_move != None and dist > max_side_move: 00620 break 00621 if -dist + start_pose[axis, 3] < object_bounding_box[0][axis] + min_edge_dist: 00622 negative_done = 1 00623 if dist + start_pose[axis, 3] > object_bounding_box[1][axis] - min_edge_dist: 00624 positive_done = 1 00625 00626 return good_grasps 00627 00628 00629 ##find grasps of high points on the object, pointing in towards the bottom center of the object 00630 def find_high_point_grasps(self, points, object_bounding_box, object_bounding_box_dims, palm_step, max_grasps_to_return = 40): 00631 print "starting high_point_grasps" 00632 00633 max_grasps_to_find = int(math.floor(max_grasps_to_return * 1.5)) 00634 00635 #points should already be z-sorted after removing outliers, so we just need to use the points at the end 00636 num_points = scipy.shape(points)[1] 00637 num_selected_points = max_grasps_to_find*2 00638 max_dist_from_top = .02 00639 step_size = 50 00640 00641 highest_z = points[2,-1] 00642 ind_of_lowest_pt = scipy.searchsorted(points[2, :].tolist()[0], highest_z - max_dist_from_top) 00643 print "ind_of_lowest_pt:", ind_of_lowest_pt 00644 00645 #use the highest point, and randomly select some more points to try 00646 selected_point_inds = scipy.random.randint(ind_of_lowest_pt, num_points-2, (1,num_selected_points)) 00647 selected_point_inds[0,0] = num_points-1 00648 selected_points = points[:, selected_point_inds.tolist()[0]] 00649 #if self.debug: 00650 #self.draw_functions.draw_rviz_points(selected_points, frame = 'object_frame', size = .01, ns = 'selected_points', id = 0, duration = 20., color = [1,1,0], opaque = 1.0) 00651 00652 #check if we can grasp each selected point from above, with the gripper y-axis pointing towards the object z-axis 00653 grasps_with_features = [] 00654 for point_ind in range(num_selected_points): 00655 point = selected_points[:, point_ind] 00656 00657 #start the wrist out so that the fingertips are at the edge of the bounding box 00658 wrist_z_pos = object_bounding_box_dims[2] + self._wrist_to_fingertip_center_dist 00659 00660 #find overhead grasps (gripper x is down (-z), y is toward the z-axis, z is x cross y) 00661 start_pose = scipy.matrix(scipy.identity(4)) 00662 point_xy_mag = math.sqrt(point[0:2,0].T*point[0:2,0]) 00663 x_axis = scipy.matrix([0.,0.,-1.]).T 00664 y_axis = scipy.matrix([point[0,0]/point_xy_mag, point[1,0]/point_xy_mag, 0]).T 00665 z_axis = scipy.matrix(scipy.cross(x_axis.T, y_axis.T).T) 00666 start_pose[0:3, 0] = x_axis 00667 start_pose[0:3, 1] = y_axis 00668 start_pose[0:3, 2] = z_axis 00669 start_pose[0, 3] = point[0,0] 00670 start_pose[1, 3] = point[1,0] 00671 start_pose[2, 3] = wrist_z_pos 00672 00673 #search along the palm direction for the best grasp 00674 (grasps, point_counts, palm_dists_moved) = self.find_best_palm_dir_grasp(points, start_pose, palm_step, object_bounding_box) 00675 #rospy.loginfo("len(grasps):%d"%len(grasps)) 00676 for (grasp, point_count, palm_dist_moved) in zip(grasps, point_counts, palm_dists_moved): 00677 grasps_with_features.append((grasp, point_count, palm_dist_moved)) 00678 00679 #stop once we have max_grasps_to_find grasps 00680 if len(grasps_with_features) > max_grasps_to_find: 00681 break 00682 00683 #compute the qualities for the found grasps 00684 grasps_with_qualities = [] 00685 for (grasp, point_count, palm_dist_moved) in grasps_with_features: 00686 00687 #figure out whether the grasp is still good when shifted to both sides in z 00688 not_edge = 1 00689 if not self.disable_grasp_neighbor_check: 00690 not_edge = self.check_grasp_neighbors(points, grasp) 00691 00692 #compute the quality 00693 orthogonality = self.orthogonal_measure(grasp) 00694 00695 #find the distances from the object box center to the fingertip center when projected on the gripper x-, y-, and z-axes 00696 (x_dist, y_dist, z_dist) = self.find_fingertip_object_center_dists(grasp) 00697 00698 #figure out whether all the points fit within the gripper (all points no more than gripper width/2 away from gripper x-z plane) 00699 fits_in_hand = self.object_fits_in_hand(points, grasp) 00700 00701 quality = self.grasp_quality(point_count, x_dist, y_dist, overhead_grasp = 1, fits_in_hand = fits_in_hand, not_edge = not_edge, 00702 orthogonality = orthogonality, centered_dist = z_dist, points = points, grasp = grasp) 00703 grasps_with_qualities.append([grasp, quality, palm_dist_moved]) 00704 00705 00706 #sort them by quality and return only the best ones 00707 grasps_with_qualities = sorted(grasps_with_qualities, key=lambda t:t[1], reverse=True) 00708 00709 # #clip off all those with quality below 0.7 00710 # for (ind, grasp_with_quality) in enumerate(grasps_with_qualities): 00711 # if ind == 0: 00712 # continue 00713 # if grasp_with_quality[1] < 0.7: 00714 # rospy.loginfo("tossing out %d grasps with quality below 0.7"%(len(grasps_with_qualities)-ind)) 00715 # grasps_with_qualities = grasps_with_qualities[:ind] 00716 # break 00717 00718 rospy.loginfo("high point grasp qualities: " + self.pplist([x[1] for x in grasps_with_qualities])) 00719 00720 return grasps_with_qualities[:max_grasps_to_return] 00721 00722 00723 ##pause for enter 00724 def keypause(self): 00725 print "press enter to continue" 00726 c = raw_input() 00727 return c 00728 00729 00730 ##initialization for planning grasps 00731 def init_cluster_grasper(self, cluster): 00732 00733 self.cluster_frame = cluster.header.frame_id 00734 00735 #use PCA to find the object frame and bounding box dims, and to get the cluster points in the object frame (z=0 at bottom of cluster) 00736 (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \ 00737 self.object_to_base_frame, self.object_to_cluster_frame) = \ 00738 self.cbbf.find_object_frame_and_bounding_box(cluster) 00739 00740 #for which directions does the bounding box fit within the hand? 00741 gripper_space = [self.gripper_opening - self.object_bounding_box_dims[i] for i in range(3)] 00742 self._box_fits_in_hand = [gripper_space[i] > 0 for i in range(3)] 00743 00744 #only half benefit for the longer dimension, if one is significantly longer than the other 00745 if self._box_fits_in_hand[0] and self._box_fits_in_hand[1]: 00746 if gripper_space[0] > gripper_space[1] and self.object_bounding_box_dims[0]/self.object_bounding_box_dims[1] < .8: 00747 self._box_fits_in_hand[1] *= .5 00748 elif gripper_space[1] > gripper_space[0] and self.object_bounding_box_dims[1]/self.object_bounding_box_dims[0] < .8: 00749 self._box_fits_in_hand[0] *= .5 00750 #rospy.loginfo("bounding box dims: "+pplist(self.object_bounding_box_dims)) 00751 #rospy.loginfo("self._box_fits_in_hand: "+str(self._box_fits_in_hand)) 00752 00753 #compute the average number of points per sq cm of bounding box surface (half the surface only) 00754 bounding_box_surf = (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[1]*100) + \ 00755 (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[2]*100) + \ 00756 (self.object_bounding_box_dims[1]*100 * self.object_bounding_box_dims[2]*100) 00757 self._points_per_sq_cm = self.object_points.shape[1] / bounding_box_surf 00758 #rospy.loginfo("bounding_box_surf: %.2f, number of points: %d"%(bounding_box_surf, self.object_points.shape[1])) 00759 #rospy.loginfo("points per sq cm: %.4f"%self._points_per_sq_cm) 00760 00761 00762 ##evaluate grasps on the point cluster 00763 def evaluate_point_cluster_grasps(self, grasps, frame): 00764 00765 #convert the grasps to 4x4 scipy matrix wrist poses in object frame 00766 grasp_poses = [] 00767 cluster_to_object_frame = self.object_to_cluster_frame**-1 00768 for grasp in grasps: 00769 pose_mat = pose_to_mat(grasp.grasp_pose) 00770 00771 #transform them to gripper model poses in object frame 00772 obj_frame_mat = cluster_to_object_frame * pose_mat * self.model_frame_in_actual_wrist_frame 00773 grasp_poses.append(obj_frame_mat) 00774 00775 #evaluate the grasp qualities 00776 probs = [self.evaluate_arbitrary_grasp(self.object_points, pose) for pose in grasp_poses] 00777 00778 return probs 00779 00780 00781 ##plan grasps for a point cluster 00782 def plan_point_cluster_grasps(self): 00783 00784 if self.debug: 00785 print "about to find grasps" 00786 self.keypause() 00787 00788 #search for grasps from all relevant sides 00789 found_grasps = [] 00790 found_grasps1 = [] 00791 found_grasps2 = [] 00792 num_overhead_grasps = 0 00793 num_overhead_grasps1 = 0 00794 00795 #start the wrist out so that the palm is at the edge of the bounding box 00796 top_wrist_z_pos_palm = self.object_bounding_box[1][2] + self._wrist_to_palm_dist 00797 top_y_start_pose_palm = scipy.matrix([[0.,0.,-1.,0.], 00798 [0.,-1.,0.,0.], 00799 [-1.,0.,0.,top_wrist_z_pos_palm], 00800 [0.,0.,0.,1.]]) 00801 top_x_start_pose_palm = scipy.matrix([[0.,-1.,0.,0.], 00802 [0.,0.,1.,0.], 00803 [-1.,0.,0.,top_wrist_z_pos_palm], 00804 [0.,0.,0.,1.]]) 00805 00806 y_center_grasps_found = 0 00807 x_center_grasps_found = 0 00808 if not self.side_grasps_only: 00809 #if bounding box fits in hand, check overhead grasp with gripper along y 00810 if self._box_fits_in_hand[1] > 0: 00811 grasp_list = self.find_grasps_along_direction(self.object_points, top_y_start_pose_palm, 0, \ 00812 self.side_step, self.palm_step, self.object_bounding_box, max_side_move = 0.) 00813 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list): 00814 quality = self.grasp_quality(point_count, \ 00815 palm_dist = top_wrist_z_pos_palm-self._wrist_to_palm_dist-palm_dist_moved-self.object_bounding_box_dims[2]/2., \ 00816 side_dist = dist, overhead_grasp = 1, fits_in_hand = self._box_fits_in_hand[1], \ 00817 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp) 00818 found_grasps.append([grasp, quality, palm_dist_moved]) 00819 y_center_grasps_found = 1 00820 00821 #if bounding box fits in hand, check overhead grasp with gripper along x 00822 if self._box_fits_in_hand[0] > 0: 00823 grasp_list = self.find_grasps_along_direction(self.object_points, top_x_start_pose_palm, 1, \ 00824 self.side_step, self.palm_step, self.object_bounding_box, max_side_move = 0.) 00825 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list): 00826 quality = self.grasp_quality(point_count, \ 00827 palm_dist = top_wrist_z_pos_palm-self._wrist_to_palm_dist-palm_dist_moved-self.object_bounding_box_dims[2]/2., \ 00828 side_dist = dist, overhead_grasp = 1, fits_in_hand = self._box_fits_in_hand[0], \ 00829 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp) 00830 found_grasps1.append([grasp, quality, palm_dist_moved]) 00831 x_center_grasps_found = 1 00832 rospy.loginfo("overhead grasp (x) qualities: " + self.pplist([x[1] for x in found_grasps])) 00833 rospy.loginfo("overhead grasp (y) qualities: " + self.pplist([x[1] for x in found_grasps1])) 00834 num_overhead_grasps = len(found_grasps) 00835 num_overhead_grasps1 = len(found_grasps1) 00836 00837 if not self.overhead_grasps_only: 00838 #for side grasps, start the wrist either halfway up the side of the object bounding box or self.side_grasp_start_height up, whichever is higher 00839 #with the palm at the edge of the bounding box 00840 side_wrist_z_pos = max(self.object_bounding_box[1][2]/2., self.side_grasp_start_height) 00841 side_wrist_y_pos = self.object_bounding_box[1][1] + self._wrist_to_palm_dist 00842 side_wrist_x_pos = self.object_bounding_box[1][0] + self._wrist_to_palm_dist 00843 00844 #find side grasps for dimensions for which the bounding box fits within the hand 00845 if self._box_fits_in_hand[1] > 0 and self.object_bounding_box_dims[2] > self.height_good_for_side_grasps: 00846 00847 #find side grasps from the right (gripper x is -x, y is -y, z is +z in object frame) 00848 start_pose = scipy.matrix([[-1.,0.,0.,side_wrist_x_pos], 00849 [0.,-1.,0.,0], 00850 [0.,0.,1.,side_wrist_z_pos], 00851 [0.,0.,0.,1.]]) 00852 grasp_list = self.find_grasps_along_direction(self.object_points, start_pose, 2, \ 00853 self.side_step, self.palm_step, self.object_bounding_box) 00854 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list): 00855 quality = self.grasp_quality(point_count, \ 00856 palm_dist = side_wrist_x_pos-self._wrist_to_palm_dist-palm_dist_moved, \ 00857 side_dist = dist, overhead_grasp = 0, fits_in_hand = self._box_fits_in_hand[1], \ 00858 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp) 00859 found_grasps.append([grasp, quality, palm_dist_moved]) 00860 00861 #find side grasps from the left (gripper x is +x, y is -y, z is -z in object frame) 00862 start_pose = scipy.matrix([[1.,0.,0.,-side_wrist_x_pos], 00863 [0.,-1.,0.,0], 00864 [0.,0.,-1.,side_wrist_z_pos], 00865 [0.,0.,0.,1.]]) 00866 grasp_list = self.find_grasps_along_direction(self.object_points, start_pose, 2, self.side_step, \ 00867 self.palm_step, self.object_bounding_box) 00868 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list): 00869 quality = self.grasp_quality(point_count, palm_dist = side_wrist_x_pos-self._wrist_to_palm_dist-palm_dist_moved, \ 00870 side_dist = dist, overhead_grasp = 0, fits_in_hand = self._box_fits_in_hand[1], \ 00871 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp) 00872 found_grasps.append([grasp, quality, palm_dist_moved]) 00873 00874 if self._box_fits_in_hand[0] > 0 and self.object_bounding_box_dims[2] > self.height_good_for_side_grasps: 00875 00876 #find side grasps from the front (gripper x is +y, y is -x, z is +z in object frame) 00877 start_pose = scipy.matrix([[0.,-1.,0.,0.], 00878 [1.,0.,0.,-side_wrist_y_pos], 00879 [0.,0.,1.,side_wrist_z_pos], 00880 [0.,0.,0.,1.]]) 00881 grasp_list = self.find_grasps_along_direction(self.object_points, start_pose, 2, self.side_step, self.palm_step, self.object_bounding_box) 00882 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list): 00883 quality = self.grasp_quality(point_count, palm_dist = side_wrist_y_pos-self._wrist_to_palm_dist-palm_dist_moved, \ 00884 side_dist = dist, overhead_grasp = 0, fits_in_hand = self._box_fits_in_hand[0], \ 00885 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp) 00886 found_grasps1.append([grasp, quality, palm_dist_moved]) 00887 00888 #find side grasps from the back (gripper x is -y, y is +x, z is +z in object frame) 00889 start_pose = scipy.matrix([[0.,1.,0.,0.], 00890 [-1.,0.,0.,side_wrist_y_pos], 00891 [0.,0.,1.,side_wrist_z_pos], 00892 [0.,0.,0.,1.]]) 00893 grasp_list = self.find_grasps_along_direction(self.object_points, start_pose, 2, self.side_step, self.palm_step, self.object_bounding_box) 00894 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list): 00895 quality = self.grasp_quality(point_count, palm_dist = side_wrist_y_pos-self._wrist_to_palm_dist-palm_dist_moved, \ 00896 side_dist = dist, overhead_grasp = 0, fits_in_hand = self._box_fits_in_hand[0], \ 00897 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp) 00898 found_grasps1.append([grasp, quality, palm_dist_moved]) 00899 rospy.loginfo("side grasp (x) qualities: " + self.pplist([x[1] for x in found_grasps[num_overhead_grasps:]])) 00900 rospy.loginfo("side grasp (y) qualities: " + self.pplist([x[1] for x in found_grasps1[num_overhead_grasps1:]])) 00901 00902 if not self.side_grasps_only: 00903 #look for overhead grasps along the axes (not just at the center of the box), starting with fingertips at the edge of the bounding box 00904 #also find overhead grasps of not-along-axis high points with the gripper oriented with one finger toward the center 00905 #if found_grasps == []: 00906 #look for overhead grasps along y 00907 top_wrist_z_pos_fingertip = self.object_bounding_box[1][2] + self._wrist_to_fingertip_center_dist 00908 top_y_start_pose_fingertip = scipy.matrix([[0.,0.,-1.,0.], 00909 [0.,-1.,0.,0.], 00910 [-1.,0.,0.,top_wrist_z_pos_fingertip], 00911 [0.,0.,0.,1.]]) 00912 top_x_start_pose_fingertip = scipy.matrix([[0.,-1.,0.,0.], 00913 [0.,0.,1.,0.], 00914 [-1.,0.,0.,top_wrist_z_pos_fingertip], 00915 [0.,0.,0.,1.]]) 00916 00917 00918 grasp_list = self.find_grasps_along_direction(self.object_points, top_y_start_pose_fingertip, 0, self.side_step, self.palm_step, 00919 self.object_bounding_box, omit_center = y_center_grasps_found) 00920 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list): 00921 quality = self.grasp_quality(point_count, \ 00922 palm_dist = top_wrist_z_pos_fingertip-self._wrist_to_fingertip_center_dist-palm_dist_moved-self.object_bounding_box_dims[2]/2., \ 00923 side_dist = dist, overhead_grasp = 1, fits_in_hand = self.object_bounding_box_dims[1] < self.gripper_opening, \ 00924 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp) 00925 found_grasps2.append([grasp, quality, palm_dist_moved]) 00926 00927 #look for overhead grasps along x 00928 grasp_list = self.find_grasps_along_direction(self.object_points, top_x_start_pose_fingertip, 1, self.side_step, self.palm_step, 00929 self.object_bounding_box, omit_center = x_center_grasps_found) 00930 for (ind, (grasp, point_count, palm_dist_moved, dist)) in enumerate(grasp_list): 00931 quality = self.grasp_quality(point_count, \ 00932 palm_dist = top_wrist_z_pos_fingertip-self._wrist_to_fingertip_center_dist-palm_dist_moved-self.object_bounding_box_dims[2]/2., \ 00933 side_dist = dist, overhead_grasp = 1, fits_in_hand = self.object_bounding_box_dims[0] < self.gripper_opening, \ 00934 not_edge = ind < len(grasp_list)-2, points = self.object_points, grasp = grasp) 00935 found_grasps2.append([grasp, quality, palm_dist_moved]) 00936 rospy.loginfo("along-axes overhead grasp qualities: " + self.pplist([x[1] for x in found_grasps2])) 00937 num_overhead_grasps = len(found_grasps) 00938 00939 #look for grasps of high points 00940 if self.include_high_point_grasps: 00941 grasps_with_qualities = self.find_high_point_grasps(self.object_points, self.object_bounding_box, self.object_bounding_box_dims, self.palm_step) 00942 found_grasps2.extend(grasps_with_qualities) 00943 00944 #sort the grasps by quality (highest quality first), but keep all the centered grasps before the more marginal grasps 00945 found_grasps = sorted(found_grasps, key=lambda t:t[1], reverse=True) 00946 found_grasps1 = sorted(found_grasps1, key=lambda t:t[1], reverse=True) 00947 found_grasps2 = sorted(found_grasps2, key=lambda t:t[1], reverse=True) 00948 00949 #priortize grasps around the short side first, if there's a significant difference and they both fit in hand 00950 if self._box_fits_in_hand[0] < self._box_fits_in_hand[1]: 00951 found_grasps = found_grasps + found_grasps1 + found_grasps2 00952 elif self._box_fits_in_hand[1] < self._box_fits_in_hand[0]: 00953 found_grasps = found_grasps1 + found_grasps + found_grasps2 00954 else: 00955 found_grasps = sorted(found_grasps + found_grasps1, key=lambda t:t[1], reverse=True) + found_grasps2 00956 00957 rospy.loginfo("total number of grasps found:" + str(len(found_grasps))) 00958 00959 #generate the pregrasp poses and turn both grasps and pregrasps into Pose messages 00960 grasp_poses = [] 00961 pregrasp_poses = [] 00962 00963 pregrasp_dists = [] 00964 qualities = [] 00965 00966 #draw the gripper model just at the best grasp 00967 if found_grasps and self.draw_gripper: 00968 (best_grasp_mat, best_grasp_quality, palm_dist_moved) = found_grasps[0] 00969 self.draw_gripper_model(best_grasp_mat, pause_after_broadcast = 0) 00970 00971 #generate the pregrasps and pose messages 00972 for (grasp_mat, quality, palm_dist_moved) in found_grasps: 00973 00974 #pregrasp is just the grasp pose, backed up by self.pregrasp_dist (m) 00975 #or palm_dist_moved, if we want the pregrasp to be just outside the bounding box 00976 back_up_mat = scipy.identity(4) 00977 if not self.pregrasp_just_outside_box: 00978 back_up_mat[0,3] = -self.pregrasp_dist 00979 pregrasp_dists.append(self.pregrasp_dist) 00980 else: 00981 back_up_mat[0,3] = -palm_dist_moved-.005 00982 pregrasp_dists.append(palm_dist_moved+.005) 00983 pregrasp_mat = grasp_mat * back_up_mat 00984 00985 #transform the grasp_mat so that it reflects the actual desired wrist frame 00986 grasp_mat = grasp_mat * self.actual_wrist_frame_in_model_frame 00987 pregrasp_mat = pregrasp_mat * self.actual_wrist_frame_in_model_frame 00988 00989 #convert to Pose messages in the same frame as the original cluster 00990 pregrasp_pose = mat_to_pose(pregrasp_mat, self.object_to_cluster_frame) 00991 grasp_pose = mat_to_pose(grasp_mat, self.object_to_cluster_frame) 00992 00993 pregrasp_poses.append(pregrasp_pose) 00994 grasp_poses.append(grasp_pose) 00995 qualities.append(quality) 00996 00997 #for now, gripper is always open all the way at the pregrasp pose 00998 gripper_openings = [.1]*len(grasp_poses) 00999 01000 return (pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists) 01001 01002 01003