def execute(self, userdata): rospy.loginfo('Executing state GRASP') #Open SDH at the pre-grasp position ----------------------------------------------- sss.move("sdh", "cylopen") #Get the current arm joint states. sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state) while sub.get_num_connections() == 0: time.sleep(0.3) continue #Move to grasp position with SDH open --------------------------------------------- #pregrasp pre_grasp_stamped = PoseStamped(); pre_grasp_stamped.header.frame_id = "/base_link"; pre_grasp_stamped.pose = userdata.grasp_configuration.pre_grasp; #grasp grasp_stamped = PoseStamped(); grasp_stamped.header.frame_id = "/base_link"; grasp_stamped.pose = userdata.grasp_configuration.grasp; #offset """ offset_x = 0#(userdata.grasp_configuration.grasp.position.x - userdata.grasp_configuration.pre_grasp.position.x)/3 offset_y = 0#(userdata.grasp_configuration.grasp.position.y - userdata.grasp_configuration.pre_grasp.position.y)/3 offset_z = 0#(userdata.grasp_configuration.grasp.position.z - userdata.grasp_configuration.pre_grasp.position.z)/3 pre_grasp_stamped.pose.position.x += offset_x pre_grasp_stamped.pose.position.y += offset_y pre_grasp_stamped.pose.position.z -= offset_z grasp_stamped.pose.position.x += offset_x grasp_stamped.pose.position.y += offset_y grasp_stamped.pose.position.z -= offset_z """ sol = False for i in range(0,10): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre_grasp_stamped) if(error_code.val == error_code.SUCCESS): for j in range(0,10): (grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, grasp_stamped) sol = True break if sol: break; if not sol: return 'failed'; else: arm_handle = sss.move("arm", [pre_grasp_conf], False) arm_handle.wait(); rospy.sleep(2); arm_handle = sss.move("arm", [grasp_conf], False) arm_handle.wait(); rospy.sleep(2); #Close SDH based on the grasp configuration to grasp. arm_handle = sss.move("sdh", [list(userdata.grasp_configuration.sdh_joint_values)], False) arm_handle.wait(); rospy.sleep(2); #TODO: Confirm the grasp based on force feedback successful_grasp = grasping_functions.sdh_tactil_sensor_result(); if successful_grasp: return 'succeeded' else: #TODO: Regrasp (close MORE the fingers) regrasp = list(userdata.grasp_configuration.sdh_joint_values) regrasp[2] -= 0.1 regrasp[4] -= 0.1 regrasp[6] -= 0.1 arm_handle = sss.move("sdh", [regrasp], False) arm_handle.wait(); successful_regrasp = grasping_functions.sdh_tactil_sensor_result(); if successful_regrasp: return 'succeeded' else: return 'failed'
def execute(self, userdata): #TODO: THIS PART SHOULD BE IN THE PREVIOUS STATE ##################################################### grasp_configuration_id = len(userdata.grasp_configuration) - 1 #TODO: userdata.grasp_configuration_id for i in range(0, len(userdata.grasp_configuration)): print userdata.grasp_configuration[i].category if userdata.grasp_configuration[i].category == "TOP": grasp_configuration_id = i #TODO: userdata.grasp_configuration_id break ####################################################################################################### category = userdata.grasp_configuration[ grasp_configuration_id].category #TODO: userdata.grasp_configuration_id if category == "TOP": userdata.grasp_categorisation = 'top' sdh_handle = sss.move("sdh", "spheropen") else: userdata.grasp_categorisation = 'side' sdh_handle = sss.move("sdh", "cylopen") sdh_handle.wait() rospy.sleep(2) #Get the current arm joint states. while self.arm_state.get_num_connections() == 0: time.sleep(0.3) continue #Move to grasp position with SDH open --------------------------------------------- #pregrasp pre_grasp_stamped = PoseStamped() pre_grasp_stamped.header.frame_id = "/base_link" pre_grasp_stamped.pose = userdata.grasp_configuration[ grasp_configuration_id].pre_grasp #TODO: userdata.grasp_configuration_id #grasp grasp_stamped = PoseStamped() grasp_stamped.header.frame_id = "/base_link" grasp_stamped.pose = userdata.grasp_configuration[ grasp_configuration_id].grasp #TODO: userdata.grasp_configuration_id #postgrasp post_grasp_stamped = copy.deepcopy(grasp_stamped) post_grasp_stamped.pose.position.z += 0.1 sol = False for w in range(0, 10): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver( self.current_arm_state, pre_grasp_stamped) if (error_code.val == error_code.SUCCESS): for k in range(0, 10): (grasp_conf, error_code) = grasping_functions.callIKSolver( pre_grasp_conf, grasp_stamped) if (error_code.val == error_code.SUCCESS): (post_grasp_conf, error_code) = grasping_functions.callIKSolver( pre_grasp_conf, post_grasp_stamped) if (error_code.val == error_code.SUCCESS): sol = True break if sol: break if not sol: sss.say(["I can not catch the object!"], False) return 'not_completed' else: sss.say(["I am grasping the object now!"], False) #Close SDH based on the grasp configuration to grasp. arm_handle = sss.move("arm", [pre_grasp_conf, grasp_conf]) arm_handle.wait() rospy.sleep(4) #Close SDH based on the grasp configuration to grasp. ssh_handle = sss.move("sdh", [ list(userdata.grasp_configuration[grasp_configuration_id]. sdh_joint_values) ]) #TODO: userdata.grasp_configuration_id ssh_handle.wait() rospy.sleep(2) #Confirm the grasp based on force feedback successful_grasp = grasping_functions.sdh_tactil_sensor_result() #False if not successful_grasp: #Regrasp (close MORE the fingers) regrasp = list( userdata.grasp_configuration[grasp_configuration_id]. sdh_joint_values) #TODO: userdata.grasp_configuration_id print "Current config, trying regrasp", regrasp regrasp[1] += 0.07 regrasp[3] += 0.07 regrasp[5] += 0.07 print "to", regrasp sdh_handle = sss.move("sdh", [regrasp]) sdh_handle.wait() rospy.sleep(2) successful_grasp = grasping_functions.sdh_tactil_sensor_result( ) #True if not successful_grasp: sss.say(["I can not catch the object.!"], False) return 'not_completed' arm_handle = sss.move("arm", [post_grasp_conf, "look_at_table", "hold"]) arm_handle.wait() rospy.sleep(2) sss.say(["I have grasped the object with success!"], False) return 'succeeded'
return 'failed'; else: arm_handle = sss.move("arm", [pre_grasp_conf], False) arm_handle.wait(); rospy.sleep(2); arm_handle = sss.move("arm", [grasp_conf], False) arm_handle.wait(); rospy.sleep(2); #Close SDH based on the grasp configuration to grasp. arm_handle = sss.move("sdh", [list(userdata.grasp_configuration.sdh_joint_values)], False) arm_handle.wait(); rospy.sleep(2); #TODO: Confirm the grasp based on force feedback successful_grasp = grasping_functions.sdh_tactil_sensor_result(); if successful_grasp: return 'succeeded' else: #TODO: Regrasp (close MORE the fingers) regrasp = list(userdata.grasp_configuration.sdh_joint_values) regrasp[2] -= 0.1 regrasp[4] -= 0.1 regrasp[6] -= 0.1 arm_handle = sss.move("sdh", [regrasp], False) arm_handle.wait(); successful_regrasp = grasping_functions.sdh_tactil_sensor_result(); if successful_regrasp: return 'succeeded'
def execute(self, userdata): #TODO: THIS PART SHOULD BE IN THE PREVIOUS STATE ##################################################### grasp_configuration_id = len(userdata.grasp_configuration)-1; #TODO: userdata.grasp_configuration_id for i in range(0, len(userdata.grasp_configuration)): print userdata.grasp_configuration[i].category if userdata.grasp_configuration[i].category == "TOP": grasp_configuration_id = i; #TODO: userdata.grasp_configuration_id break; ####################################################################################################### category = userdata.grasp_configuration[grasp_configuration_id].category #TODO: userdata.grasp_configuration_id if category == "TOP": userdata.grasp_categorisation = 'top' sdh_handle=sss.move("sdh", "spheropen") else: userdata.grasp_categorisation = 'side' sdh_handle=sss.move("sdh", "cylopen") sdh_handle.wait() rospy.sleep(2) #Get the current arm joint states. while self.arm_state.get_num_connections() == 0: time.sleep(0.3) continue #Move to grasp position with SDH open --------------------------------------------- #pregrasp pre_grasp_stamped = PoseStamped(); pre_grasp_stamped.header.frame_id = "/base_link"; pre_grasp_stamped.pose = userdata.grasp_configuration[grasp_configuration_id].pre_grasp; #TODO: userdata.grasp_configuration_id #grasp grasp_stamped = PoseStamped(); grasp_stamped.header.frame_id = "/base_link"; grasp_stamped.pose = userdata.grasp_configuration[grasp_configuration_id].grasp; #TODO: userdata.grasp_configuration_id #postgrasp post_grasp_stamped = copy.deepcopy(grasp_stamped); post_grasp_stamped.pose.position.z += 0.1; sol = False for w in range(0,10): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(self.current_arm_state, pre_grasp_stamped) if(error_code.val == error_code.SUCCESS): for k in range(0,10): (grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, grasp_stamped) if(error_code.val == error_code.SUCCESS): (post_grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, post_grasp_stamped) if(error_code.val == error_code.SUCCESS): sol = True break if sol: break if not sol: sss.say(["I can not catch the object!"], False) return 'not_completed'; else: sss.say(["I am grasping the object now!"], False) #Close SDH based on the grasp configuration to grasp. arm_handle = sss.move("arm", [pre_grasp_conf, grasp_conf]) arm_handle.wait() rospy.sleep(4) #Close SDH based on the grasp configuration to grasp. ssh_handle = sss.move("sdh", [list(userdata.grasp_configuration[grasp_configuration_id].sdh_joint_values)]) #TODO: userdata.grasp_configuration_id ssh_handle.wait() rospy.sleep(2); #Confirm the grasp based on force feedback successful_grasp = grasping_functions.sdh_tactil_sensor_result();#False if not successful_grasp: #Regrasp (close MORE the fingers) regrasp = list(userdata.grasp_configuration[grasp_configuration_id].sdh_joint_values) #TODO: userdata.grasp_configuration_id print "Current config, trying regrasp", regrasp regrasp[1] += 0.07 regrasp[3] += 0.07 regrasp[5] += 0.07 print "to", regrasp sdh_handle = sss.move("sdh", [regrasp]) sdh_handle.wait() rospy.sleep(2) successful_grasp = grasping_functions.sdh_tactil_sensor_result();#True if not successful_grasp: sss.say(["I can not catch the object.!"], False) return 'not_completed' arm_handle = sss.move("arm",[post_grasp_conf,"look_at_table","hold"]) arm_handle.wait() rospy.sleep(2) sss.say(["I have grasped the object with success!"], False) return 'succeeded'