$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2011, 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 segmented_clutter_grasp_planner 00037 # Finds clusters in clutter, then runs the cluster planner to plan grasps on regions of interest in PointCloud2s 00038 00039 from __future__ import division 00040 import roslib 00041 roslib.load_manifest('segmented_clutter_grasp_planner') 00042 import rospy 00043 import actionlib 00044 import tf 00045 import time 00046 00047 from sensor_msgs.msg import CameraInfo, JointState, RegionOfInterest, PointCloud2 00048 from object_manipulation_msgs.msg import Grasp 00049 from object_manipulator.convert_functions import * 00050 from object_manipulator.image_region_functions import * 00051 from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningResponse, GraspPlanningRequest 00052 from object_manipulation_msgs.msg import GraspPlanningAction, GraspPlanningResult, GraspPlanningErrorCode 00053 from pr2_gripper_grasp_planner_cluster.srv import SetPointClusterGraspParams, SetPointClusterGraspParamsRequest 00054 from object_manipulation_msgs.msg import FindContainerAction, FindContainerGoal 00055 from arm_navigation_msgs.srv import SetPlanningSceneDiff, SetPlanningSceneDiffRequest 00056 from interpolated_ik_motion_planner import ik_utilities 00057 00058 class ClusteredClutterGraspPlanner(object): 00059 00060 def __init__(self, tf_listener = None, advertise_service = False, advertise_action = True): 00061 00062 #init a TF transform listener 00063 if tf_listener == None: 00064 self.tf_listener = tf.TransformListener() 00065 else: 00066 self.tf_listener = tf_listener 00067 00068 #point cloud processing action 00069 self._container_client = actionlib.SimpleActionClient('find_container_action', FindContainerAction) 00070 rospy.loginfo("Waiting for find_container_action...") 00071 self._container_client.wait_for_server() 00072 00073 #cluster grasp planning service 00074 service_name = "plan_point_cluster_grasp" 00075 rospy.loginfo("waiting for plan_point_cluster_grasp service") 00076 rospy.wait_for_service(service_name) 00077 rospy.loginfo("service found") 00078 self._cluster_grasp_srv = rospy.ServiceProxy(service_name, GraspPlanning) 00079 00080 #cluster grasp planning params 00081 service_name = "set_point_cluster_grasp_params" 00082 rospy.loginfo("waiting for set_point_cluster_grasp_params service") 00083 rospy.wait_for_service(service_name) 00084 rospy.loginfo("service found") 00085 self._cluster_grasp_params_srv = rospy.ServiceProxy(service_name, SetPointClusterGraspParams) 00086 00087 #make IKUtilities classes if we're checking ik 00088 self._check_ik = rospy.get_param("~check_ik", False) 00089 if self._check_ik: 00090 #inverse kinematics for checking whether grasps are in reach 00091 #TODO: add support for one-armed case 00092 self.ik_utilities = [0]*2 00093 self.ik_utilities[0] = ik_utilities.IKUtilities('right', self.tf_listener) 00094 self.ik_utilities[1] = ik_utilities.IKUtilities('left', self.tf_listener) 00095 00096 #planning scene setting service 00097 planning_scene_srv_name = "environment_server/set_planning_scene_diff" 00098 rospy.loginfo("waiting for %s service"%planning_scene_srv_name) 00099 rospy.wait_for_service(planning_scene_srv_name) 00100 self._planning_scene_srv = rospy.ServiceProxy(planning_scene_srv_name, SetPlanningSceneDiff) 00101 00102 #top or side opening container 00103 #top is assumed to be pointing in +z in roi_box_pose, which currently needs to be axis-aligned (container opens up, if frame is base_link) 00104 #side is currently assumed to be pointing in -x in roi_box_pose (robot is facing container, if frame is base_link) 00105 self._side_opening = rospy.get_param("~side_opening", False) 00106 00107 #whether to make the pregrasp just outside the cluster bounding box, or let it be the default (10 cm) 00108 self._pregrasp_just_outside_box = rospy.get_param("~pregrasp_just_outside_box", False) 00109 00110 #inserted for HRI studies--if we are on task number 3 (shelf), set self._side_opening to True 00111 task_number = rospy.get_param("interactive_grasping/task_number", 0) 00112 if task_number == 3: 00113 rospy.loginfo("Task number is 3, doing front grasps only.") 00114 self._side_opening = True 00115 00116 #name of the cluster planner service topic 00117 self._cluster_planner_name = rospy.get_param("cluster_planner_name", "point_cluster_grasp_planner") 00118 00119 #advertise the grasp planning service 00120 if advertise_service: 00121 rospy.Service('plan_segmented_clutter_grasps', GraspPlanning, self.plan_segmented_clutter_grasps_callback) 00122 rospy.loginfo("segmented clutter grasp planner is ready for queries") 00123 00124 #advertise the grasp planning action 00125 if advertise_action: 00126 self._as = actionlib.SimpleActionServer(rospy.get_name(), GraspPlanningAction, execute_cb = self.segmented_clutter_grasps_execute_cb) 00127 self._as.start() 00128 rospy.loginfo(rospy.get_name()+" is ready for queries") 00129 00130 00131 ##service callback for the plan_segmented_clutter_grasps service 00132 def plan_segmented_clutter_grasps_callback(self, req): 00133 rospy.loginfo("planning grasps for a point cloud") 00134 resp = GraspPlanningResponse() 00135 (grasps, error_code) = self.plan_grasps_for_target(req.target, req.arm_name) 00136 resp.grasps = grasps 00137 resp.error_code = error_code 00138 return resp 00139 00140 00141 ##action callback for the segmented_clutter_grasps_action 00142 def segmented_clutter_grasps_execute_cb(self, goal): 00143 result = GraspPlanningResult() 00144 (grasps, error_code) = self.plan_grasps_for_target(goal.target, goal.arm_name) 00145 result.grasps = grasps 00146 result.error_code = error_code 00147 self._as.set_succeeded(result) 00148 00149 00150 ##return grasps for either a service or action request 00151 def plan_grasps_for_target(self, target, arm_name): 00152 00153 #plan grasps for the point cloud region (returned in the roi_box_pose frame) 00154 grasps = self.find_grasps(target.region.cloud, target.region.roi_box_pose, target.region.roi_box_dims, arm_name, target.reference_frame_id) 00155 00156 error_code = GraspPlanningErrorCode() 00157 error_code.value = error_code.SUCCESS 00158 if grasps == None: 00159 error_code.value = error_code.OTHER_ERROR 00160 return ([], error_code) 00161 00162 #if grasps aren't in the reference_frame_id, transform them 00163 #if target.region.roi_box_pose.header.frame_id == target.reference_frame_id: 00164 # transformed_grasps = grasps 00165 #else: 00166 # for grasp in grasps: 00167 # transformed_grasp_pose = change_pose_stamped_frame(self.tf_listener, 00168 # stamp_pose(grasp.grasp_pose, target.region.roi_box_pose.header.frame_id), 00169 # target.reference_frame_id).pose 00170 # grasp.grasp_pose = transformed_grasp_pose 00171 00172 return (grasps, error_code) 00173 00174 00175 ##call the find_container_action to remove parallel-to-container-side points and cluster the contents 00176 def process_point_cloud(self, point_cloud, box_pose, box_dims, side = False): 00177 00178 # x: 0 - 0.5 y: -0.3 to -0.7 z: 0.22 - 0.82 00179 container_goal = FindContainerGoal() 00180 container_goal.cloud = point_cloud 00181 container_goal.box_pose = box_pose 00182 container_goal.box_dims = box_dims 00183 if side: 00184 container_goal.opening_dir.x = -1. 00185 else: 00186 container_goal.opening_dir.z = 1. 00187 00188 success = self._container_client.send_goal_and_wait(container_goal, rospy.Duration(10.0)) 00189 00190 if not success: 00191 rospy.logerr("Timed out while processing find_container_action") 00192 return [] 00193 00194 result = self._container_client.get_result() 00195 return result.clusters 00196 00197 00198 #call plan_point_cluster_grasp to get candidate grasps for a cluster 00199 def call_plan_point_cluster_grasp(self, cluster, arm_name, reference_frame_id, side = False): 00200 00201 req = GraspPlanningRequest() 00202 req.target.reference_frame_id = reference_frame_id 00203 req.target.region.cloud = cluster 00204 req.arm_name = arm_name #not that cluster planner cares right now which arm 00205 try: 00206 res = self._cluster_grasp_srv(req) 00207 except rospy.ServiceException, e: 00208 rospy.logerr("error when calling plan_point_cluster_grasp: %s"%e) 00209 return 0 00210 if res.error_code.value != 0: 00211 return [] 00212 return res.grasps 00213 00214 00215 #call the set_point_cluster_grasp_params service with appropriate params 00216 def call_set_point_cluster_grasp_params(self, side = False, pregrasp_just_outside_box = False): 00217 00218 req = SetPointClusterGraspParamsRequest() 00219 req.height_good_for_side_grasps = 0.02 00220 req.gripper_opening = 0.083 00221 req.side_step = 0.04 00222 req.palm_step = 0.02 00223 if side: 00224 req.side_grasps_only = True 00225 req.overhead_grasps_only = False 00226 else: 00227 req.side_grasps_only = False 00228 req.overhead_grasps_only = True 00229 req.include_high_point_grasps = False 00230 req.pregrasp_just_outside_box = pregrasp_just_outside_box 00231 req.backoff_depth_steps = 1 00232 req.disable_grasp_neighbor_check = True 00233 try: 00234 res = self._cluster_grasp_params_srv(req) 00235 except rospy.ServiceException, e: 00236 rospy.logerr("error when calling set_cluster_grasp_params: %s"%e) 00237 return 0 00238 return 1 00239 00240 00241 #find grasps on a point cloud inside a bounding box 00242 def find_grasps(self, point_cloud, box_pose, box_dims, arm_name, reference_frame_id): 00243 00244 start_time = time.time() 00245 00246 #top or side opening container 00247 #side is currently assumed to be pointing in -x in roi_box_pose (robot is facing container, if frame is base_link) 00248 self._side_opening = rospy.get_param("~side_opening", False) 00249 00250 #overrule if we're doing task number 3 with the interactive grasping study interfaces 00251 task_number = rospy.get_param("interactive_grasping/task_number", 0) 00252 if task_number == 3: 00253 rospy.loginfo("Task number is 3, doing front grasps only.") 00254 self._side_opening = True 00255 00256 #remove container-parallel points and cluster the contents 00257 clusters = self.process_point_cloud(point_cloud, box_pose, box_dims, self._side_opening) 00258 if len(clusters) == 0: 00259 return [] 00260 00261 #whether to make the pregrasp just outside the cluster bounding box, or let it be the default (10 cm) 00262 self._pregrasp_just_outside_box = rospy.get_param("~pregrasp_just_outside_box", False) 00263 00264 #set the params for the point cluster grasp planner 00265 self.call_set_point_cluster_grasp_params(self._side_opening, self._pregrasp_just_outside_box) 00266 00267 #compute grasps on all the clusters in turn 00268 grasp_plan_start_time = time.time() 00269 grasps = [] 00270 rospy.loginfo("computing grasps on %d clusters"%len(clusters)) 00271 for (ind, cluster) in enumerate(clusters): 00272 cluster_grasps = self.call_plan_point_cluster_grasp(cluster, arm_name, reference_frame_id, self._side_opening) 00273 grasps.extend(cluster_grasps) 00274 rospy.loginfo("%d grasps from cluster %d"%(len(cluster_grasps), ind)) 00275 grasp_plan_end_time = time.time() 00276 rospy.loginfo("total cluster planning time elapsed: %.2f"%(grasp_plan_end_time-grasp_plan_start_time)) 00277 00278 #sort the grasps by quality 00279 sorted(grasps, key=lambda t:t.success_probability, reverse=True) 00280 00281 #set the planning scene so IK isn't unhappy 00282 if self._check_ik: 00283 scene_req = SetPlanningSceneDiffRequest() 00284 try: 00285 res = self._planning_scene_srv(scene_req) 00286 except rospy.ServiceException, e: 00287 rospy.logerr("error when setting planning scene: %s"%e) 00288 self._check_ik = False 00289 00290 #filter out grasps pointing the wrong way/out of reach 00291 filtered_grasps = [] 00292 print "%d grasps before filtering"%len(grasps) 00293 start_angles = [0]*7 00294 for grasp in grasps: 00295 00296 #if side, filter out ones that are pointing the wrong way 00297 if self._side_opening: 00298 grasp_mat = pose_to_mat(grasp.grasp_pose) 00299 if grasp_mat[0,0] < 0.7: 00300 continue 00301 00302 #filter out all those remaining grasps that are out of reach 00303 if self._check_ik: 00304 grasp_pose_stamped = stamp_pose(grasp.grasp_pose, reference_frame_id) 00305 if arm_name == "right_arm": 00306 (solution, error_code) = self.ik_utilities[0].run_ik(grasp_pose_stamped, start_angles, 00307 self.ik_utilities[0].link_name, collision_aware = 0) 00308 else: 00309 (solution, error_code) = self.ik_utilities[1].run_ik(grasp_pose_stamped, start_angles, 00310 self.ik_utilities[1].link_name, collision_aware = 0) 00311 if error_code != "SUCCESS": 00312 continue 00313 00314 filtered_grasps.append(grasp) 00315 00316 end_time = time.time() 00317 print "total grasp planning time elapsed: %.2f"%(end_time - start_time) 00318 print "returning with %d grasps"%len(filtered_grasps) 00319 return filtered_grasps 00320 00321 00322 if __name__ == '__main__': 00323 rospy.init_node('segmented_clutter_grasp_planner_server', anonymous=False) 00324 00325 ccgps = ClusteredClutterGraspPlanner(advertise_service = False, advertise_action = True) 00326 rospy.spin()