pr2_gripper_grasp_planner_cluster_server.py

Go to the documentation of this file.

#!/usr/bin/python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of the Willow Garage nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# author: Kaijen Hsiao

## @package pr2_gripper_grasp_planner_cluster_server
# Server for the pr2_gripper_grasp_planner_cluster

import roslib
roslib.load_manifest('pr2_gripper_grasp_planner_cluster')
import rospy
from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningResponse
from object_manipulation_msgs.msg import Grasp
import pr2_gripper_grasp_planner_cluster.pr2_gripper_grasp_planner_cluster as grasp_planner_cluster
from sensor_msgs.msg import JointState
import random
import pdb
from object_manipulator.convert_functions import *

##class for the point cluster grasp planner service
class PointClusterGraspPlannerServer:

    def __init__(self):
        
        self.pcgp = grasp_planner_cluster.PointClusterGraspPlanner()

        #advertise service for planning grasps
        rospy.Service('plan_point_cluster_grasp', GraspPlanning, self.plan_point_cluster_grasp_callback)

        #advertise service for evaluating grasps
        rospy.Service('evaluate_point_cluster_grasps', GraspPlanning, self.evaluate_point_cluster_grasps_callback) 

        #param to randomize grasps (generally a bad idea, unless you actually want bad grasps.)
        self.randomize_grasps = rospy.get_param("~randomize_grasps", 0)
        rospy.loginfo("randomize_grasps:"+str(self.randomize_grasps))
        random.seed()


    ##service callback for the evaluate_point_cluster_grasps service
    def evaluate_point_cluster_grasps_callback(self, req):
        #rospy.loginfo("evaluating grasps for a point cluster")
        
        #find the cluster bounding box and relevant frames, and transform the cluster
        self.pcgp.init_cluster_grasper(req.target.cluster)

        #evaluate the grasps on the cluster
        probs = self.pcgp.evaluate_point_cluster_grasps(req.grasps_to_evaluate, req.target.cluster.header.frame_id)

        #return the same grasps with the qualities added
        try:
            for (grasp, prob) in zip(req.grasps_to_evaluate, probs):
                grasp.success_probability = prob
        except:
            pdb.set_trace()

        #fill in the response
        resp = GraspPlanningResponse()
        resp.error_code.value = 0
        resp.grasps = req.grasps_to_evaluate
        
        return resp


    ##create a joint state message for a given joint angle 
    #(same for all four joints; 0.5 is open and 0 is closed)
    def create_joint_state(self, hand_joints, position):
        js = JointState()
        js.name = hand_joints
        js.position = len(hand_joints)*[position]
        js.effort = len(hand_joints)*[100.]
        return js


    ##service callback for the plan_point_cluster_grasp service
    def plan_point_cluster_grasp_callback(self, req):
        rospy.loginfo("planning grasps for a point cluster")

        #find the cluster bounding box and relevant frames, and transform the cluster
        self.pcgp.init_cluster_grasper(req.target.cluster)

        #plan grasps for the cluster (returned in the cluster frame)
        (pregrasp_poses, grasp_poses, gripper_openings, qualities) = self.pcgp.plan_point_cluster_grasps()

        #get the names of the hand joints
        arm_name = "right_arm"
        if req.arm_name:
            arm_name = req.arm_name
        else:
            rospy.logerr("point cluster grasp planner: missing arm_name in request!  Using right_arm")
        hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints')
        rospy.loginfo("hand_joints:"+str(hand_joints))

        #add error code to service
        resp = GraspPlanningResponse()        
        resp.error_code.value = resp.error_code.SUCCESS
        grasp_list = []
        if pregrasp_poses == None:
            resp.error_code.value = resp.error_code.OTHER_ERROR
        #fill in the list of grasps
        else:
            for (grasp_pose, quality) in zip(grasp_poses, qualities):
                pre_grasp_joint_state = self.create_joint_state(hand_joints, 10.)  #should be .5?
                grasp_joint_state = self.create_joint_state(hand_joints, 0.)

                #if the cluster isn't in the same frame as the graspable object reference frame,
                #transform the grasps to be in the reference frame
                if req.target.cluster.header.frame_id == req.target.reference_frame_id:
                    transformed_grasp_pose = grasp_pose
                else:
                    transformed_grasp_pose = change_pose_stamped_frame(self.pcgp.tf_listener, 
                                             stamp_pose(grasp_pose, req.target.cluster.header.frame_id), 
                                             req.target.reference_frame_id).pose
                grasp_list.append(Grasp(pre_grasp_posture=pre_grasp_joint_state, grasp_posture=grasp_joint_state, 
                  grasp_pose=transformed_grasp_pose, success_probability=quality))

        #if requested, randomize the first few grasps
        if self.randomize_grasps:
            first_grasps = grasp_list[:30]
            random.shuffle(first_grasps)
            shuffled_grasp_list = first_grasps + grasp_list[30:]
            resp.grasps = shuffled_grasp_list
        else:
            resp.grasps = grasp_list

        return resp


if __name__ == '__main__':
    rospy.init_node('point_cluster_grasp_planner', anonymous=True)

    pcgps = PointClusterGraspPlannerServer()
    
    rospy.loginfo("point cluster grasp planner is ready for queries")
    rospy.spin()

---

pr2_gripper_grasp_planner_cluster
Author(s): Kaijen Hsiao
autogenerated on Fri Jan 11 09:42:43 2013