Python ALProxy.getSummary方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getSummary [as 别名]
def main(ip, port):
    motion = ALProxy("ALMotion", ip, port)

    motion.wakeUp()
    print motion.getSummary()
    time.sleep(2)

    motion.openHand('LHand')
    time.sleep(2)

    motion.closeHand('LHand')
    time.sleep(2)

    motion.rest()
    print motion.getSummary()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getSummary [as 别名]
class NaoMonkey:
	PART = {
		'LShoulder': ['LShoulderPitch', 'LShoulderRoll'],
		'RShoulder': ['RShoulderPitch', 'RShoulderRoll'],
		'LElbow': ['LElbowYaw', 'LElbowRoll'],
		'RElbow': ['RElbowYaw', 'RElbowRoll'],
		'Head': ['HeadYaw', 'HeadPitch'],
	}
	LIMITS = {
		'Head': [[-2.0, 2.0], [-0.67, 0.51]],
		'LShoulder': [[-2.0, 2.0], [-0.31, 1.32]],
		'RShoulder': [[-2.0, 2.0], [-1.32, 0.31]],
		'LElbow': [[-2.0, 2.0], [-1.54, -0.03]],
		'RElbow': [[-2.0, 2.0], [0.03, 1.54]],
	}

	def __init__(self):
		rospy.init_node('nao_mykinect', anonymous=True)

		self.listener = rospy.Subscriber('nao', NaoCoords, self.move)
                ip = rospy.get_param('~ip', '10.104.16.141')
		port = int(rospy.get_param('~port', '9559'))

		self.al = ALProxy("ALAutonomousLife", ip, port)
		self.postureProxy = ALProxy("ALRobotPosture", ip, port)
		self.motionProxy = ALProxy("ALMotion", ip, port)
		
		self.al.setState("disabled")
		for part in ["Head", "LArm", "RArm"]:
			self.motionProxy.setStiffnesses(part, 1.0)
		rospy.loginfo(self.motionProxy.getSummary())

	def move(self, coords):
		part = coords.Part.data
		angles1 = coords.Angles1
		angles2 = coords.Angles2
		angles = [float(angles1.data), float(angles2.data)]
		speed = 1.0
		if part not in NaoMonkey.PART:
			error_msg = 'Wat? I Do not have ' + str(part)
			rospy.loginfo(error_msg)
			return
		if len(NaoMonkey.PART[part]) != len(angles):
			error_msg = 'Wat? What shall i do with rest joint?'
			rospy.loginfo(error_msg)
			return
		angles = map(lambda x: float(x)*math.pi/180.0, angles)
		for limit, angle in zip(NaoMonkey.LIMITS[part], angles):
			if angle < limit[0] or angle > limit[1]:
				error_msg = 'Wat? Limits man!'
				rospy.loginfo(error_msg)
		self.motionProxy.setAngles(NaoMonkey.PART[part], angles, speed);

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getSummary [as 别名]
				moveheadGoRight()
			elif (keyboard == "43"):#r,l,u,d
				#newpos="43"
				moveheadGoUp()
			elif (keyboard == "44"):#r,l,u,d
				#newpos="44"
				moveheadGoDown()
			elif (keyboard == "50"):
				print"stop"
				postureProxy.stopMove()
			elif (keyboard == "51"):
				print "rest"
				motionProxy.rest()
			elif (keyboard == "52"):
				print "::SUMMARY::"
				print motionProxy.getSummary()
			elif (keyboard == "60"):
				getNaoNetworkInfo()
			elif (keyboard == '70'):
				naoTalks()
			elif(keyboard == '80'):
				menu_RArmControl()
			elif (remoteInput[2] == 'KEY_POWER' or keyboard=="9"):
				break
			#								#
			# Main Menu for All Commands	#
			#								#
		
		
			if(keyboard=="9911" or keyboard=="9927"):#Only if Standing!
				print "::Standing"

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import getSummary [as 别名]
#motion.setStiffnesses("Body", 0.0) # 非僵硬状态，此时可任意改变机器人的姿态，程序控制无效。

#motion.stiffnessInterpolation("Body", 0.0, 1.0) #阻塞一段时候(参数3)后调节某部位(参数1)的状态(参数2)
#与setStiffnesses()相比，先延迟一段时间，然后才设置状态;

#setStiffnesses()为非阻塞函数，立刻关闭/打开电机，这样可能有危险
#因此一般建议使用stiffnessInterpolation(), 并加上一段时间供机器人开关电机。

#stiffnesses = motion.getStiffnesses("Body") #获得关节的stiffness值

# ----------> Wake robot <----------

# Wake up robot
# sets Motor on and, if needed, goes to initial position. 唤醒电机，必要时进入初始姿态。
# 对于H25 NAO Robot, 将设置stiffness为开启状态，并恢复起始站立状态;
# 要执行wakeUp(), 则无需执行motion.setStiffnesses("Body", 1.0)
motion.wakeUp()
time.sleep(1)
print motion.getSummary() #打印机器人当前姿态
print "Robot is WakeUp? ", motion.robotIsWakeUp()

print motion.getStiffnesses("HeadYaw");
motion.setStiffnesses("HeadYaw", 0.0)
time.sleep(1)
print motion.getStiffnesses("HeadYaw");

# ----------> robot rest <----------
# 关闭电机，进入非僵硬模式，此后程序控制无效(因电机已关闭), 但可手工改变电机位置
# 进入复位状态(蹲伏动作)
motion.rest()