Python PyPR2.moveBodyTo方法代码示例

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveBodyTo [as 别名]
def adjust_to_shooting():
	global last_proximity
	proximity = check_head_proximity()

	if proximity== True and last_proximity==False:
		PyPR2.moveBodyTo(0.0,0.0,(0.65)*PyPR2.getHeadPos()[0],1)
		#PyPR2.moveHeadTo(0.0,y)
	last_proximity = proximity

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveBodyTo [as 别名]
def rotate_robot_to(desired_angle, time_to_reach = None, in_degrees = True):
    global body_reached, body_failed
    max_speed = math.pi/18.0 # (The maximum speed is 10 degrees per second)
    if in_degrees:
        gain = math.pi/180.0
    else:
        gain = 1.0     
    body_reached = False
    body_failed  = False
    tau0         = body_angle(in_degrees = in_degrees)
    d_theta      = gain*(desired_angle - tau0)
    if time_to_reach == None:
        time_to_reach = abs(d_theta/max_speed)    

    PyPR2.moveBodyTo(0.0, 0.0, d_theta, time_to_reach + 2.0) 	

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveBodyTo [as 别名]
def take_robot_to(X, Y, time_to_reach = 5.0):
    global body_reached, body_failed
    body_reached = False
    body_failed  = False
    rp = PyPR2.getRobotPose()
    P0 = rp['position']
    dX = X - P0[0]
    dY = Y - P0[1]
    tau = body_angle(in_degrees = False)
    S   = math.sin(tau)
    C   = math.cos(tau)
    dx =  C*dX + S*dY
    dy = -S*dX + C*dY

    PyPR2.moveBodyTo(dx, dy, 0.0, time_to_reach)

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveBodyTo [as 别名]
def move_robot_to(x, y , tau, time_to_reach = 5.0, in_degrees = True):
    global body_reached, body_failed
    if in_degrees:
        gain = math.pi/180.0
    else:
        gain = 1.0     
    body_reached = False
    body_failed  = False
    rp = PyPR2.getRobotPose()
    p0 = rp['position']
    dx = x - p0[0]
    dy = y - p0[1]

    '''
    o  = quat(rp['orientation'])   # convert to cgkit quaternion
    R  = numpy.linalg.inv(o.toMat3())
    '''
    o  = geo.Orientation_3D(rp['orientation'], representation = 'quaternion')
    R  = numpy.linalg.inv(o.matrix())

    dp = numpy.dot(R.T, vecmat.as_vector([dx, dy, 0.0]))
    tau0 = body_angle(in_degrees = in_degrees)
    PyPR2.moveBodyTo( dp[0], dp[1], gain*(tau - tau0), time_to_reach)

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