def execute(self, userdata): global leg_counter leg_counter = leg_counter + 1 #print leg_counter move_hand('open') pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1, latch=True) pubrel.publish(1) rospy.sleep(0.1) pubrel.publish(0) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1, latch=True) hand2.publish(0) rospy.sleep(0.1) pubpiecedone = rospy.Publisher('legdone', Int8, queue_size=1, latch=True) pubpiecedone.publish(0) pubrel.publish(0) rospy.sleep(0.2) if leg_counter >= 4: return 'outcome2' return 'outcome1'
def execute(self, userdata): #global cov #cov.start() #Path planning towards the piece theplans = interface([-0.5,0.0,-0.75,0.0,1.39,0.0,0.0,-0.5,0.0]) for i,plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([-1.15,0.05,-0.75,-1000.0,1.39,-1000.0,-1000.0,-0.5,-1000.0]) for i,plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([-1.15,0.05,-0.48,-1000.0,1.39,-1000.0,-1000.0,-0.5,-1000.0]) for i,plan in enumerate(theplans): set_robot_joints(plan) move_hand('close') rospy.sleep(0.5) hand = rospy.Publisher('robot_gripper', Int8, queue_size=1,latch=True) hand.publish(1) rospy.sleep(0.5) hand.publish(0) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1,latch=True) pubpress.publish(1) rospy.sleep(0.2) #Path planning towards goal location theplans = interface([0.0,0.05,-0.75,0.0,1.39,0.0,0.0,-0.5,0.0]) #print theplans for i,plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1,latch=True) hand2.publish(1) rospy.sleep(0.1) #cov.stop() #cov.save() return 'outcome1'
def execute(self, userdata): theplans = interface( [0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) for i, plan in enumerate(theplans): set_robot_joints(plan) move_hand('close') rospy.sleep(0.5) hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True) hand.publish(1) rospy.sleep(0.5) hand.publish(0) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1, latch=True) pubpress.publish(1) rospy.sleep(0.2) rospy.sleep(1) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1, latch=True) hand2.publish(1) rospy.sleep(0.1) return 'outcome1'
def execute(self, userdata): #global cov #cov.start() # If online the assert_normal value will be set by a publisher subscriber # interaction to force the drop pathway if assert_normal == 1: trigger_drop_pathway = 1 else: # Randomly assign the value trigger_drop_pathway = random.randint(0, 10) theplans = interface( [-0.5, 0.0, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) for i, plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([ -1.15, 0.05, -0.75, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0 ]) for i, plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([ -1.15, 0.05, -0.48, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0 ]) for i, plan in enumerate(theplans): set_robot_joints(plan) move_hand('close') rospy.sleep(0.5) hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True) hand.publish(1) rospy.sleep(0.5) hand.publish(0) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1, latch=True) pubpress.publish(1) rospy.sleep(0.2) #Path planning towards goal location theplans = interface( [0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) #print theplans for i, plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1, latch=True) hand2.publish(1) rospy.sleep(0.1) # Introduce state fork if trigger_drop_pathway == 1: return 'outcome2' else: return 'outcome1'
def execute(self, userdata): #global cov #cov.start() #Path planning towards the piece #x = random.randint(0, 10) trigger_drop_pathway = 2 theplans = interface( [-0.5, 0.0, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) for i, plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([ -1.15, 0.05, -0.75, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0 ]) for i, plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([ -1.15, 0.05, -0.48, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0 ]) for i, plan in enumerate(theplans): set_robot_joints(plan) move_hand('close') rospy.sleep(0.5) hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True) hand.publish(1) rospy.sleep(0.5) hand.publish(0) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1, latch=True) pubpress.publish(1) rospy.sleep(0.2) #Path planning towards goal location theplans = interface( [0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) #print theplans for i, plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1, latch=True) hand2.publish(1) rospy.sleep(0.1) if trigger_drop_pathway == 2: #Will I need to thread this return 'outcome2' #cov.stop() #cov.save() return 'outcome1'
def Reset(): move_hand('close') # Hand raised: targ = [0.5,0.05,-0.77,0.0,0.0,1.4,0.0,-0.17,0.0] targ = fixTarget(targ) theplans = interface(targ) followPlans(theplans) rospy.sleep(4)
def execute(self, userdata): #Announce start of Reset pubReset = rospy.Publisher('robot_reset', Robot, queue_size=1,latch=True) pubReset.publish(1) rospy.sleep(0.1) #Target position of reset: centre of hand in x=0.0, y=0.2, z=0.585 set_robot_joints([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) move_hand('open') rospy.sleep(0.5) #Select random initial position for the robot set_robot_joints([0.0, 0.0, 0.0, 0.0, random.uniform(-0.5,0.5), 0.0, 0.0, random.uniform(-0.5,0.5), 0.0, 0.0, random.uniform(-0.5,0.5),0.0]) return 'outcome1'
def execute(self, userdata): global leg_counter leg_counter = leg_counter + 1 move_hand('open') dropped = rospy.Publisher('dropped_piece', Int8, queue_size=1, latch=True) dropped.publish(1) rospy.sleep(0.2) dropped.publish(0) return 'outcome1'
def execute(self, userdata): #global cov #cov.start() print 'Piece is released' global leg_counter leg_counter = leg_counter + 1 global good_legs good_legs = good_legs + 1 print leg_counter move_hand('open') rospy.sleep(0.5) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1,latch=True) pubpress.publish(0) rospy.sleep(0.1) pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1,latch=True) pubrel.publish(1) rospy.sleep(0.05) pubrel.publish(0) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1,latch=True) hand2.publish(0) rospy.sleep(0.1) pubpiecedone = rospy.Publisher('legdone', Int8, queue_size=1,latch=True) pubpiecedone.publish(1) #pubrel.publish(0) rospy.sleep(0.1) pubpiecedone.publish(0) rospy.sleep(0.1) if leg_counter < 4: #cov.stop() #cov.save() return 'outcome1' else: leg_counter=0 if good_legs >= 4: pubtabledone = rospy.Publisher('tabledone', Int8, queue_size=1,latch=True) pubtabledone.publish(1) rospy.sleep(0.1) pubtableontime = rospy.Publisher('tableontime', Int8, queue_size=1,latch=True) if (time.time()-table_timeout)<=300: pubtableontime.publish(1) rospy.sleep(0.1) print 'Table on time' else: pubtableontime.publish(0) rospy.sleep(0.1) print 'Table not on time' #cov.stop() #cov.save() return 'outcome2'
def execute(self, userdata): print 'Piece is released' move_hand('open') rospy.sleep(0.5) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1,latch=True) pubpress.publish(0) rospy.sleep(0.1) pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1,latch=True) pubrel.publish(1) rospy.sleep(0.1) print 'Interaction done' pubdone = rospy.Publisher('done', Int8, queue_size=1,latch=True) pubdone.publish(1) rospy.sleep(0.1) return 'outcome1'
def execute(self, userdata): #Reset position with hand open #global cov #cov.start() theplans = interface([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) for i,plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) set_robot_joints([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) move_hand('open') rospy.sleep(0.5) global table_timeout table_timeout = time.time() #cov.stop() #cov.save() return 'outcome1'
def execute(self, userdata): global leg_counter leg_counter = leg_counter + 1 move_hand('open') if assert_normal != 1: subprocess.Popen([ "python", "/home/harrison/catkin_ws/src/table_simulator/scripts/bdi_test_generator/drop_loop_run.py", str(file_number) ]) legDrop = rospy.Publisher('Leg_Drop', Int8, queue_size=1, latch=True) legDrop.publish(1) rospy.sleep(1) legDrop.publish(0) return 'outcome1'
def execute(self, userdata): #Reset position with hand open #global cov #cov.start() theplans = interface([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) for i, plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) set_robot_joints( [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) move_hand('open') rospy.sleep(0.5) global table_timeout table_timeout = time.time() #cov.stop() #cov.save() return 'outcome1'
def execute(self, userdata): global leg_counter leg_counter = leg_counter + 1 #print leg_counter move_hand('open') pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1,latch=True) pubrel.publish(1) rospy.sleep(0.1) pubrel.publish(0) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1,latch=True) hand2.publish(0) rospy.sleep(0.1) pubpiecedone = rospy.Publisher('legdone', Int8, queue_size=1,latch=True) pubpiecedone.publish(0) pubrel.publish(0) rospy.sleep(0.2) if leg_counter >= 4: return 'outcome2' return 'outcome1'
def Move(): # Move hand laterally to a point above the piece targ = [1.5,0.05,-0.77,0.0,0.0,1.4,0.0,-0.17,1000.0] targ = fixTarget(targ) theplans = interface(targ) followPlans(theplans) move_hand('open') rospy.sleep(2) # Bring the hand down to the piece targ = [1.5,0.05,-0.67,1000.0,0,1.4,0.0,-0.17,1000.0] targ = fixTarget(targ) theplans = interface(targ) followPlans(theplans) rospy.sleep(3) # Close the hand move_hand('close') rospy.sleep(1) # Raise the hand targ = [1.5,0.05,-0.77,0.0,0.0,1.4,0.0,-0.17,1000.0] targ = fixTarget(targ) theplans = interface(targ) followPlans(theplans) rospy.sleep(1) # Path planning towards goal location targ = [0.0,0.05,-0.77,0.0,0.0,1.4,0.0,-0.17,0.0] targ = fixTarget(targ) theplans = interface(targ) followPlans(theplans) rospy.sleep(5)
def Release(): move_hand('open') rospy.sleep(1)
def execute(self, userdata): #global cov #cov.start() print 'Piece is released' global leg_counter leg_counter = leg_counter + 1 global good_legs good_legs = good_legs + 1 print leg_counter move_hand('open') rospy.sleep(0.5) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1, latch=True) pubpress.publish(0) rospy.sleep(0.1) pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1, latch=True) pubrel.publish(1) rospy.sleep(0.05) pubrel.publish(0) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1, latch=True) hand2.publish(0) rospy.sleep(0.1) pubpiecedone = rospy.Publisher('legdone', Int8, queue_size=1, latch=True) pubpiecedone.publish(1) #pubrel.publish(0) rospy.sleep(0.1) pubpiecedone.publish(0) rospy.sleep(0.1) if leg_counter < 4: #cov.stop() #cov.save() return 'outcome1' else: leg_counter = 0 if good_legs >= 4: pubtabledone = rospy.Publisher('tabledone', Int8, queue_size=1, latch=True) pubtabledone.publish(1) rospy.sleep(0.1) pubtableontime = rospy.Publisher('tableontime', Int8, queue_size=1, latch=True) if (time.time() - table_timeout) <= 300: pubtableontime.publish(1) rospy.sleep(0.1) print 'Table on time' else: pubtableontime.publish(0) rospy.sleep(0.1) print 'Table not on time' #cov.stop() #cov.save() return 'outcome2'