test_simplegrasp.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_public
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 simple grasp test.
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 tf
00063 import sys
00064 import grasping_functions
00065 
00066 from simple_script_server import *
00067 from pr2_controllers_msgs.msg import *
00068 from geometry_msgs.msg import *
00069 from srs_grasping.srv import *
00070 from cob_object_detection_msgs.srv import *
00071 
00072 #from numpy import matrix
00073 
00074 
00075 class GraspScript(script):
00076                 
00077         def __init__(self):
00078                 
00079                 self.sss = simple_script_server()
00080                 self.detection_service = rospy.ServiceProxy('/object_detection/detect_object', DetectObjects)
00081                 self.arm_state = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state)
00082                 self.listener = tf.TransformListener(True, rospy.Duration(10.0))
00083 
00084 
00085                 # initialize components (not needed for simulation)
00086                 """
00087                 self.sss.init("tray")
00088                 self.sss.init("torso")
00089                 self.sss.init("arm")
00090                 self.sss.init("sdh")
00091                 self.sss.init("base")
00092                 """
00093 
00094                 # move to initial positions
00095                 handle_arm = self.sss.move("arm","folded",False)
00096                 handle_torso = self.sss.move("torso","home",False)
00097                 handle_sdh = self.sss.move("sdh","home",False)
00098                 self.sss.move("tray","down")
00099                 handle_arm.wait()
00100                 handle_torso.wait()
00101                 handle_sdh.wait()
00102                 
00103                 if not self.sss.parse:
00104                         print "Please localize the robot with rviz"
00105                 #self.sss.wait_for_input()
00106                 
00107 
00108         def run(self, object_id):
00109 
00110                 # prepare for grasping
00111                 #self.sss.move("base","kitchen")
00112                 self.sss.move("arm","look_at_table")
00113                 self.sss.move("sdh","cylopen")
00114                 rospy.sleep(2);
00115 
00116                 #current_joint_configuration
00117                 while self.arm_state.get_num_connections() == 0:
00118                         time.sleep(0.3)
00119                         continue
00120 
00121                 #Detection
00122                 req = DetectObjectsRequest()
00123                 req.object_name.data = grasping_functions.databaseutils.get_object_name(object_id);
00124                 res = self.detection_service(req)
00125                 
00126                 print "---------------------------------------------------------"
00127                 for i in range(0,len(res.object_list.detections)):
00128                         print str(i)+": "+res.object_list.detections[i].label
00129                 print "---------------------------------------------------------"
00130 
00131                 index = -1;
00132                 while (index < 0):
00133                         index = 0#int(raw_input("Select object to grasp: "))
00134                 
00135                 obj = res.object_list.detections[index].pose
00136                 obj.header.stamp = self.listener.getLatestCommonTime("/base_link", obj.header.frame_id)
00137                 obj = self.listener.transformPose("/base_link", obj)
00138                 print "obj_pose:\n",obj.pose
00139 
00140 
00141 
00142                 print "Calling get_feasible_grasps service..."
00143                 get_grasps_from_position = rospy.ServiceProxy('get_feasible_grasps', GetFeasibleGrasps)
00144                 req = GetFeasibleGraspsRequest(object_id, obj.pose, [0.0, 0.0]) 
00145                 grasp_configuration = (get_grasps_from_position(req)).grasp_configuration
00146                 print "get_feasible_grasps service has finished."
00147 
00148 
00149                 for i in range(0,len(grasp_configuration)):
00150 
00151                         pre = PoseStamped()
00152                         pre.header.stamp = rospy.Time.now()
00153                         pre.header.frame_id = "/base_link"
00154                         pre.pose.position.x = grasp_configuration[i].pre_grasp.pose.position.x
00155                         pre.pose.position.y = grasp_configuration[i].pre_grasp.pose.position.y
00156                         pre.pose.position.z = grasp_configuration[i].pre_grasp.pose.position.z
00157                         pre.pose.orientation.x = grasp_configuration[i].pre_grasp.pose.orientation.x
00158                         pre.pose.orientation.y = grasp_configuration[i].pre_grasp.pose.orientation.y
00159                         pre.pose.orientation.z = grasp_configuration[i].pre_grasp.pose.orientation.z
00160                         pre.pose.orientation.w = grasp_configuration[i].pre_grasp.pose.orientation.w
00161 
00162                         g = PoseStamped()
00163                         g.header.stamp = rospy.Time.now()
00164                         g.header.frame_id = "/base_link"
00165                         g.pose.position.x = grasp_configuration[i].grasp.pose.position.x
00166                         g.pose.position.y = grasp_configuration[i].grasp.pose.position.y
00167                         g.pose.position.z = grasp_configuration[i].grasp.pose.position.z
00168                         g.pose.orientation.x = grasp_configuration[i].grasp.pose.orientation.x
00169                         g.pose.orientation.y = grasp_configuration[i].grasp.pose.orientation.y
00170                         g.pose.orientation.z = grasp_configuration[i].grasp.pose.orientation.z
00171                         g.pose.orientation.w = grasp_configuration[i].grasp.pose.orientation.w
00172         
00173                         sol = False
00174                         for w in range(0,5):
00175                                 (pre_grasp_conf, error_code) = grasping_functions.graspingutils.callIKSolver(current_joint_configuration, pre);
00176                                 if(error_code.val == error_code.SUCCESS):
00177                                         for k in range(0,5):
00178                                                 (grasp_conf, error_code) = grasping_functions.graspingutils.callIKSolver(pre_grasp_conf, g);
00179                                                 if(error_code.val == error_code.SUCCESS):               
00180                                                         print str(i)+": IK solution found"
00181                                                         sol = True
00182                                                         break
00183                                         if sol:
00184                                                 break
00185 
00186 
00187                         if sol:
00188                                 print "category:",grasp_configuration[i].category
00189                                 print "grasp position:\n",g.pose.position
00190                                 res = raw_input("Execute this grasp? (y/n): ")
00191 
00192                                 if res != "y":  
00193                                         continue
00194                                 else:
00195                                         #grasping_functions.openraveutils.grasp_view(object_id, grasp_configuration[i], obj.pose)
00196                                         # execute grasp
00197                                         handle_say = self.sss.say(["I am grasping the object now."], False)
00198                                         handle_arm = self.sss.move("arm", [pre_grasp_conf], False)
00199                                         self.sss.move("sdh", "cylopen")
00200                                         handle_say.wait()
00201                                         handle_arm.wait()
00202 
00203                                         raw_input("Grasp...")
00204                                         handle_arm2 = self.sss.move("arm", [grasp_conf], False)
00205                                         handle_arm2.wait()
00206 
00207                                         raw_input("Catch the object")
00208                                         handle_sdh = self.sss.move("sdh", [list(grasp_configuration[i].sdh_joint_values)], False)
00209                                         handle_sdh.wait()
00210                                         rospy.sleep(4);
00211 
00212                 
00213                                         # place obj on tray
00214                                         handle01 = self.sss.move("arm","grasp-to-tray",False)
00215                                         self.sss.move("tray","up")
00216                                         handle01.wait()
00217 
00218                                         self.sss.move("arm","tray")
00219                                         self.sss.move("sdh","cylopen")
00220                                         handle01 = self.sss.move("arm","tray-to-folded",False)
00221                                         self.sss.sleep(4)
00222                                         self.sss.move("sdh","cylclosed",False)
00223                                         handle01.wait()
00224 
00225                                         # deliver cup to order position
00226                                         #self.sss.move("base","order")
00227                                         #self.sss.say("Here's your drink.")
00228                                         #self.sss.move("torso","nod")
00229 
00230                                         res = grasping_functions.graspingutils.sdh_tactil_sensor_result()
00231                                         if not res:
00232                                                 val = list(grasp_configuration[i].sdh_joint_values)
00233                                                 val[1] += 0.07
00234                                                 val[3] += 0.07
00235                                                 val[5] += 0.07
00236                                                 handle_sdh = self.sss.move("sdh", [val], False)
00237                                                 handle_sdh.wait()
00238                                                 rospy.sleep(4);
00239                                                 return 0;
00240                                         
00241                                         return 0;
00242                 return -1
00243 
00244 
00245         def get_joint_state(self, msg):
00246                 global current_joint_configuration
00247                 current_joint_configuration = list(msg.desired.positions)
00248                 rospy.spin()
00249 
00250 
00251 if __name__ == "__main__":
00252 
00253         rospy.init_node('grasp_test')
00254         print "---------------------------------------------------------------------------------------"
00255         print "usage:\t\trosrun srs_grasping test_graspMilk.py [object_id]\ndefault:\tobject_id: 9 (Milkbox)"
00256         print "---------------------------------------------------------------------------------------"
00257         s = GraspScript()
00258 
00259         if len(sys.argv) == 1:
00260                 s.run(9)
00261         else:
00262                 s.run(int(sys.argv[1]))
00263 


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