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): # prepare for grasping #self.sss.move("base","kitchen") self.sss.move("arm", "look_at_table") self.sss.move("sdh", "cylopen") rospy.sleep(2) #current_joint_configuration sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state) while sub.get_num_connections() == 0: time.sleep(0.3) continue #Detection self.srv_name_object_detection = '/object_detection/detect_object' detector_service = rospy.ServiceProxy(self.srv_name_object_detection, DetectObjects) req = DetectObjectsRequest() req.object_name.data = "anti_grippal" res = detector_service(req) for i in range(0, len(res.object_list.detections)): print str(i) + ": " + res.object_list.detections[i].label index = -1 while (index < 0): index = int(raw_input("Select object to grasp: ")) obj = res.object_list.detections[index].pose obj.header.stamp = self.listener.getLatestCommonTime( "/base_link", obj.header.frame_id) obj = self.listener.transformPose("/base_link", obj) print "Object pose in base_link:", obj object_id = 28 #self.getObjectID(req.object_name.data); print "Calling get_grasps_from_position service..." get_grasps_from_position = rospy.ServiceProxy( 'get_grasps_from_position', GetGraspsFromPosition) req = srs_grasping.srv.GetGraspsFromPositionRequest( object_id, obj.pose) grasp_configuration = ( get_grasps_from_position(req)).grasp_configuration print "get_grasps_from_position service has finished." for i in range(0, len(grasp_configuration)): pre = PoseStamped() pre.header.stamp = rospy.Time.now() pre.header.frame_id = "/base_link" pre.pose.position.x = grasp_configuration[i].pre_grasp.position.x pre.pose.position.y = grasp_configuration[i].pre_grasp.position.y pre.pose.position.z = grasp_configuration[i].pre_grasp.position.z pre.pose.orientation.x = grasp_configuration[ i].pre_grasp.orientation.x pre.pose.orientation.y = grasp_configuration[ i].pre_grasp.orientation.y pre.pose.orientation.z = grasp_configuration[ i].pre_grasp.orientation.z pre.pose.orientation.w = grasp_configuration[ i].pre_grasp.orientation.w g = PoseStamped() g.header.stamp = rospy.Time.now() g.header.frame_id = "/base_link" g.pose.position.x = grasp_configuration[i].grasp.position.x g.pose.position.y = grasp_configuration[i].grasp.position.y g.pose.position.z = grasp_configuration[i].grasp.position.z g.pose.orientation.x = grasp_configuration[i].grasp.orientation.x g.pose.orientation.y = grasp_configuration[i].grasp.orientation.y g.pose.orientation.z = grasp_configuration[i].grasp.orientation.z g.pose.orientation.w = grasp_configuration[i].grasp.orientation.w offset_x = 0 #(g.pose.position.x - pre.pose.position.x)/3 offset_y = 0 #(g.pose.position.y - pre.pose.position.y)/3 offset_z = 0 #(g.pose.position.z - pre.pose.position.z)/3 pre.pose.position.x += offset_x pre.pose.position.y += offset_y pre.pose.position.z -= offset_z g.pose.position.x += offset_x g.pose.position.y += offset_y g.pose.position.z -= offset_z sol = False for w in range(0, 10): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver( current_joint_configuration, pre) if (error_code.val == error_code.SUCCESS): for k in range(0, 10): (grasp_conf, error_code) = grasping_functions.callIKSolver( pre_grasp_conf, g) if (error_code.val == error_code.SUCCESS): print str(i) + ": IK solution found" sol = True break if sol: break if sol: print "PREGRASP: #####################################" print grasp_configuration[i] print "###############################################" res = raw_input("Execute this grasp? (y/n): ") if res != "y": continue else: #grasping_functions.grasp_view(object_id, grasp_configuration[i], obj.pose) # execute grasp handle_say = self.sss.say( ["I am grasping the object now."], False) handle_arm = self.sss.move("arm", [pre_grasp_conf], False) self.sss.move("sdh", "cylopen") handle_say.wait() handle_arm.wait() raw_input("Grasp...") handle_arm2 = self.sss.move("arm", [grasp_conf], False) handle_arm2.wait() raw_input("Catch the object") handle_sdh = self.sss.move( "sdh", [list(grasp_configuration[i].sdh_joint_values)], False) handle_sdh.wait() rospy.sleep(4) """ # place obj on tray handle01 = self.sss.move("arm","grasp-to-tray",False) self.sss.move("tray","up") handle01.wait() self.sss.move("arm","tray") self.sss.move("sdh","cylopen") handle01 = self.sss.move("arm","tray-to-folded",False) self.sss.sleep(4) self.sss.move("sdh","cylclosed",False) handle01.wait() # deliver cup to order position #self.sss.move("base","order") self.sss.say("Here's your drink.") self.sss.move("torso","nod") res = grasping_functions.sdh_tactil_sensor_result() if not res: val = list(grasp_configuration[i].sdh_joint_values) val[2] -= 0.1 val[4] -= 0.1 val[6] -= 0.1 handle_sdh = self.sss.move("sdh", [val], False) handle_sdh.wait() rospy.sleep(4); return 0; """ return 0 return -1
def execute(self): # prepare for grasping #self.sss.move("base","kitchen") self.sss.move("arm","look_at_table") self.sss.move("sdh","cylopen") rospy.sleep(2); #current_joint_configuration sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state) while sub.get_num_connections() == 0: time.sleep(0.3) continue #Detection self.srv_name_object_detection = '/object_detection/detect_object' detector_service = rospy.ServiceProxy(self.srv_name_object_detection, DetectObjects) req = DetectObjectsRequest() req.object_name.data = "anti_grippal" res = detector_service(req) for i in range(0,len(res.object_list.detections)): print str(i)+": "+res.object_list.detections[i].label index = -1; while (index < 0): index = int(raw_input("Select object to grasp: ")) obj = res.object_list.detections[index].pose obj.header.stamp = self.listener.getLatestCommonTime("/base_link", obj.header.frame_id) obj = self.listener.transformPose("/base_link", obj) print "Object pose in base_link:",obj object_id = 28#self.getObjectID(req.object_name.data); print "Calling get_grasps_from_position service..." get_grasps_from_position = rospy.ServiceProxy('get_grasps_from_position', GetGraspsFromPosition) req = srs_grasping.srv.GetGraspsFromPositionRequest(object_id, obj.pose) grasp_configuration = (get_grasps_from_position(req)).grasp_configuration print "get_grasps_from_position service has finished." for i in range(0,len(grasp_configuration)): pre = PoseStamped() pre.header.stamp = rospy.Time.now() pre.header.frame_id = "/base_link" pre.pose.position.x = grasp_configuration[i].pre_grasp.position.x pre.pose.position.y = grasp_configuration[i].pre_grasp.position.y pre.pose.position.z = grasp_configuration[i].pre_grasp.position.z pre.pose.orientation.x = grasp_configuration[i].pre_grasp.orientation.x pre.pose.orientation.y = grasp_configuration[i].pre_grasp.orientation.y pre.pose.orientation.z = grasp_configuration[i].pre_grasp.orientation.z pre.pose.orientation.w = grasp_configuration[i].pre_grasp.orientation.w g = PoseStamped() g.header.stamp = rospy.Time.now() g.header.frame_id = "/base_link" g.pose.position.x = grasp_configuration[i].grasp.position.x g.pose.position.y = grasp_configuration[i].grasp.position.y g.pose.position.z = grasp_configuration[i].grasp.position.z g.pose.orientation.x = grasp_configuration[i].grasp.orientation.x g.pose.orientation.y = grasp_configuration[i].grasp.orientation.y g.pose.orientation.z = grasp_configuration[i].grasp.orientation.z g.pose.orientation.w = grasp_configuration[i].grasp.orientation.w offset_x = 0#(g.pose.position.x - pre.pose.position.x)/3 offset_y = 0#(g.pose.position.y - pre.pose.position.y)/3 offset_z = 0#(g.pose.position.z - pre.pose.position.z)/3 pre.pose.position.x += offset_x pre.pose.position.y += offset_y pre.pose.position.z -= offset_z g.pose.position.x += offset_x g.pose.position.y += offset_y g.pose.position.z -= offset_z sol = False for w in range(0,10): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre) if(error_code.val == error_code.SUCCESS): for k in range(0,10): (grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g) if(error_code.val == error_code.SUCCESS): print str(i)+": IK solution found" sol = True break if sol: break if sol: print "PREGRASP: #####################################" print grasp_configuration[i] print "###############################################" res = raw_input("Execute this grasp? (y/n): ") if res != "y": continue else: #grasping_functions.grasp_view(object_id, grasp_configuration[i], obj.pose) # execute grasp handle_say = self.sss.say(["I am grasping the object now."], False) handle_arm = self.sss.move("arm", [pre_grasp_conf], False) self.sss.move("sdh", "cylopen") handle_say.wait() handle_arm.wait() raw_input("Grasp...") handle_arm2 = self.sss.move("arm", [grasp_conf], False) handle_arm2.wait() raw_input("Catch the object") handle_sdh = self.sss.move("sdh", [list(grasp_configuration[i].sdh_joint_values)], False) handle_sdh.wait() rospy.sleep(4); """ # place obj on tray handle01 = self.sss.move("arm","grasp-to-tray",False) self.sss.move("tray","up") handle01.wait() self.sss.move("arm","tray") self.sss.move("sdh","cylopen") handle01 = self.sss.move("arm","tray-to-folded",False) self.sss.sleep(4) self.sss.move("sdh","cylclosed",False) handle01.wait() # deliver cup to order position #self.sss.move("base","order") self.sss.say("Here's your drink.") self.sss.move("torso","nod") res = grasping_functions.sdh_tactil_sensor_result() if not res: val = list(grasp_configuration[i].sdh_joint_values) val[2] -= 0.1 val[4] -= 0.1 val[6] -= 0.1 handle_sdh = self.sss.move("sdh", [val], False) handle_sdh.wait() rospy.sleep(4); return 0; """ return 0; return -1
def get_grasps_from_position(self, req): x = time.time() rospy.loginfo("/get_grasps_from_position service has been called...") obj_id = req.object_id obj_pose = req.object_pose req = GetGraspConfigurationsRequest() req.object_id = obj_id grasp_configuration = (self.client(req)).grasp_configuration #current_joint_configuration while self.arm_state.get_num_connections() == 0: time.sleep(0.3) continue rotacion = grasping_functions.rotation_matrix(obj_pose) resp = GetGraspsFromPositionResponse() resp.feasible_grasp_available = False resp.grasp_configuration = [] for i in range(0, len(grasp_configuration)): pre_trans = rotacion * grasping_functions.matrix_from_pose( grasp_configuration[i].pre_grasp.pose) grasp_trans = rotacion * grasping_functions.matrix_from_pose( grasp_configuration[i].grasp.pose) t = translation_from_matrix(pre_trans) q = quaternion_from_matrix(pre_trans) tg = translation_from_matrix(grasp_trans) qg = quaternion_from_matrix(grasp_trans) pre = PoseStamped() pre.header.stamp = rospy.Time.now() pre.header.frame_id = "/base_link" pre.pose.position.x = t[0] pre.pose.position.y = t[1] pre.pose.position.z = t[2] pre.pose.orientation.x = q[0] pre.pose.orientation.y = q[1] pre.pose.orientation.z = q[2] pre.pose.orientation.w = q[3] g = PoseStamped() g.header.stamp = rospy.Time.now() g.header.frame_id = "/base_link" g.pose.position.x = tg[0] g.pose.position.y = tg[1] g.pose.position.z = tg[2] g.pose.orientation.x = qg[0] g.pose.orientation.y = qg[1] g.pose.orientation.z = qg[2] g.pose.orientation.w = qg[3] sol = False for w in range(0, self.ik_loop_reply): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver( current_joint_configuration, pre) if (error_code.val == error_code.SUCCESS): for k in range(0, self.ik_loop_reply): (grasp_conf, error_code) = grasping_functions.callIKSolver( pre_grasp_conf, g) if (error_code.val == error_code.SUCCESS): new_valid_grasp = GraspSubConfiguration() new_valid_grasp.sdh_joint_values = grasp_configuration[ i].sdh_joint_values new_valid_grasp.grasp = g.pose new_valid_grasp.pre_grasp = pre.pose new_valid_grasp.category = grasping_functions.get_grasp_category( pre.pose.position, g.pose.position) sol = grasping_functions.valid_grasp( new_valid_grasp) if sol: resp.feasible_grasp_available = True resp.grasp_configuration.append( new_valid_grasp) break if sol: break #for #for #order grasps if len(resp.grasp_configuration) > 0: (resp.grasp_configuration).sort() rospy.loginfo( str(len(resp.grasp_configuration)) + " valid grasps for this pose.") rospy.loginfo("/get_grasps_from_position call has finished.") print "Time employed: " + str(time.time() - x) print "---------------------------------------" return resp
def execute(self, userdata): rospy.loginfo('Executing state GRASP') #Open SDH at the pre-grasp position ----------------------------------------------- #sss.move("sdh", "cylopen") pre_p = userdata.grasp_configuration[userdata.grasp_configuration_id].pre_grasp.position g_p = userdata.grasp_configuration[userdata.grasp_configuration_id].grasp.position category = grasping_functions.get_grasp_category(pre_p , g_p); if category == "TOP" or category == "DOWN": userdata.grasp_categorisation = 'top' arm_handle=sss.move("sdh", "spheropen") else: userdata.grasp_categorisation = 'side' arm_handle=sss.move("sdh", "cylopen") arm_handle.wait() rospy.sleep(2) #Get the current arm joint states. while self.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[userdata.grasp_configuration_id].pre_grasp; #grasp grasp_stamped = PoseStamped(); grasp_stamped.header.frame_id = "/base_link"; grasp_stamped.pose = userdata.grasp_configuration[userdata.grasp_configuration_id].grasp; #postgrasp post_grasp_stamped = copy.deepcopy(grasp_stamped); post_grasp_stamped.pose.position.z += 0.1; #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 """ #global current_joint_configuration sol = False for w in range(0,10): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(self.current_joint_configuration, 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: return 'not_completed'; else: #sss.move("torso","home") arm_handle = sss.move("arm", [pre_grasp_conf, grasp_conf], False) arm_handle.wait(); #Close SDH based on the grasp configuration to grasp. arm_handle = sss.move("sdh", [list(userdata.grasp_configuration[userdata.grasp_configuration_id].sdh_joint_values)], False) arm_handle.wait(); rospy.sleep(2); #TODO: Confirm the grasp based on force feedback successful_grasp = False#grasping_functions.sdh_tactil_sensor_result(); if not successful_grasp: #TODO: Regrasp (close MORE the fingers) regrasp = list(userdata.grasp_configuration[userdata.grasp_configuration_id].sdh_joint_values) print "Current config, trying regrasp", regrasp #regrasp[2] -= 0.1 #regrasp[4] -= 0.1 #regrasp[6] -= 0.1 regrasp[1] += 0.07 regrasp[3] += 0.07 regrasp[5] += 0.07 print "to", regrasp arm_handle = sss.move("sdh", [regrasp], False) arm_handle.wait(); successful_grasp = True#grasping_functions.sdh_tactil_sensor_result(); if not successful_grasp: return 'not_completed' sss.move("arm",[post_grasp_conf,"look_at_table","hold"]) return 'succeeded'
def get_grasps_from_position(self, req): x = time.time(); rospy.loginfo("/get_grasps_from_position service has been called..."); obj_id = req.object_id; obj_pose = req.object_pose; req = GetGraspConfigurationsRequest(); req.object_id = obj_id; grasp_configuration = (self.client(req)).grasp_configuration; #current_joint_configuration rospy.loginfo("Waiting /arm_controller/state..."); sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state); while sub.get_num_connections() == 0: time.sleep(0.3); continue; rospy.loginfo("/arm_controller/state has finished."); rotacion = grasping_functions.rotation_matrix(obj_pose); resp = GetGraspsFromPositionResponse(); resp.feasible_grasp_available = False; resp.grasp_configuration = []; for i in range(0,len(grasp_configuration)): pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose); grasp_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose); t = translation_from_matrix(pre_trans); q = quaternion_from_matrix(pre_trans); tg = translation_from_matrix(grasp_trans); qg = quaternion_from_matrix(grasp_trans); pre = PoseStamped(); pre.header.stamp = rospy.Time.now(); pre.header.frame_id = "/base_link"; pre.pose.position.x = t[0]; pre.pose.position.y = t[1]; pre.pose.position.z = t[2]; pre.pose.orientation.x = q[0]; pre.pose.orientation.y = q[1]; pre.pose.orientation.z = q[2]; pre.pose.orientation.w = q[3]; g = PoseStamped(); g.header.stamp = rospy.Time.now(); g.header.frame_id = "/base_link"; g.pose.position.x = tg[0]; g.pose.position.y = tg[1]; g.pose.position.z = tg[2]; g.pose.orientation.x = qg[0]; g.pose.orientation.y = qg[1]; g.pose.orientation.z = qg[2]; g.pose.orientation.w = qg[3]; offset_x = 0#(g.pose.position.x - pre.pose.position.x)/3 offset_y = 0#(g.pose.position.y - pre.pose.position.y)/3 offset_z = 0#(g.pose.position.z - pre.pose.position.z)/3 pre.pose.position.x += offset_x pre.pose.position.y += offset_y pre.pose.position.z -= offset_z g.pose.position.x += offset_x g.pose.position.y += offset_y g.pose.position.z -= offset_z sol = False; for w in range(0,self.ik_loop_reply): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre); if(error_code.val == error_code.SUCCESS): for k in range(0,self.ik_loop_reply): (grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g); if(error_code.val == error_code.SUCCESS): new_valid_grasp = GraspSubConfiguration(); new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values; new_valid_grasp.grasp = g.pose; new_valid_grasp.pre_grasp = pre.pose; new_valid_grasp.category = grasping_functions.get_grasp_category(pre.pose.position, g.pose.position); sol = grasping_functions.valid_grasp(new_valid_grasp); if sol: resp.feasible_grasp_available = True; resp.grasp_configuration.append(new_valid_grasp); break; if sol: break; #for #for #order grasps if len(resp.grasp_configuration) > 0: (resp.grasp_configuration).sort(); rospy.loginfo(str(len(resp.grasp_configuration))+" valid grasps for this pose."); rospy.loginfo("/get_grasps_from_position call has finished."); print "Time employed: " + str(time.time() - x); print "---------------------------------------"; return resp;
def get_grasps_from_position(self, req): x = time.time() rospy.loginfo("/get_grasps_from_position service has been called...") obj_id = req.object_id obj_pose = req.object_pose req = GetGraspConfigurationsRequest() req.object_id = obj_id grasp_configuration = (self.client(req)).grasp_configuration # current_joint_configuration while self.arm_state.get_num_connections() == 0: time.sleep(0.3) continue rotacion = grasping_functions.rotation_matrix(obj_pose) resp = GetGraspsFromPositionResponse() resp.feasible_grasp_available = False resp.grasp_configuration = [] for i in range(0, len(grasp_configuration)): pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose) grasp_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose) t = translation_from_matrix(pre_trans) q = quaternion_from_matrix(pre_trans) tg = translation_from_matrix(grasp_trans) qg = quaternion_from_matrix(grasp_trans) pre = PoseStamped() pre.header.stamp = rospy.Time.now() pre.header.frame_id = "/base_link" pre.pose.position.x = t[0] pre.pose.position.y = t[1] pre.pose.position.z = t[2] pre.pose.orientation.x = q[0] pre.pose.orientation.y = q[1] pre.pose.orientation.z = q[2] pre.pose.orientation.w = q[3] g = PoseStamped() g.header.stamp = rospy.Time.now() g.header.frame_id = "/base_link" g.pose.position.x = tg[0] g.pose.position.y = tg[1] g.pose.position.z = tg[2] g.pose.orientation.x = qg[0] g.pose.orientation.y = qg[1] g.pose.orientation.z = qg[2] g.pose.orientation.w = qg[3] sol = False for w in range(0, self.ik_loop_reply): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre) if error_code.val == error_code.SUCCESS: for k in range(0, self.ik_loop_reply): (grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g) if error_code.val == error_code.SUCCESS: new_valid_grasp = GraspSubConfiguration() new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values new_valid_grasp.grasp = g.pose new_valid_grasp.pre_grasp = pre.pose new_valid_grasp.category = grasping_functions.get_grasp_category( pre.pose.position, g.pose.position ) sol = grasping_functions.valid_grasp(new_valid_grasp) if sol: resp.feasible_grasp_available = True resp.grasp_configuration.append(new_valid_grasp) break if sol: break # for # for # order grasps if len(resp.grasp_configuration) > 0: (resp.grasp_configuration).sort() rospy.loginfo(str(len(resp.grasp_configuration)) + " valid grasps for this pose.") rospy.loginfo("/get_grasps_from_position call has finished.") print "Time employed: " + str(time.time() - x) print "---------------------------------------" return resp
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);
def execute_cb(self, server_goal): x = time.time() rospy.loginfo("/get_grasps_from_position_server has been called...") obj_id = server_goal.object_id obj = server_goal.object_pose goal = GraspCGoal() goal.object_id = obj_id self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration.from_sec(5.0)) grasp_configuration = (self.client.get_result()).grasp_configuration # current_joint_configuration rospy.loginfo("Waiting /arm_controller/state...") sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state) while sub.get_num_connections() == 0: time.sleep(0.3) continue rospy.loginfo("/arm_controller/state has finished.") rotacion = grasping_functions.rotation_matrix(obj) server_result = GraspFActionResult().result server_result.feasible_grasp_available = False server_result.grasp_configuration = [] for i in range(0, len(grasp_configuration)): pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose) grasp_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose) t = translation_from_matrix(pre_trans) q = quaternion_from_matrix(pre_trans) tg = translation_from_matrix(grasp_trans) qg = quaternion_from_matrix(grasp_trans) pre = PoseStamped() pre.header.stamp = rospy.Time.now() pre.header.frame_id = "/base_link" pre.pose.position.x = t[0] pre.pose.position.y = t[1] pre.pose.position.z = t[2] pre.pose.orientation.x = q[0] pre.pose.orientation.y = q[1] pre.pose.orientation.z = q[2] pre.pose.orientation.w = q[3] g = PoseStamped() g.header.stamp = rospy.Time.now() g.header.frame_id = "/base_link" g.pose.position.x = tg[0] g.pose.position.y = tg[1] g.pose.position.z = tg[2] g.pose.orientation.x = qg[0] g.pose.orientation.y = qg[1] g.pose.orientation.z = qg[2] g.pose.orientation.w = qg[3] offset_x = 0 # (g.pose.position.x - pre.pose.position.x)/3 offset_y = 0 # (g.pose.position.y - pre.pose.position.y)/3 offset_z = 0 # (g.pose.position.z - pre.pose.position.z)/3 pre.pose.position.x += offset_x pre.pose.position.y += offset_y pre.pose.position.z -= offset_z g.pose.position.x += offset_x g.pose.position.y += offset_y g.pose.position.z -= offset_z sol = False for w in range(0, self.ik_loop_reply): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre) if error_code.val == error_code.SUCCESS: for k in range(0, self.ik_loop_reply): (grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g) if error_code.val == error_code.SUCCESS: print i new_valid_grasp = GraspSubConfiguration() new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values new_valid_grasp.grasp = g.pose new_valid_grasp.pre_grasp = pre.pose new_valid_grasp.category = grasping_functions.get_grasp_category( pre.pose.position, g.pose.position ) server_result.feasible_grasp_available = True server_result.grasp_configuration.append(new_valid_grasp) sol = True break if sol: break # for # while # order grasps if len(server_result.grasp_configuration) > 0: (server_result.grasp_configuration).sort() rospy.loginfo(str(len(server_result.grasp_configuration)) + " valid grasps for this pose.") rospy.loginfo("/get_grasps_from_position call has finished.") print "Time employed: " + str(time.time() - x) print "---------------------------------------" self.get_grasps_from_position_server.set_succeeded(server_result)
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'
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'
def execute_cb(self, server_goal): x = time.time() rospy.loginfo("/get_grasps_from_position_server has been called...") obj_id = server_goal.object_id obj = server_goal.object_pose goal = GraspCGoal() goal.object_id = obj_id self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration.from_sec(5.0)) grasp_configuration = (self.client.get_result()).grasp_configuration #current_joint_configuration rospy.loginfo("Waiting /arm_controller/state...") sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state) while sub.get_num_connections() == 0: time.sleep(0.3) continue rospy.loginfo("/arm_controller/state has finished.") rotacion = grasping_functions.rotation_matrix(obj) server_result = GraspFActionResult().result server_result.feasible_grasp_available = False server_result.grasp_configuration = [] for i in range(0, len(grasp_configuration)): pre_trans = rotacion * grasping_functions.matrix_from_pose( grasp_configuration[i].pre_grasp.pose) grasp_trans = rotacion * grasping_functions.matrix_from_pose( grasp_configuration[i].grasp.pose) t = translation_from_matrix(pre_trans) q = quaternion_from_matrix(pre_trans) tg = translation_from_matrix(grasp_trans) qg = quaternion_from_matrix(grasp_trans) pre = PoseStamped() pre.header.stamp = rospy.Time.now() pre.header.frame_id = "/base_link" pre.pose.position.x = t[0] pre.pose.position.y = t[1] pre.pose.position.z = t[2] pre.pose.orientation.x = q[0] pre.pose.orientation.y = q[1] pre.pose.orientation.z = q[2] pre.pose.orientation.w = q[3] g = PoseStamped() g.header.stamp = rospy.Time.now() g.header.frame_id = "/base_link" g.pose.position.x = tg[0] g.pose.position.y = tg[1] g.pose.position.z = tg[2] g.pose.orientation.x = qg[0] g.pose.orientation.y = qg[1] g.pose.orientation.z = qg[2] g.pose.orientation.w = qg[3] offset_x = 0 #(g.pose.position.x - pre.pose.position.x)/3 offset_y = 0 #(g.pose.position.y - pre.pose.position.y)/3 offset_z = 0 #(g.pose.position.z - pre.pose.position.z)/3 pre.pose.position.x += offset_x pre.pose.position.y += offset_y pre.pose.position.z -= offset_z g.pose.position.x += offset_x g.pose.position.y += offset_y g.pose.position.z -= offset_z sol = False for w in range(0, self.ik_loop_reply): (pre_grasp_conf, error_code) = grasping_functions.callIKSolver( current_joint_configuration, pre) if (error_code.val == error_code.SUCCESS): for k in range(0, self.ik_loop_reply): (grasp_conf, error_code) = grasping_functions.callIKSolver( pre_grasp_conf, g) if (error_code.val == error_code.SUCCESS): new_valid_grasp = GraspSubConfiguration() new_valid_grasp.sdh_joint_values = grasp_configuration[ i].sdh_joint_values new_valid_grasp.grasp = g.pose new_valid_grasp.pre_grasp = pre.pose new_valid_grasp.category = grasping_functions.get_grasp_category( pre.pose.position, g.pose.position) sol = grasping_functions.valid_grasp( new_valid_grasp) if sol: server_result.feasible_grasp_available = True server_result.grasp_configuration.append( new_valid_grasp) break if sol: break #for #while #order grasps if len(server_result.grasp_configuration) > 0: (server_result.grasp_configuration).sort() rospy.loginfo( str(len(server_result.grasp_configuration)) + " valid grasps for this pose.") rospy.loginfo("/get_grasps_from_position call has finished.") print "Time employed: " + str(time.time() - x) print "---------------------------------------" self.get_grasps_from_position_server.set_succeeded(server_result)