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 manipulation_msgs.srv import GraspPlanning, GraspPlanningResponse
00044 from manipulation_msgs.msg import Grasp, GraspPlanningAction, GraspPlanningErrorCode, GraspPlanningResult, GripperTranslation
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         hand_description = rospy.get_param('hand_description')
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 = hand_description.keys()[0]
00168             rospy.logerr("point cluster grasp planner: missing arm_name in request!  Using "+arm_name)
00169         try:
00170             hand_joints = hand_description[arm_name]["hand_joints"]
00171         except KeyError:
00172             arm_name = hand_description.keys()[0]
00173             rospy.logerr("arm_name "+arm_name+" not found!  Using joint names from "+arm_name)
00174             try:
00175                 hand_joints = hand_description[arm_name]["hand_joints"]
00176             except KeyError:
00177                 rospy.logerr("no hand joints found for %s!"%arm_name)
00178                 return ([], error_code.OTHER_ERROR)
00179         try:
00180             hand_frame = hand_description[arm_name]["hand_frame"]
00181             hand_approach_direction = hand_description[arm_name]["hand_approach_direction"]
00182         except KeyError:
00183             rospy.logerr("couldn't find hand_frame or hand_approach_direction!")
00184             return ([], error_code.OTHER_ERROR)
00185         pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name]
00186         grasp_joint_angles = grasp_joint_angles_dict[arm_name]
00187         pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name]
00188         grasp_joint_efforts = grasp_joint_efforts_dict[arm_name]
00189 
00190         #hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints')
00191         rospy.loginfo("hand_joints:"+str(hand_joints))
00192 
00193         #find the cluster bounding box and relevant frames, and transform the cluster
00194         init_start_time = time.time()
00195         if len(target.cluster.points) > 0:
00196             self.pcgp.init_cluster_grasper(target.cluster)
00197             cluster_frame = target.cluster.header.frame_id
00198         else:
00199             self.pcgp.init_cluster_grasper(target.region.cloud)
00200             cluster_frame = target.region.cloud.header.frame_id
00201             if len(cluster_frame) == 0:
00202                 rospy.logerr("region.cloud.header.frame_id was empty!")
00203                 error_code.value = error_code.OTHER_ERROR
00204                 return (grasps, error_code)
00205         init_end_time = time.time()
00206         #print "init time: %.3f"%(init_end_time - init_start_time)
00207 
00208         #plan grasps for the cluster (returned in the cluster frame)
00209         grasp_plan_start_time = time.time()
00210         (pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists) = self.pcgp.plan_point_cluster_grasps()
00211         grasp_plan_end_time = time.time()
00212         #print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time)
00213 
00214         #add error code to service
00215         error_code.value = error_code.SUCCESS
00216         grasp_list = []
00217         if pregrasp_poses == None:
00218             error_code.value = error_code.OTHER_ERROR
00219             return (grasps, error_code)
00220 
00221         #fill in the list of grasps
00222         for (grasp_pose, quality, pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists):
00223             pre_grasp_joint_state = self.create_joint_state(hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts)
00224             grasp_joint_state = self.create_joint_state(hand_joints, grasp_joint_angles, grasp_joint_efforts)
00225 
00226             #if the cluster isn't in the same frame as the graspable object reference frame,
00227             #transform the grasps to be in the reference frame
00228             if cluster_frame == target.reference_frame_id:
00229                 transformed_grasp_pose = stamp_pose(grasp_pose, cluster_frame)
00230             else:
00231                 transformed_grasp_pose = change_pose_stamped_frame(self.pcgp.tf_listener, 
00232                                          stamp_pose(grasp_pose, cluster_frame), 
00233                                          target.reference_frame_id)
00234             if self.pcgp.pregrasp_just_outside_box:
00235                 min_approach_distance = pregrasp_dist
00236             else:
00237                 min_approach_distance = max(pregrasp_dist-.05, .05)
00238             approach = GripperTranslation(create_vector3_stamped(hand_approach_direction, hand_frame), pregrasp_dist, min_approach_distance)
00239             retreat = GripperTranslation(create_vector3_stamped([-1.*x for x in hand_approach_direction], hand_frame), pregrasp_dist, min_approach_distance)
00240             grasp_list.append(Grasp(id="id", pre_grasp_posture=pre_grasp_joint_state, grasp_posture=grasp_joint_state, 
00241                                     grasp_pose=transformed_grasp_pose, grasp_quality=quality, 
00242                                     approach=approach, retreat=retreat, max_contact_force=-1))
00243 
00244         #if requested, randomize the first few grasps
00245         if self.randomize_grasps:
00246             first_grasps = grasp_list[:30]
00247             random.shuffle(first_grasps)
00248             shuffled_grasp_list = first_grasps + grasp_list[30:]
00249             grasps = shuffled_grasp_list
00250         else:
00251             grasps = grasp_list
00252 
00253         return (grasps, error_code)
00254 
00255 
00256 if __name__ == '__main__':
00257     rospy.init_node('point_cluster_grasp_planner', anonymous=False)
00258 
00259     pcgps = PointClusterGraspPlannerServer()
00260     
00261     rospy.loginfo("point cluster grasp planner is ready for queries")
00262     rospy.spin()