$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_server 00037 # Server for the point_cluster_grasp_planner 00038 00039 import roslib 00040 roslib.load_manifest('pr2_gripper_grasp_planner_cluster') 00041 import rospy 00042 import actionlib 00043 from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningResponse 00044 from object_manipulation_msgs.msg import Grasp, GraspPlanningAction, GraspPlanningErrorCode, GraspPlanningResult 00045 from pr2_gripper_grasp_planner_cluster.srv import SetPointClusterGraspParams, SetPointClusterGraspParamsResponse 00046 import pr2_gripper_grasp_planner_cluster.point_cluster_grasp_planner as grasp_planner_cluster 00047 from sensor_msgs.msg import JointState 00048 import random 00049 import pdb 00050 from object_manipulator.convert_functions import * 00051 import time 00052 00053 ##class for the point cluster grasp planner service 00054 class PointClusterGraspPlannerServer: 00055 00056 def __init__(self): 00057 00058 self.pcgp = grasp_planner_cluster.PointClusterGraspPlanner() 00059 00060 #advertise service and action for planning grasps 00061 rospy.Service('plan_point_cluster_grasp', GraspPlanning, self.plan_point_cluster_grasp_callback) 00062 self._as = actionlib.SimpleActionServer('plan_point_cluster_grasp', GraspPlanningAction, 00063 execute_cb = self.plan_point_cluster_grasp_execute_cb) 00064 00065 #advertise service for evaluating grasps 00066 rospy.Service('evaluate_point_cluster_grasps', GraspPlanning, self.evaluate_point_cluster_grasps_callback) 00067 00068 #advertise service for changing params 00069 rospy.Service('set_point_cluster_grasp_params', SetPointClusterGraspParams, self.set_point_cluster_grasp_params_callback) 00070 00071 #param to randomize grasps (generally a bad idea, unless you actually want bad grasps.) 00072 self.randomize_grasps = rospy.get_param("~randomize_grasps", 0) 00073 rospy.loginfo("randomize_grasps:"+str(self.randomize_grasps)) 00074 random.seed() 00075 00076 #start the action server 00077 self._as.start() 00078 00079 rospy.loginfo("point cluster grasp planner service/action is ready") 00080 00081 00082 ##service callback for changing params 00083 def set_point_cluster_grasp_params_callback(self, req): 00084 self.pcgp.height_good_for_side_grasps = req.height_good_for_side_grasps 00085 self.pcgp.gripper_opening = req.gripper_opening 00086 self.pcgp.side_step = req.side_step 00087 self.pcgp.palm_step = req.palm_step 00088 self.pcgp.overhead_grasps_only = req.overhead_grasps_only 00089 self.pcgp.side_grasps_only = req.side_grasps_only 00090 self.pcgp.include_high_point_grasps = req.include_high_point_grasps 00091 self.pcgp.pregrasp_just_outside_box = req.pregrasp_just_outside_box 00092 self.pcgp.backoff_depth_steps = req.backoff_depth_steps 00093 if self.pcgp.backoff_depth_steps < 1: 00094 self.pcgp.backoff_depth_steps = 1 00095 self.pcgp.disable_grasp_neighbor_check = req.disable_grasp_neighbor_check 00096 self.randomize_grasps = req.randomize_grasps 00097 resp = SetPointClusterGraspParamsResponse() 00098 return resp 00099 00100 00101 ##service callback for the evaluate_point_cluster_grasps service 00102 def evaluate_point_cluster_grasps_callback(self, req): 00103 #rospy.loginfo("evaluating grasps for a point cluster") 00104 00105 #find the cluster bounding box and relevant frames, and transform the cluster 00106 if len(req.target.cluster.points) > 0: 00107 self.pcgp.init_cluster_grasper(req.target.cluster) 00108 cluster_frame = req.target.cluster.header.frame_id 00109 else: 00110 self.pcgp.init_cluster_grasper(req.target.region.cloud) 00111 cluster_frame = req.target.region.cloud.header.frame_id 00112 00113 #evaluate the grasps on the cluster 00114 probs = self.pcgp.evaluate_point_cluster_grasps(req.grasps_to_evaluate, cluster_frame) 00115 00116 #return the same grasps with the qualities added 00117 for (grasp, prob) in zip(req.grasps_to_evaluate, probs): 00118 grasp.success_probability = prob 00119 00120 #fill in the response 00121 resp = GraspPlanningResponse() 00122 resp.error_code.value = 0 00123 resp.grasps = req.grasps_to_evaluate 00124 00125 return resp 00126 00127 00128 def create_joint_state(self, hand_joints, position, effort): 00129 js = JointState() 00130 js.name = hand_joints 00131 js.position = position 00132 js.effort = effort 00133 return js 00134 00135 00136 ##action callback for the plan_point_cluster_grasp action 00137 def plan_point_cluster_grasp_execute_cb(self, goal): 00138 rospy.loginfo("planning grasps for a point cluster") 00139 result = GraspPlanningResult() 00140 (grasps, error_code) = self.plan_point_cluster_grasps(goal.target, goal.arm_name) 00141 result.grasps = grasps 00142 result.error_code = error_code 00143 self._as.set_succeeded(result) 00144 00145 00146 ##service callback for the plan_point_cluster_grasp service 00147 def plan_point_cluster_grasp_callback(self, req): 00148 rospy.loginfo("planning grasps for a point cluster") 00149 resp = GraspPlanningResponse() 00150 (grasps, error_code) = self.plan_point_cluster_grasps(req.target, req.arm_name) 00151 resp.grasps = grasps 00152 resp.error_code = error_code 00153 return resp 00154 00155 00156 def plan_point_cluster_grasps(self, target, arm_name): 00157 error_code = GraspPlanningErrorCode() 00158 grasps = [] 00159 00160 #get the hand joint names from the param server (loaded from yaml config file) 00161 joint_names_dict = rospy.get_param('~joint_names') 00162 pregrasp_joint_angles_dict = rospy.get_param('~pregrasp_joint_angles') 00163 grasp_joint_angles_dict = rospy.get_param('~grasp_joint_angles') 00164 pregrasp_joint_efforts_dict = rospy.get_param('~pregrasp_joint_efforts') 00165 grasp_joint_efforts_dict = rospy.get_param('~grasp_joint_efforts') 00166 if not arm_name: 00167 arm_name = joint_names_dict.keys()[0] 00168 rospy.logerr("point cluster grasp planner: missing arm_name in request! Using "+arm_name) 00169 try: 00170 hand_joints = joint_names_dict[arm_name] 00171 except KeyError: 00172 arm_name = joint_names_dict.keys()[0] 00173 rospy.logerr("arm_name "+arm_name+" not found! Using joint names from "+arm_name) 00174 hand_joints = joint_names_dict[arm_name] 00175 pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name] 00176 grasp_joint_angles = grasp_joint_angles_dict[arm_name] 00177 pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name] 00178 grasp_joint_efforts = grasp_joint_efforts_dict[arm_name] 00179 00180 #hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints') 00181 rospy.loginfo("hand_joints:"+str(hand_joints)) 00182 00183 #find the cluster bounding box and relevant frames, and transform the cluster 00184 init_start_time = time.time() 00185 if len(target.cluster.points) > 0: 00186 self.pcgp.init_cluster_grasper(target.cluster) 00187 cluster_frame = target.cluster.header.frame_id 00188 else: 00189 self.pcgp.init_cluster_grasper(target.region.cloud) 00190 cluster_frame = target.region.cloud.header.frame_id 00191 if len(cluster_frame) == 0: 00192 rospy.logerr("region.cloud.header.frame_id was empty!") 00193 error_code.value = error_code.OTHER_ERROR 00194 return (grasps, error_code) 00195 init_end_time = time.time() 00196 #print "init time: %.3f"%(init_end_time - init_start_time) 00197 00198 #plan grasps for the cluster (returned in the cluster frame) 00199 grasp_plan_start_time = time.time() 00200 (pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists) = self.pcgp.plan_point_cluster_grasps() 00201 grasp_plan_end_time = time.time() 00202 #print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time) 00203 00204 #add error code to service 00205 error_code.value = error_code.SUCCESS 00206 grasp_list = [] 00207 if pregrasp_poses == None: 00208 error_code.value = error_code.OTHER_ERROR 00209 return (grasps, error_code) 00210 00211 #fill in the list of grasps 00212 for (grasp_pose, quality, pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists): 00213 pre_grasp_joint_state = self.create_joint_state(hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts) 00214 grasp_joint_state = self.create_joint_state(hand_joints, grasp_joint_angles, grasp_joint_efforts) 00215 00216 #if the cluster isn't in the same frame as the graspable object reference frame, 00217 #transform the grasps to be in the reference frame 00218 if cluster_frame == target.reference_frame_id: 00219 transformed_grasp_pose = grasp_pose 00220 else: 00221 transformed_grasp_pose = change_pose_stamped_frame(self.pcgp.tf_listener, 00222 stamp_pose(grasp_pose, cluster_frame), 00223 target.reference_frame_id).pose 00224 if self.pcgp.pregrasp_just_outside_box: 00225 min_approach_distance = pregrasp_dist 00226 else: 00227 min_approach_distance = max(pregrasp_dist-.05, .05) 00228 grasp_list.append(Grasp(pre_grasp_posture=pre_grasp_joint_state, grasp_posture=grasp_joint_state, 00229 grasp_pose=transformed_grasp_pose, success_probability=quality, 00230 desired_approach_distance = pregrasp_dist, min_approach_distance = min_approach_distance)) 00231 00232 #if requested, randomize the first few grasps 00233 if self.randomize_grasps: 00234 first_grasps = grasp_list[:30] 00235 random.shuffle(first_grasps) 00236 shuffled_grasp_list = first_grasps + grasp_list[30:] 00237 grasps = shuffled_grasp_list 00238 else: 00239 grasps = grasp_list 00240 00241 return (grasps, error_code) 00242 00243 00244 if __name__ == '__main__': 00245 rospy.init_node('point_cluster_grasp_planner', anonymous=False) 00246 00247 pcgps = PointClusterGraspPlannerServer() 00248 00249 rospy.loginfo("point cluster grasp planner is ready for queries") 00250 rospy.spin()