pr2_gripper_grasp_planner_cluster: point_cluster_grasp_planner

Go to the documentation of this file.
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()