def timerActions( id ): global a,b,x, y , last_x, tracking_data, csvFile, csvFileCounter,LostConnectionCounter,NumPeople, msgTryTimer, NEW_INTERACTION_INITIALISER if msgTryTimer == id : b +=1 vel_x = x - last_x last_x = x if vel_x < -0.01: PyPR2.moveArmWithJointPos(**initial_left) a+=1 tracking_data.append((x,y,x,y,time.time(),NumPeople,"empty",1,LostConnectionCounter)) #b +=1 #updateCsv() with open(csvFile, "w") as output: writer = csv.writer(output, lineterminator='\n') writer.writerows(tracking_data)
def alt_bow_arrow(): PyPR2.openGripper(2) PyPR2.moveHeadTo(0.0,0.15) PyPR2.moveArmWithJointPos(**left_shooting) PyPR2.openGripper(2) time.sleep(2) PyPR2.moveHeadTo(0.0,0.0) PyPR2.moveArmWithJointPos(**alt_right_intermediate) PyPR2.moveHeadTo(0.0,0.3) time.sleep(3) PyPR2.moveArmWithJointPos(**alt_right_shooting) time.sleep(3) PyPR2.closeGripper(2) time.sleep(2) PyPR2.moveHeadTo(0.0,0.18) PyPR2.moveArmWithJointPos(**best_pullback_right) time.sleep(5) PyPR2.moveHeadTo(0.0,0.1) PyPR2.openGripper(2) #PyPR2.moveHeadTo(0.0,0.0) PyPR2.moveArmWithJointPos(**alt_right_release) PyPR2.moveHeadTo(0.0,0.1)
def twist_forearm(angle = 180.0, is_left_arm = True): if is_left_arm: joint_name = 'l_forearm_roll_joint' else: joint_name = 'r_forearm_roll_joint' jd = PyPR2.getArmJointPositions(is_left_arm) jd[joint_name] = jd[joint_name] + angle*math.pi/180.0 PyPR2.moveArmWithJointPos(**jd)
def play(self): if not self.isarm_finished or not self.isgripper_finished: print "still playing action '%s'" % self.name return if self.arm_action: if isinstance(self.arm_action, list): PyPR2.moveArmWithJointTrajectory(self.arm_action) else: PyPR2.moveArmWithJointPos(**(self.arm_action)) self.isarm_finished = False if self.grip_action != None: if self.is_left: PyPR2.setGripperPosition(1, self.grip_action) else: PyPR2.setGripperPosition(2, self.grip_action) self.isgripper_finished = False
def timerActions(id): global a, b, x, y, last_x, tracking_data, csvFile, csvFileCounter, LostConnectionCounter, NumPeople, msgTryTimer, NEW_INTERACTION_INITIALISER if msgTryTimer == id: b += 1 vel_x = x - last_x last_x = x if vel_x < -0.01: PyPR2.moveArmWithJointPos(**initial_left) a += 1 tracking_data.append((x, y, x, y, time.time(), NumPeople, "empty", 1, LostConnectionCounter)) #b +=1 #updateCsv() with open(csvFile, "w") as output: writer = csv.writer(output, lineterminator='\n') writer.writerows(tracking_data)
def play(self): if not self.action_finishes: print "still playing action '%s'." % self.name return myaction = PyPR2.getArmJointPositions(self.isleft) myaction['time_to_reach'] = 1.0 rot = 0.0 if self.clockwise: rot = math.pi / 2.0 else: rot = -math.pi / 2.0 if self.isleft == True: myaction[ 'l_wrist_roll_joint'] = myaction['l_wrist_roll_joint'] + rot else: myaction[ 'r_wrist_roll_joint'] = myaction['r_wrist_roll_joint'] + rot PyPR2.moveArmWithJointPos(**myaction) self.action_finishes = False
def finishsArmAction(self, is_left): self.count = self.count + 1 if self.count == self.revolution * 4: self.count = 0 self.action_finishes = True return myaction = PyPR2.getArmJointPositions(self.isleft) myaction['time_to_reach'] = 1.0 rot = 0.0 if self.clockwise: rot = math.pi / 2.0 else: rot = -math.pi / 2.0 if self.isleft == True: myaction[ 'l_wrist_roll_joint'] = myaction['l_wrist_roll_joint'] + rot else: myaction[ 'r_wrist_roll_joint'] = myaction['r_wrist_roll_joint'] + rot PyPR2.moveArmWithJointPos(**myaction)
def alt_bow_arrow(): PyPR2.openGripper(2) PyPR2.moveHeadTo(0.0, 0.15) PyPR2.moveArmWithJointPos(**left_shooting) PyPR2.openGripper(2) time.sleep(2) PyPR2.moveHeadTo(0.0, 0.0) PyPR2.moveArmWithJointPos(**alt_right_intermediate) PyPR2.moveHeadTo(0.0, 0.3) time.sleep(3) PyPR2.moveArmWithJointPos(**alt_right_shooting) time.sleep(3) PyPR2.closeGripper(2) time.sleep(2) PyPR2.moveHeadTo(0.0, 0.18) PyPR2.moveArmWithJointPos(**best_pullback_right) time.sleep(5) PyPR2.moveHeadTo(0.0, 0.1) PyPR2.openGripper(2) #PyPR2.moveHeadTo(0.0,0.0) PyPR2.moveArmWithJointPos(**alt_right_release) PyPR2.moveHeadTo(0.0, 0.1)
def take_larm_to(qd, time_to_reach = 5.0): global larm_reached, larm_failed larm_reached = False larm_failed = False g = gen_larm_joint_dict(qd, time_to_reach) PyPR2.moveArmWithJointPos(**g)
def twist_rg(angle = 180.0): jd = PyPR2.getArmJointPositions(False) jd['r_wrist_roll_joint'] = jd['r_wrist_roll_joint'] + angle*math.pi/180.0 PyPR2.moveArmWithJointPos(**jd)
def refill(): PyPR2.moveArmWithJointPos(**left_intermediate) PyPR2.moveHeadTo(0.1,0.3) time.sleep(3) PyPR2.moveArmWithJointPos(**right_refill) PyPR2.moveHeadTo(-0.7,0.3) time.sleep(3) PyPR2.moveHeadTo(-0.2,0.3) PyPR2.moveArmWithJointPos(**left_refill) time.sleep(4) PyPR2.moveArmWithJointPos(**right_pick) PyPR2.moveHeadTo(-0.2,0.19) time.sleep(2) PyPR2.closeGripper(2) time.sleep(3) PyPR2.moveArmWithJointPos(**right_pick_further) time.sleep(2) PyPR2.moveArmWithJointPos(**left_pick_further) time.sleep(4) PyPR2.moveArmWithJointPos(**r2) PyPR2.moveArmWithJointPos(**l2)
def timerActions( id ): global msgTryTimer,busy_moving,track_d,last_action_counter,d,start_time,x,y,elapsed_time,focus_obj,a if msgTryTimer == id : #PyPR2.removeTimer( msgTryTimer ) #msgTryTimer += 1 #while True: #time.sleep(1) if abs(a-focus_obj['est_pos'][0]) < 0.04: a = focus_obj['est_pos'][0] PyPR2.moveBodyTo(0.1,0.0,0.0,1) else: a = focus_obj['est_pos'][0] d = math.sqrt(math.pow(x,2)+math.pow(y,2)) adjust_to_shooting() ''' with open(csvfile, "w") as output: writer = csv.writer(output, lineterminator='\n') writer.writerows(track_d) ''' #PyPR2.moveTorsoBy(0.03,5) #if abs(previous_pos - focus_obj['est_pos'][0])< 0.1: # PyPR2.moveHeadTo(0.2,1.0) #adjust_to_shooting() #if busymoving>0: busymoving-=1 if d>=3.5: if last_action_counter <4 and last_action_counter!=0: #obj.larm_reference = True #obj.arm_down() PyPR2.moveArmWithJointPos(**right_high_five) PyPR.moveArmWithJointPos(**shooting_back_down) last_action_counter=4 #busymoving=3 elif last_action_counter == 4: PyPR2.moveTorsoBy(0.1,2) PyPR2.say("Move Back") #busymoving=5 else: PyPR2.moveArmWithJointPos(**left_shooting) last_action_counter=4 #busymoving=5 elif d<=3.5 and d>3: if last_action_counter <3 and last_action_counter!=0: PyPR2.moveArmWithJointPos(**shooting_down) #obj.larm_reference = False #obj.arm_right() #PyPR2.moveBodyTo(0.01,0.0,0.0,1) last_action_counter=3 elif last_action_counter ==3: PyPR2.moveBodyTo(0.07,0.0,0.0,0.51) #busymoving=10 elif last_action_counter ==0: PyPR2.moveArmWithJointPos(**left_shooting) PyPR2.moveArWithJointPos(**alt_right_shooting) #busymoving=10 PyPR2.say("Move Back") else: PyPR2.moveArmWithJointPos(**alt_right_shooting) PyPR2.moveArmWithJointPos(**left_shooting) last_action_counter=3 elif d<=3 and d >2: if last_action_counter <2 and last_action_counter!=0: #obj.larm_reference = False #obj.arm_right() PyPR2.moveArmWithJointPos(**left_relax1) last_action_counter=2 #elif last_action_counter==0: else: PyPR2.moveArmWithJointPos(**alt_right_shooting) PyPR2.moveArmWithJointPos(**left_shooting) PyPR2.closeGripper(2) #PyPR2.closeGripper(2) last_action_counter =2 #previous_pos = focus_obj['est_pos'][0] #movement_tracker.append(str(CONDITION_TAG)+":"+str(focus_obj['est_pos'])) elif d <2 and d>1 : if last_action_counter ==1: PyPR2.say("Move") last_action_counter=1 #busymoving=10 elif last_action_counter >=2: PyPR2.moveArmWithJointPos(**left_shooting) PyPR2.moveArmWithJointPos(**alt_right_shooting) else : PyPR2.moveArmWithJointPos(**alt_right_shooting) PyPr2.moveArmWithJointPos(**left_shooting)
def onHumanTracking(tracking_objs): global HUMAN_COUNTER, st_time,msgTryTimer,x,y,Numpeople,avg_y,avg_x,actionIdentifier,isNearest #focus_obj = tracking_objs[object_inde x] Numpeople = len(tracking_objs) avgPos(tracking_objs) if len(tracking_objs) == 0: #PyPR2.cancelMoveBodyAction() if HUMAN_COUNTER !=0: PyPR2.removeTimer(msgTryTimer) PyPR2.moveArmWithJointPos(**initial_left) PyPR2.moveArmWithJointPos(**initial_right) #msgTryTimer = -1 #elapsed_time = time.time()-st_time HUMAN_COUNTER =0 elif len(tracking_objs) > 0: if HUMAN_COUNTER ==0: PyPR2.onTimer = timerActions msgTryTimer = PyPR2.addTimer(1,-1,0.5) #a +=1 #st_time = time.time() #no_objTracker.append(elapsed_time) HUMAN_COUNTER= Numpeople object_index = closest_obj_index(tracking_objs) focus_obj = tracking_objs[object_index] x = focus_obj['est_pos'][0] y = focus_obj['est_pos'][1] d = math.sqrt((math.pow(x,2))+(math.pow(y,2))) if d<0.1: PyPR2.cancelMoveBodyAction() PyPR2.cancelMoveArmAction(True) PyPR2.cancelMoveArmAction(False) #c+=1 # track_y.append(y) mid_x = focus_obj['bound'][0] + focus_obj['bound'][2] / 2 mid_y = focus_obj['bound'][1] + focus_obj['bound'][3] / 2 #print "track obj {} mid pt ({}.{})".format(focus_obj['track_id'],mid_x,mid_y) ofs_x = mid_x - 320 ofs_y = mid_y - 240 chx = chy = 0.0 if math.fabs(ofs_x) > 10: chx = -ofs_x * 90.0 / 640 * 0.01745329252 #head_yaw_list.append(chx) if math.fabs(ofs_y) > 10: chy = ofs_y * 90.0 / 640 * 0.01745329252 PyPR2.updateHeadPos( chx, chy ) if Numpeople ==1: if x>3.5: if isNearest == False: if y>0: PyPR2.moveArmWithJointPos(**initial_left) actionIdentifier= "initial_left" else: PyPR2.moveArmWithJointPos(**initial_right) actionIdentifier= "initial_right" elif 2.5<x<3.5: PyPR2.moveTorsoBy(0.1,2) if y>0: PyPR2.moveArmWithJointPos(**second_left) actionIdentifier = "second_left" else: PyPR2.moveArmWithJointPos(**second_right) actionIdentifier = "second_right" elif 1.5<x<2.5: PyPR2.moveTorsoBy(0.1,2) isNearest = True if y>0: PyPR2.moveArmWithJointPos(**third_left) actionIdentifier = "third_left" else: PyPR2.moveArmWithJointPos(**third_right) actionIdentifier = "third_right" else: isNearest = True PyPR2.moveArmWithJointPos(**full_stretch_left) PyPR2.moveArmWithJointPos(**full_stretch_right) actionIdentifier = "full_stretch" if 1<Numpeople<3: if x>3.5: if isNearest ==False: PyPR2.moveArmWithJointPos(**initial_left) PyPR2.moveArmWithJointPos(**initial_right) actionIdentifier = "Both_initial" elif 2.5<x<3.5: PyPR2.moveArmWithJointPos(**second_left) PyPR2.moveArmWithJointPos(**second_right) PyPR2.moveTorsoBy(0.1,2) actionIdentifier = "Multiple_Second" elif 1.5<x<2.5: isNearest = True PyPR2.moveArmWithJointPos(**third_right) PyPR2.moveArmWithJointPos(**third_left) PyPR2.moveTorsoBy(0.1,2) actionIdentifier = "Multiple_third" else: isNearest = True PyPR2.moveArmWithJointPos(**full_stretch_left) PyPR2.moveArmWithJointPos(**full_stretch_right) actionIdentifier = "Full_Multiple" elif Numpeople > 3: isNearest = True PyPR2.moveArmWithJointPos(**full_stretch_left) PyPR2.moveArmWithJointPos(**full_stretch_right) actionIdentifier = "TooManyPeopleBehaviour"
def refill(): PyPR2.moveArmWithJointPos(**left_intermediate) PyPR2.moveHeadTo(0.1, 0.3) time.sleep(3) PyPR2.moveArmWithJointPos(**right_refill) PyPR2.moveHeadTo(-0.7, 0.3) time.sleep(3) PyPR2.moveHeadTo(-0.2, 0.3) PyPR2.moveArmWithJointPos(**left_refill) time.sleep(4) PyPR2.moveArmWithJointPos(**right_pick) PyPR2.moveHeadTo(-0.2, 0.19) time.sleep(2) PyPR2.closeGripper(2) time.sleep(3) PyPR2.moveArmWithJointPos(**right_pick_further) time.sleep(2) PyPR2.moveArmWithJointPos(**left_pick_further) time.sleep(4) PyPR2.moveArmWithJointPos(**r2) PyPR2.moveArmWithJointPos(**l2)
def timerActions(id): global msgTryTimer, busy_moving, track_d, last_action_counter, d, start_time, x, y, elapsed_time, focus_obj, a if msgTryTimer == id: #PyPR2.removeTimer( msgTryTimer ) #msgTryTimer += 1 #while True: #time.sleep(1) if abs(a - focus_obj['est_pos'][0]) < 0.04: a = focus_obj['est_pos'][0] PyPR2.moveBodyTo(0.1, 0.0, 0.0, 1) else: a = focus_obj['est_pos'][0] d = math.sqrt(math.pow(x, 2) + math.pow(y, 2)) adjust_to_shooting() ''' with open(csvfile, "w") as output: writer = csv.writer(output, lineterminator='\n') writer.writerows(track_d) ''' #PyPR2.moveTorsoBy(0.03,5) #if abs(previous_pos - focus_obj['est_pos'][0])< 0.1: # PyPR2.moveHeadTo(0.2,1.0) #adjust_to_shooting() #if busymoving>0: busymoving-=1 if d >= 3.5: if last_action_counter < 4 and last_action_counter != 0: #obj.larm_reference = True #obj.arm_down() PyPR2.moveArmWithJointPos(**right_high_five) PyPR.moveArmWithJointPos(**shooting_back_down) last_action_counter = 4 #busymoving=3 elif last_action_counter == 4: PyPR2.moveTorsoBy(0.1, 2) PyPR2.say("Move Back") #busymoving=5 else: PyPR2.moveArmWithJointPos(**left_shooting) last_action_counter = 4 #busymoving=5 elif d <= 3.5 and d > 3: if last_action_counter < 3 and last_action_counter != 0: PyPR2.moveArmWithJointPos(**shooting_down) #obj.larm_reference = False #obj.arm_right() #PyPR2.moveBodyTo(0.01,0.0,0.0,1) last_action_counter = 3 elif last_action_counter == 3: PyPR2.moveBodyTo(0.07, 0.0, 0.0, 0.51) #busymoving=10 elif last_action_counter == 0: PyPR2.moveArmWithJointPos(**left_shooting) PyPR2.moveArWithJointPos(**alt_right_shooting) #busymoving=10 PyPR2.say("Move Back") else: PyPR2.moveArmWithJointPos(**alt_right_shooting) PyPR2.moveArmWithJointPos(**left_shooting) last_action_counter = 3 elif d <= 3 and d > 2: if last_action_counter < 2 and last_action_counter != 0: #obj.larm_reference = False #obj.arm_right() PyPR2.moveArmWithJointPos(**left_relax1) last_action_counter = 2 #elif last_action_counter==0: else: PyPR2.moveArmWithJointPos(**alt_right_shooting) PyPR2.moveArmWithJointPos(**left_shooting) PyPR2.closeGripper(2) #PyPR2.closeGripper(2) last_action_counter = 2 #previous_pos = focus_obj['est_pos'][0] #movement_tracker.append(str(CONDITION_TAG)+":"+str(focus_obj['est_pos'])) elif d < 2 and d > 1: if last_action_counter == 1: PyPR2.say("Move") last_action_counter = 1 #busymoving=10 elif last_action_counter >= 2: PyPR2.moveArmWithJointPos(**left_shooting) PyPR2.moveArmWithJointPos(**alt_right_shooting) else: PyPR2.moveArmWithJointPos(**alt_right_shooting) PyPr2.moveArmWithJointPos(**left_shooting)