get_feasible_grasps.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 #   Copyright (c) 2012 \n
00007 #   Robotnik Automation SLL \n\n
00008 #
00009 #################################################################
00010 #
00011 # \note
00012 #   Project name: srs
00013 # \note
00014 #   ROS stack name: srs
00015 # \note
00016 #   ROS package name: srs_grasping
00017 #
00018 # \author
00019 #   Author: Manuel Rodriguez, email:mrodriguez@robotnik.es
00020 # \author
00021 #   Supervised by: Manuel Rodriguez, email:mrodriguez@robotnik.es
00022 #
00023 # \date Date of creation: March 2012
00024 #
00025 # \brief
00026 #   Implements a service that returns all the feasible grasping configurations for a given object_id and target pose.
00027 #
00028 #################################################################
00029 #
00030 # Redistribution and use in source and binary forms, with or without
00031 # modification, are permitted provided that the following conditions are met:
00032 #
00033 #     - Redistributions of source code must retain the above copyright
00034 #       notice, this list of conditions and the following disclaimer. \n
00035 #     - Redistributions in binary form must reproduce the above copyright
00036 #       notice, this list of conditions and the following disclaimer in the
00037 #       documentation and/or other materials provided with the distribution. \n
00038 #     - Neither the name of the Robotnik Automation SLL nor the names of its
00039 #       contributors may be used to endorse or promote products derived from
00040 #       this software without specific prior written permission. \n
00041 #
00042 # This program is free software: you can redistribute it and/or modify
00043 # it under the terms of the GNU Lesser General Public License LGPL as 
00044 # published by the Free Software Foundation, either version 3 of the 
00045 # License, or (at your option) any later version.
00046 # 
00047 # This program is distributed in the hope that it will be useful,
00048 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00049 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00050 # GNU Lesser General Public License LGPL for more details.
00051 # 
00052 # You should have received a copy of the GNU Lesser General Public 
00053 # License LGPL along with this program. 
00054 # If not, see <http://www.gnu.org/licenses/>.
00055 #
00056 #################################################################
00057 
00058 import roslib
00059 roslib.load_manifest('srs_grasping')
00060 import tf
00061 import rospy
00062 import time
00063 import grasping_functions 
00064 
00065 from srs_grasping.srv import *
00066 from srs_msgs.msg import *
00067 from pr2_controllers_msgs.msg import *
00068 from geometry_msgs.msg import *
00069 from kinematics_msgs.srv import *
00070 from srs_msgs.msg import GraspingErrorCodes
00071 
00072 class get_feasible_grasps():
00073 
00074         def __init__(self):
00075 
00076                 self.ik_loop_reply = 1;
00077                 self.finger_correction = -1;
00078                 self.cjc = []
00079                 self.cylopen_position = [[[0.09121056454806604, 0.03300469295835018, 0.1386980387655565],[0.01977019790965633, 6.428768666637955e-07, 0.9998045500082638, -3.251111695101345e-05]], [[0.01905, 0.033, 0.091],[0.47359137769338583, 1.5399994595997778e-05, 0.880744688270724, -2.863959117221133e-05]], [[-0.11021758805551472, 0.0, 0.13877808854293133],[0.0, -0.019200739055054376, 0.0, 0.9998156488171905]]]
00080                 self.spheropen_position = [[[0.04967501468386526, 0.08596897096059274, 0.15214447310255375],[-0.11284765597435997, -0.06510645561927879, 0.8587963331213594, -0.49547493800907255]], [[0.01905, 0.033, 0.091],[0.3316048124188912, 0.19131645949114948, 0.8001899672091825, -0.4616625142744111]], [[-0.0992496375660626, 0.0, 0.1521898252840332],[0.0, 0.13060867295881992, 0.0, 0.9914339990881572]]]
00081 
00082                 self.listener = tf.TransformListener(True, rospy.Duration(10.0))
00083                 self.arm_state = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state);
00084                 
00085                 rospy.loginfo("Waiting %s service...",  grasping_functions.graspingutils.get_ik_srv_name());
00086                 rospy.wait_for_service( grasping_functions.graspingutils.get_ik_srv_name())
00087                 rospy.loginfo("%s is ready.",  grasping_functions.graspingutils.get_ik_srv_name());
00088 
00089                 rospy.loginfo("Waiting /get_db_grasps service...");
00090                 rospy.wait_for_service('/get_db_grasps')
00091                 self.client = rospy.ServiceProxy('/get_db_grasps', GetDBGrasps)
00092                 rospy.loginfo("/get_feasible_grasps service is ready.");
00093 
00094 
00095         def get_feasible_grasps(self, request):
00096                 x = time.time();
00097                 rospy.loginfo("/get_feasible_grasps service has been called...");
00098 
00099                 pregrasp_offsets = request.pregrasp_offsets
00100                 if len(pregrasp_offsets) != 2:
00101                         print "pregrasps_offsets value must be an array with length 2: [X,Z]"
00102                         print "Using default values: [0.5, 0.0]"
00103                         pregrasp_offsets = [0.0, 0.0];
00104 
00105                 client_response = self.client(request.object_id)
00106                 grasp_configuration = client_response.grasp_configuration;
00107                 error_code = client_response.error_code.val
00108 
00109                 #current_joint_configuration
00110                 while self.arm_state.get_num_connections() == 0:
00111                         time.sleep(0.3);
00112                         continue;
00113 
00114                 rotacion = grasping_functions.graspingutils.rotation_matrix(request.object_pose);
00115 
00116 
00117                 resp = GetFeasibleGraspsResponse();
00118                 resp.error_code.val = 0;
00119                 resp.feasible_grasp_available = False;
00120                 resp.grasp_configuration = []
00121 
00122                 if error_code == GraspingErrorCodes.SUCCESS:
00123 
00124                         for grasp_configuration in grasp_configuration:
00125                                 pre_trans = rotacion * grasping_functions.graspingutils.matrix_from_pose(grasp_configuration.pre_grasp.pose);
00126                                 grasp_trans = rotacion *  grasping_functions.graspingutils.matrix_from_pose(grasp_configuration.grasp.pose);
00127 
00128                                 pre = grasping_functions.graspingutils.pose_from_matrix(pre_trans);
00129                                 g = grasping_functions.graspingutils.pose_from_matrix(grasp_trans);
00130                                 surface_distance = g.pose.position.z - request.object_pose.position.z
00131                                 category = grasping_functions.graspingutils.get_grasp_category(pre.pose.position, g.pose.position);
00132                                 
00133                                 nvg = FeasibleGrasp(grasp_configuration.sdh_joint_values, "/sdh_palm_link", g, pre, surface_distance, category);
00134                                 fpos = self.get_finger_positions(category)
00135                                 pre.pose = grasping_functions.graspingutils.set_pregrasp_offsets(category, pre.pose, pregrasp_offsets);
00136                         
00137                                 if (grasping_functions.graspingutils.valid_grasp(category)) and (not self.checkCollisions(fpos, nvg, request.object_pose)):
00138                                         class FeasibleGraspFound(Exception): pass
00139                                         try:
00140                                                 for w in range(0, self.ik_loop_reply):
00141                                                         (pgc, error) = grasping_functions.graspingutils.callIKSolver(self.cjc, pre);                    
00142                                                         if(error.val == error.SUCCESS):
00143                                                                 for k in range(0,self.ik_loop_reply):
00144                                                                         (gc, error) = grasping_functions.graspingutils.callIKSolver(pgc, g);
00145                                                                         if(error.val == error.SUCCESS):
00146                                                                                 raise FeasibleGraspFound();
00147                                         except FeasibleGraspFound:
00148                                                 resp.feasible_grasp_available = True;
00149                                                 resp.grasp_configuration.append(nvg);
00150                                                 resp.error_code.val = GraspingErrorCodes.SUCCESS
00151 
00152                         #order grasps
00153                         if len(resp.grasp_configuration) > 0:
00154                                 (resp.grasp_configuration).sort();
00155                         else:
00156                                 resp.error_code.val = GraspingErrorCodes.GOAL_UNREACHABLE
00157                 else:
00158                         resp.error_code.val = error_code;
00159 
00160 
00161                 rospy.loginfo(str(len(resp.grasp_configuration))+" valid grasps for this values.");     
00162                 rospy.loginfo("/get_feasible_grasps call has finished with error_code: "+str(resp.error_code));
00163                 print "Time employed: " + str(time.time() - x);
00164                 print "---------------------------------------";
00165                 return resp;
00166          
00167 
00168 
00169         def __cmp__(self, other):
00170                 if other.grasp.position.z > self.grasp.position.z:
00171                         return 1
00172                 elif other.grasp.position.z < self.grasp.position.z:
00173                         return -1
00174                 else:
00175                         return 0
00176 
00177 
00178         def get_joint_state(self, msg):
00179                 self.cjc = list(msg.desired.positions);
00180 
00181 
00182         def get_finger_positions(self, category):
00183                 fing_pos = [];
00184                 trans = (self.cylopen_position, self.spheropen_position)[category == "TOP"]
00185 
00186                 fp1 = PoseStamped();
00187                 fp2 = PoseStamped();
00188                 fp3 = PoseStamped();
00189 
00190                 for finger in range(0,len(trans)):
00191                         fp1.pose.position.x = trans[finger][0][0]
00192                         fp1.pose.position.y = trans[finger][0][1]
00193                         fp1.pose.position.z = trans[finger][0][2]
00194         
00195                         fp1.pose.orientation.x = trans[finger][1][0]
00196                         fp1.pose.orientation.y = trans[finger][1][1]
00197                         fp1.pose.orientation.z = trans[finger][1][2]
00198                         fp1.pose.orientation.w = trans[finger][1][3]
00199 
00200                 fp1.header.frame_id = "/sdh_finger_13_link"
00201                 fp2.header.frame_id = "/sdh_finger_23_link"
00202                 fp3.header.frame_id = "/sdh_thumb_3_link"
00203                 fing_pos.append(fp1);
00204                 fing_pos.append(fp2);
00205                 fing_pos.append(fp3);
00206 
00207                 return fing_pos;
00208 
00209 
00210         def transform_finger_positions(self, fpositions, g):
00211                 response = [];
00212 
00213                 for finger_pos in fpositions:
00214                         ps = PoseStamped();
00215                         ps.header.stamp = self.listener.getLatestCommonTime("/sdh_palm_link", finger_pos.header.frame_id);
00216                         ps = self.listener.transformPose("/sdh_palm_link", finger_pos);
00217 
00218                         #transform to object position
00219                         matrix = grasping_functions.graspingutils.matrix_from_pose(g) * grasping_functions.graspingutils.matrix_from_pose(ps.pose);
00220                         response.append(grasping_functions.graspingutils.pose_from_matrix(matrix));
00221 
00222                 return response;
00223 
00224 
00225         def checkCollisions(self, fpositions, nvg, obj_pose):
00226 
00227                 self.finger_correction = (0, -0.0685)[nvg.category == "TOP"];
00228                 finger_pos = self.transform_finger_positions(fpositions, nvg.grasp.pose);
00229 
00230                 for fp in finger_pos:
00231                         if (obj_pose.position.z + 0.05)> (fp.pose.position.z + self.finger_correction):
00232                                 return True;
00233 
00234                 return False;
00235 
00236 
00237         def get_feasible_grasps_server(self):
00238                 s = rospy.Service('/get_feasible_grasps', GetFeasibleGrasps, self.get_feasible_grasps);
00239 
00240 
00241 ## Main routine for running the grasp server
00242 if __name__ == '__main__':
00243         rospy.init_node('get_feasible_grasps');
00244         SCRIPT = get_feasible_grasps();
00245         SCRIPT.get_feasible_grasps_server();
00246         rospy.spin();


srs_grasping
Author(s): Robotnik Automation SLL
autogenerated on Mon Oct 6 2014 08:59:42