Python PyPR2.moveArmWithJointPos方法代码示例

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveArmWithJointPos [as 别名]
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)

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveArmWithJointPos [as 别名]
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)

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveArmWithJointPos [as 别名]
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)    

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveArmWithJointPos [as 别名]
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)

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveArmWithJointPos [as 别名]
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)    

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveArmWithJointPos [as 别名]
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)

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveArmWithJointPos [as 别名]
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']))
#.........这里部分代码省略.........

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveArmWithJointPos [as 别名]
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:
#.........这里部分代码省略.........