def main(): try: pnp.goto_home(0.3, goal_tol=0.01, orientation_tol=0.1) reset_gripper() activate_gripper() # # 255 = closed, 0 = open # gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER # rospy.sleep(1.0) group = pnp.group current_pose = group.get_current_pose().pose allow_replanning = True planning_time = 10 status = pnp.go_to_pose_goal(pnp.q[0], pnp.q[1], pnp.q[2], pnp.q[3], 0.75, 0, current_pose.position.z, allow_replanning, planning_time) rospy.sleep(0.1) print "\n", group.get_current_pose().pose.position # gripper_to_pos(255, 60, 200, False) # GRIPPER TO POSITION 50 except rospy.ROSInterruptException: return except KeyboardInterrupt: return
def execute(self, userdata): global pnp, done_onions, current_state # rospy.loginfo('Executing state: Claim') if userdata.counter >= 50: userdata.counter = 0 return 'timed_out' if len(userdata.color) == 0: return 'not_found' max_index = len(userdata.color) print '\nMax index is = ', max_index pnp.onion_index = 0 for i in range(max_index): if len(userdata.y) >= i: try: if userdata.y[i] > -0.35 and userdata.y[i] < 0.25: pnp.target_location_x = userdata.x[i] pnp.target_location_y = userdata.y[i] pnp.target_location_z = userdata.z[i] pnp.onion_color = userdata.color[i] pnp.onion_index = i self.is_updated = True break else: done_onions += 1 print '\nDone onions = ', done_onions except IndexError: pass else: print '\nSort complete!' return 'completed' if max_index == done_onions: print '\nAll onions are sorted!' return 'completed' else: done_onions = 0 if self.is_updated == False: userdata.counter += 1 return 'not_updated' else: print '\n(x,y,z) after claim: ',pnp.target_location_x,pnp.target_location_y,pnp.target_location_z reset_gripper() activate_gripper() userdata.counter = 0 current_state = vals2sid(ol=0, eefl=3, pred=2, listst=2) rospy.sleep(0.05) return 'updated'
def main(): try: limb = 'right' hover_distance = 0.15 # meters # Starting Joint angles for right arm starting_joint_angles = { 'right_j0': -0.041662954890248294, 'right_j1': -1.0258291091425074, 'right_j2': 0.0293680414401436, 'right_j3': 2.17518162913313, 'right_j4': -0.06703022873354225, 'right_j5': 0.3968371433926965, 'right_j6': 1.7659649178699421 } pnp = PickAndPlace(limb) # An orientation for gripper fingers to be overhead and parallel to the obj overhead_orientation = Quaternion(x=0.7071057, y=-0.7071029, z=0.0023561, w=-0.0012299) # Move to the desired starting angles print("Running. Ctrl-c to quit") pnp.move_to_start(starting_joint_angles) rospy.sleep(1) reset_gripper() activate_gripper() # 255 = closed, 0 = open gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER sleep(1.0) pnp.go_to_pose_goal( 0.7071029, 0.7071057, 0.0012299, 0.0023561, # GO TO WAYPOINT 1 (HOVER POS) 0.665, 0.0, 0.15) sleep(1.0) pnp.go_to_pose_goal( 0.7071029, 0.7071057, 0.0012299, 0.0023561, # GO TO WAYPOINT 2 (HOVER POS) 0.665, 0.0, 0.002) sleep(1.0) pnp.go_to_pose_goal( 0.7071029, 0.7071057, 0.0012299, 0.0023561, # GO TO WAYPOINT 3 (PLUNGE AND PICK) 0.665, 0.0, -0.001) sleep(1.0) gripper_to_pos(50, 60, 200, False) # GRIPPER TO POSITION 50 os.system('rosrun gazebo_ros_link_attacher attach.py' ) # ATTACH CUBE AND SAWYER EEF sleep(1.0) pnp.go_to_pose_goal( 0.7071029, 0.7071057, 0.0012299, 0.0023561, # GO TO WAYPOINT 4 (TRAVEL TO PLACE DESTINATION) 0.665, 0.04, 0.15) sleep(1.0) pnp.go_to_pose_goal( 0.7071029, 0.7071057, 0.0012299, 0.0023561, # GO TO WAYPOINT 5 (TRAVEL TO PLACE DESTINATION) 0.665, 0.5, 0.02) sleep(1.0) pnp.go_to_pose_goal( 0.7071029, 0.7071057, 0.0012299, 0.0023561, # GO TO WAYPOINT 6 (PLACE) 0.665, 0.5, -0.055) os.system('rosrun gazebo_ros_link_attacher detach.py' ) # DETACH CUBE AND SAWYER EEF gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER sleep(1.0) pnp.go_to_pose_goal( 0.7071029, 0.7071057, 0.0012299, 0.0023561, # GO TO WAYPOINT 7 (RETURN TO HOVER POS) 0.665, 0.5, 0.12) rospy.sleep(1.0) delete_gazebo_models() except rospy.ROSInterruptException: return except KeyboardInterrupt: return
def main(): global target_location_x,target_location_y,pnp,location_EE,\ ConveyorWidthRANGE_LOWER_LIMIT, onion_attached, onion_index,\ ObjectPoseZ_RANGE_UPPER_LIMIT, location_claimed_onion,\ SAWYERRANGE_UPPER_LIMIT, gap_needed_to_pick ConveyorWidthRANGE_LOWER_LIMIT = rospy.get_param( "/ConveyorWidthRANGE_LOWER_LIMIT") ObjectPoseZ_RANGE_UPPER_LIMIT = rospy.get_param( "/ObjectPoseZ_RANGE_UPPER_LIMIT") SAWYERRANGE_UPPER_LIMIT = rospy.get_param("/SAWYERRANGE_UPPER_LIMIT") gap_needed_to_pick = rospy.get_param("/gap_needed_to_pick") reset_gripper() activate_gripper() gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER rospy.Subscriber("cylinder_blocks_poses", cylinder_blocks_poses, callback_poses) rospy.Subscriber("current_cylinder_blocks", Int8MultiArray, callback_modelname) # attach and detach service attach_srv.wait_for_service() detach_srv.wait_for_service() req.link_name_1 = "base_link" req.model_name_2 = "sawyer" req.link_name_2 = "right_l6" # pub_location_EE = rospy.Publisher('location_EE', Int64, queue_size=10) # pub_location_Onion = rospy.Publisher('location_claimed_onion', Int64, queue_size=10) srv_moverobot = rospy.Service('move_sawyer', move_robot, handle_move_sawyer) srv_updatestate = rospy.Service('update_state', update_state, handle_update_state) print "Ready to move sawyer." # rospy.spin() last_location_EE = -1 last_location_claimed_onion = -1 str_locEE = "Unknown" str_locO = "Unknown" # deciding orientation for roll action # global pnp # goto_home(0.3) # joint_goal = pnp.group.get_current_joint_values() # joint_goal[5]=joint_goal[5]-1.2 # joint_goal[6]=joint_goal[6]-1.57 # pnp.go_to_joint_goal(joint_goal,False,0.5,\ # goal_tol=0.02,orientation_tol=0.02) # sleep(5.0) # current_pose = pnp.group.get_current_pose() # q_local=[0,0,0,0] # q_local[0] = current_pose.pose.orientation.x # q_local[1] = current_pose.pose.orientation.y # q_local[2] = current_pose.pose.orientation.z # q_local[3] = current_pose.pose.orientation.w # print "q_local:" # print q_local rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): global location_EE # print "onion_index,location_EE:"\ # +str((onion_index,location_EE)) loc = location_EE if loc == 0: str_locEE = "Conv" else: if loc == 1: str_locEE = "InFront" else: if loc == 2: str_locEE = "AtBin" else: if loc == 3: str_locEE = "Home" else: str_locEE = "Unknown" if location_EE != last_location_EE: print "str_locEE:" + str_locEE # pub_location_EE.publish(location_EE) last_location_EE = location_EE rate.sleep() #### For refering to what param values worked first time ##### # print "goto_home()" # goto_home(0.4,goal_tol=0.1,\ # orientation_tol=0.1) ## PICKING ## # hovertime=1.0 # print "hover()" # success=hover(hovertime) # print "lowergripper()" # reset_gripper() # activate_gripper() # gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER # lowergripper() # sleep(5.0) # attach_srv.call(req) # sleep(2.0) # print "liftgripper()" # liftgripper() # sleep(2.0) ## INPECTION ## # print "lookNrotate()" # lookNrotate() ## PLACING ## # print "goto_bin()" # goto_bin() # sleep(5.0) # detach_srv.call(req) ## ROLLING ## # print "hover() on leftmost onion" # hover() # sleep(2.0) # print "roll()" # roll() # sleep(50.0) return
def execute(self, userdata): global pnp, total_onions, done_onions while len(userdata.x) == 0: rospy.sleep(0.1) # rospy.loginfo('Executing state: Claim') if userdata.counter >= 50: userdata.counter = 0 return 'timed_out' if len(userdata.color) == 0: return 'not_found' max_index = len(userdata.color) # print("I'm here 1") # # attach and detach service # attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) # attach_srv.wait_for_service() # detach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach) # detach_srv.wait_for_service() # pnp.req.model_name_1 = None # pnp.req.link_name_1 = "base_link" # pnp.req.model_name_2 = "sawyer" # pnp.req.link_name_2 = "right_l6" pnp.target_location_x = userdata.x[pnp.onion_index] pnp.target_location_y = userdata.y[pnp.onion_index] pnp.target_location_z = userdata.z[pnp.onion_index] # pnp.bad_onions = [i for i in range(pnp.onion_index, max_index) if (oi.color[i] == 0)] # print("Bad onion indices are: ", pnp.bad_onions) for i in range(total_onions): if len(done_onions) == total_onions: return 'completed' if i in done_onions: pass else: onion_guess = "onion_" + str(i) # 255 = closed, 0 = open gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER rospy.sleep(0.1) # print("I'm here 2") self.is_updated = True # rospy.wait_for_service('/gazebo/get_model_state') # try: # model_coordinates = rospy.ServiceProxy( # '/gazebo/get_model_state', GetModelState) # # print 'Onion name guessed: ', onion_guess # onion_coordinates = model_coordinates(onion_guess, "").posepnp.req.model_name_1 # except rospy.ServiceException, e: # print "Service call failed: %s" % e # if pnp.target_location_y - 0.05 <= onion_coordinates.position.y <= pnp.target_location_y + 0.05: # if pnp.target_location_x - 0.05 <= onion_coordinates.position.x <= pnp.target_location_x + 0.05: # pnp.req.model_name_1 = onion_guess # done_onions.append(i) # self.is_updated = True # break if self.is_updated == False: userdata.counter += 1 return 'not_updated' else: # print "Onion name set as: ", onion_guess reset_gripper() activate_gripper() userdata.counter = 0 rospy.sleep(0.05) return 'updated'
def execute(self, userdata): global pnp, done_onions, current_state # rospy.loginfo('Executing state: Claim') if userdata.counter >= 50: userdata.counter = 0 return 'timed_out' if len(userdata.color) == 0: return 'not_found' max_index = len(userdata.color) print '\nMax index is = ', max_index pnp.onion_index = 0 for i in range(max_index): if len(userdata.y) >= i: # The number of onions found is >= the index value I'm considering sorting now. try: if userdata.y[i] > -0.35 and userdata.y[i] < 0.25: pnp.target_location_x = userdata.x[i] pnp.target_location_y = userdata.y[i] pnp.target_location_z = userdata.z[i] pnp.onion_color = userdata.color[i] pnp.onion_index = i self.is_updated = True break else: done_onions += 1 print '\nDone onions = ', done_onions except IndexError: pass else: print '\nCompleted this batch, moving on!' if max_index == done_onions: # print '\nAll onions are sorted!' msg = Empty() rospy.sleep(0.1) self.conv_pub.publish(msg) rospy.sleep(3.25) # With the conveyor speed controller knob at 40, takes around 3 secs to move to next batch. self.conv_pub.publish(msg) rospy.sleep(0.5) userdata.x = [] userdata.y = [] userdata.z = [] userdata.color = [] userdata.counter = 0 max_index = 0 done_onions = 0 return 'move' else: done_onions = 0 if self.is_updated == False: userdata.counter += 1 return 'not_updated' else: print '\n(x,y,z) after claim: ',pnp.target_location_x,pnp.target_location_y,pnp.target_location_z reset_gripper() activate_gripper() userdata.counter = 0 current_state = vals2sid(ol=0, eefl=3, pred=2, listst=2) rospy.sleep(0.05) return 'updated'