get_pregrasps.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: Abril 2012
00024 #
00025 # \brief
00026 #   Implements a service that returns a group of pregrasp positions.
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 rospy
00061 import time
00062 import grasping_functions 
00063 
00064 from srs_grasping.srv import *
00065 from srs_msgs.msg import *
00066 from tf.transformations import *
00067 from geometry_msgs.msg import *
00068 
00069 
00070 class get_pregrasps():
00071 
00072         def __init__(self):
00073                 self.ik_loop_reply = 1
00074 
00075                 rospy.loginfo("Waiting /get_db_grasps service...");
00076                 rospy.wait_for_service('/get_db_grasps')
00077                 self.client = rospy.ServiceProxy('/get_db_grasps', GetDBGrasps)
00078                 rospy.loginfo("/get_pregrasps service is ready.");
00079                 print "---------------------------------------------------------------------------";
00080 
00081         def get_pregrasps(self, req):
00082                 x = time.time();
00083                 rospy.loginfo("/get_pregrasps service has been called...");
00084 
00085                 obj_id = req.object_id;
00086                 obj_pose = req.object_pose;
00087                 pregrasp_offsets = req.pregrasp_offsets;
00088                 num_configurations = (1, req.num_configurations)[req.num_configurations>0];
00089                 
00090 
00091                 req = GetDBGraspsRequest();
00092                 req.object_id = obj_id;
00093                 grasp_configuration = (self.client(req)).grasp_configuration;
00094 
00095 
00096                 rotacion = grasping_functions.graspingutils.rotation_matrix(obj_pose);
00097 
00098                 resp = GetPreGraspResponse();
00099                 resp.side = []
00100                 resp.mside = []
00101                 resp.top = []
00102                 resp.front = []
00103 
00104                 for i in range(0,len(grasp_configuration)):
00105                         pre_trans = rotacion * grasping_functions.graspingutils.matrix_from_pose(grasp_configuration[i].pre_grasp.pose);
00106                         grasp_trans = rotacion *  grasping_functions.graspingutils.matrix_from_pose(grasp_configuration[i].grasp.pose);
00107 
00108                         t = translation_from_matrix(pre_trans);
00109                         q = quaternion_from_matrix(pre_trans);
00110                         tg = translation_from_matrix(grasp_trans);
00111                         qg = quaternion_from_matrix(grasp_trans);
00112 
00113                         pre = Pose();
00114                         pre.position.x = t[0];
00115                         pre.position.y = t[1];
00116                         pre.position.z = t[2];
00117                         pre.orientation.x = q[0];
00118                         pre.orientation.y = q[1];
00119                         pre.orientation.z = q[2];
00120                         pre.orientation.w = q[3];
00121 
00122                         g = Pose();
00123                         g.position.x = tg[0];
00124                         g.position.y = tg[1];
00125                         g.position.z = tg[2];
00126                         g.orientation.x = qg[0];
00127                         g.orientation.y = qg[1];
00128                         g.orientation.z = qg[2];
00129                         g.orientation.w = qg[3];
00130 
00131 
00132                         category = grasping_functions.graspingutils.get_grasp_category(pre.position, g.position);
00133                         pre = grasping_functions.graspingutils.set_pregrasp_offsets(category, pre, pregrasp_offsets);
00134 
00135 
00136                         aux = DBGrasp();
00137                         aux.object_id = obj_id;
00138                         aux.hand_type = "SDH";
00139                         aux.sdh_joint_values = grasp_configuration[i].sdh_joint_values;
00140                         aux.pre_grasp.pose = pre;
00141                         aux.grasp.pose = g;
00142                         aux.category = grasping_functions.graspingutils.get_grasp_category(pre.position, g.position);
00143 
00144                         if aux.category == "TOP":
00145                                 if len(resp.top) < num_configurations:
00146                                         resp.top.append(aux);
00147                         elif aux.category == "FRONT":
00148                                 if len(resp.front) < num_configurations:
00149                                         resp.front.append(aux);
00150                         elif aux.category == "SIDE":
00151                                 if len(resp.side) < num_configurations:
00152                                         resp.side.append(aux);
00153                         elif aux.category == "-SIDE":
00154                                 if len(resp.mside) < num_configurations:
00155                                         resp.mside.append(aux);
00156                         else:
00157                                 continue
00158 
00159                         if len(resp.top)==num_configurations and len(resp.side)==num_configurations and len(resp.mside)==num_configurations and len(resp.front)==num_configurations:
00160                                 break;
00161 
00162                 rospy.loginfo("/get_pregrasps call has finished.");
00163                 print "Time employed: " + str(time.time() - x);
00164                 print "---------------------------------------";
00165                 return resp;
00166 
00167 
00168         def get_pregrasps_server(self):
00169                 s = rospy.Service('/get_pregrasps', GetPreGrasp, self.get_pregrasps);
00170 
00171 
00172 ## Main routine for running the grasp server
00173 if __name__ == '__main__':
00174         rospy.init_node('get_pregrasps');
00175         SCRIPT = get_pregrasps();
00176         SCRIPT.get_pregrasps_server();
00177         rospy.spin();


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