Python ALProxy.openHand方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [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 openHand [as 别名]

#.........这里部分代码省略.........
                 subtle fashion.
        """
        if not radians:
            angles = [x * self.TO_RAD for x in angles]

        # Perform te movement
        self.__Motion.changeAngles(names, angles, max_speed)

    def set_angles(self, names, angles, max_speed, disable_stiffness=False, radians=False):
        """
        This method will set the angles for the joints in the list of names.
        To make sure the joints actually move, stiffness is set on these joints.
        CAUTION: This method sometimes results in very sudden movements of the NAO.
                 If you experience this and want to avoid it, call set_stiffness
                 on these joints before calling set_angles, as the sudden motion
                 results from the stifness being increased in a very short
                 amount of time. set_stiffness allows to do this in a more
                 subtle fashion.
        """
        if not radians:
            angles = [x * self.TO_RAD for x in angles]

        # Perform te movement
        self.__Motion.setAngles(names, angles, max_speed)

    def get_angles(self, names, radians=False):
        useSensors  = False     # Cannot use sensors in simulation :(
        angles = self.__Motion.getAngles(names, useSensors)
        if not radians:
            angles = [x / self.TO_RAD for x in angles]
        return angles

    def open_hand(self, hand):
        return self.__Motion.openHand(hand)

    def close_hand(self, hand):
        return self.__Motion.closeHand(hand)

    def get_proxy(self, which = "motion"):
        if which == "motion":
            return self.__Motion
        elif which == "tts":
            return self.__TTS
        elif which == "video":
            return self.__Video
        elif which == "frame":
            return self.__FM
        elif which == "memory":
            return self.__Memory
        elif which == "vision":
            return self.__VisionTB
        elif which == "balltracker":
            return self.__BallTracker
        elif which == "navigation":
            return self.__Navigation
        elif which == "leds":
            return self.__Leds

    def emergency(self):
        """Disable the NAO stiffness so it stops moving"""
        self.logger.warn("Emergency button pressed")
        self.set_stifness(['Body'], [0], [0.25])
        self.say("My emergency button has been pressed. I am now in emergency mode")
    
    def emergencyLeds(self, mode):
        

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]

#.........这里部分代码省略.........
        
        ## calculate which arm to use
        nameEffector = 'RArm'
        nameHand = 'RHand'
        if self.goal == 0 or self.goal == 3 or self.goal == 6:
            nameEffector = 'LArm'
            nameHand = 'LHand'
            self.behavior.runBehavior('xo_animations-8894e3/request_token_left')
        else:
            self.behavior.runBehavior('xo_animations-8894e3/request_token_right')
        print("[INFO ] Using %s" % nameHand)
        
        ## Update the state of the game
        self.state[i*3+j] = self.mode
        if self.mode == 'x':
            self.board[i][j] = 1
        else:
            self.board[i][j] = 0

        return result, nameEffector, nameHand
    
    def playMove(self, nextMove):
        '''
        Moves nameHand with nameEffector to the goalPos using proxy to ALMotion
        TODO: rewrite to utilize PositionInterpolation method from ALMotion
        '''
        
        ## unpack next move
        goalPos = nextMove[0]
        nameEffector = nextMove[1]
        nameHand = nextMove[2]
        print("[INFO ] Put object in %s and touch arm tactile sensor" % nameHand)
        ## open hand
        self.motion.openHand(nameHand)
        ## wait for the object to be placed in the hand
        while True:
            if nameEffector == 'RArm':
                val1 = self.memory.getData("HandRightLeftTouched")
                val2 = self.memory.getData("HandRightBackTouched")
                val3 = self.memory.getData("HandRightRightTouched")
            else:
                val1 = self.memory.getData("HandLeftLeftTouched")
                val2 = self.memory.getData("HandLeftBackTouched")
                val3 = self.memory.getData("HandLeftRightTouched")   
            if val1 or val2 or val3:
                break
        ## close hand
        self.motion.closeHand(nameHand)
        
        ## enable control of the arm
        print("[INFO ]Enableing whole body motion")
        self.motion.wbEnableEffectorControl(nameEffector, True)
        
        ## extract current position and elevate the hand
        currPos = self.motion.getPosition(nameEffector,2, True)
        currPos[0] += 0.00
        currPos[2] += 0.06
        self.motion.closeHand(nameHand)
        self.motion.setStiffnesses(nameEffector, 1.0)
        
        # lift the hand
        print("[INFO ]Lifting the hand")
        self.motion.positionInterpolations(nameEffector, 2, currPos, 7, 3)
        ## extract goal position and move arm towards it
        goalPosition = [goalPos[0,0], goalPos[1,0], goalPos[2,0]+self.height+0.08, 0.0, 0.0, 0.0]
        midPoint = [(goalPosition[0]+currPos[0])/2, (goalPosition[1]+currPos[1])/2, goalPosition[2], currPos[3], currPos[4], currPos[5]]

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]
class DeviceHumanoidMotion(HumanoidMotion):
    
    # Initialization of NAO proxies
    def __init__(self, parameters):

        self.nao_ip = parameters["nao_ip"]
        self.nao_port = int(parameters["nao_port"])

        self.posture = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
        self.motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)

        self.allowed_joints = ['HeadYaw', 'LShoulderPitch', 'LHipYawPitch1', \
                'RHipYawPitch1', 'RShoulderPitch', 'HeadPitch', 'LShoulderRoll', \
                'LHipRoll', 'RHipRoll', 'RShoulderRoll', \
                'LElbowYaw', 'LHipPitch', 'RHipPitch', 'RElbowYaw', \
                'LElbowRoll', 'LKneePitch', 'RKneePitch', 'RElbowRoll', \
                'LWristYaw2', 'LAnklePitch', 'RAnklePitch', 'RWristYaw2', \
                'LHand2', 'RAnkleRoll', 'LAnkleRoll', 'RHand2']

    # Assistive function to return errors
    def ret_exc(self, text):
        print text
        return {'error': text}

    # Sets a humanoid robot's joints. 'joints' are the names of the joints.
    # The nonmeclature used is the one of the NAO robot:
    # http://doc.aldebaran.com/2-1/family/robots/bodyparts.html#nao-chains
    # Angles are in rads.
    # Speed must be between [0,1], where 1 is the maximum nominal speed
    def setJointAngles(self, joints, angles, speed):
 
        if type(joints) != list or type(angles) != list:
            return self.ret_exc('humanoid_motion.setJointAngles: Joints or angles \
                    not lists')
        if len(joints) == 0 or len(angles) == 0:
            return self.ret_exc('humanoid_motion.setJointAngles: Empty joints \
                    or angles')

        for j in joints:
            if type(j) != str:
                return self.ret_exc('humanoid_motion.setJointAngles: A joint is \
                        not string')
            if j not in self.allowed_joints:
                return self.ret_exc('hunanoid_motion.setJointAngles: Not a valid \
                        joint: ' + str(j))
        if speed <= 0 or speed > 1:
            return self.ret_exc('humanoid_motion.setJointAngles: Speed out of bounds')

        for a in angles:
            if type(a) not in [float, int]:
                return self.ret_exc('humanoid_motion.setJointAngles: Angle not a number')

        try:
            self.motion.setAngles(joints, angles, speed)
        except Exception as e:
            return self.ret_exc("humanoid_motion.setJointAngles: Unrecognized exception: "\
                    + e.message)

        return {'error': None}

    # Returns the joint angles of a humanoid. Again 'joints' are the joints'
    # names, which can be found here:
    # http://doc.aldebaran.com/2-1/family/robots/bodyparts.html#nao-chains
    def getJointAngles(self, joints):
        for i in joints:
            if i not in self.allowed_joints:
                return self.ret_exc('hunanoid_motion.getJointAngles: Not a valid \
                        joint: ' + str(j))

        try:
            data = self.motion.getAngles(joints, False) 
        except Exception as e:
            return self.ret_exc("humanoid_motion.setJointAngles: Unrecognized exception: "\
                    + e.message)

        return {'angles': data, 'error': None}

    # Opens a hand of a humanoid robot. The input must be 'Right' or 'Left'
    def openHand(self, hand_name):
        if hand_name not in ['Right', 'Left']:
            return self.ret_exc('humanoid_motion.openHand: Unsupported hand name: ' +\
                    str(hand_name))
        try:
            if hand_name == "Right":
                self.motion.openHand('RHand')
            elif hand_name == "Left":
                self.motion.openHand('LHand')
        except Exception as e:
            return self.ret_exc("humanoid_motion.openHand: Unrecognized exception: "\
                    + e.message)

        return {'error': None}

    # Closes a NAO's hand. The input must be 'Right' or 'Left'
    def closeHand(self, hand_name):
        if hand_name not in ['Right', 'Left']:
            return self.ret_exc('humanoid_motion.closeHand: Unsupported hand name: ' +\
                    str(hand_name))

        if hand_name == "Right":
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]
def main(robotIP, PORT=9559):
	# We need this broker to be able to construct
	# NAOqi modules and subscribe to other modules
	# The broker must stay alive until the program exists
	myBroker = ALBroker("myBroker",
	   "0.0.0.0",   # listen to anyone
	   0,           # find a free port and use it
	   robotIP,          # parent broker IP
	   PORT)        # parent broker port

	#StartProxies
	motionProxy  = ALProxy("ALMotion")
	postureProxy = ALProxy("ALRobotPosture")
	tts = ALProxy("ALTextToSpeech")


	
	# Send robot to Pose Init
	postureProxy.goToPosture("StandInit", 0.5) #other postures exists - Stand/StandInit/StandZero/Crouch

	
	#Loop for the broker
	try:
		while True:
			time.sleep(1)
			
			recordLetter()
			
			
			list = analyzeLetter()
			currentTestNumber = list[0]   #get values from getContours
			currentPath = list[1]
			
			time.sleep(4)
			#getEffectorPosition(motionProxy,currentPath)
			
			#BringArmToPosition
			bringArmToPosition(motionProxy)
			time.sleep(1)
			
			#TEST - Get position of arm to set it
			#print("Checking position of torso")
			time.sleep(1)
			
			#generate the list of the "evolution" of the letter from the starting point. Then generate final list of mv from it.
			#WAS AFTER reactToTouch(), MOVED HERE
			evolList = scaleLetterPoints(currentPath)
			movementsList = finalizeMovementsList(motionProxy,evolList,currentPath)
			
			#OpenHand
			motionProxy.openHand("RHand")
			time.sleep(1)
			
			tts.say("Please give me the pen. Touch the sensor on my right hand when it's ready")
			
			#activate sensors, wait for touch to close hand and begin drawing	
			reactToTouch()
			
			
			reproduceMovement(motionProxy,movementsList)
			
			
			#print("hand is in positon. Be ready to catch the arm")
			time.sleep(2)
			#recordWriting(motionProxy,currentPath)
			
			tts.say("I am done. Can you take the pen back?")
			motionProxy.openHand("RHand")
			
			#generate file that contains current testNumber to keep track of number of tests
			print("Test number "+str(currentTestNumber)+" is over, creating a new testNumber and saving it")			
			currentTestNumber+=1 
			pickle.dump(currentTestNumber, open( "currentTestNumber.p", "wb" ) )
			print("Test number saved")
			
			tts.say("Get ready to turn me off")
			time.sleep(4)

	except KeyboardInterrupt:
		# Go to rest position
		motionProxy.rest()
		print "Interrupted by user, shutting down"
		myBroker.shutdown()
		sys.exit(0)	

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]
class Tracker():

    def __init__(self):
	rospy.init_node('tracker_mykinect', anonymous=True)
	self.myFrame = 'kinect_link'
	self.r = rospy.Rate(20)		#rospy.Rate(1) = 1Hz
	
#Skeleton Listener
	self.tf = tf.TransformListener()
#Nao init and connect
	self.nao_init()
	self.startXPosition = 0.0;
	self.startYPosition = 0.0;
	self.naoXPosition = self.startXPosition
	self.naoYPosition = self.startYPosition
	rospy.loginfo("Start tracking for 3s...")
        rospy.sleep(3.0)
	rospy.loginfo("Tracking started!")
	self.hoh = 0.86 #Nao Hight in meters
	for i in range(1, 4):	
#Get all nessecery joins. (Right side = Left side if you stand front to sensor)		
		try:
			self.j_head = "{0}_{1}".format(rospy.get_param('~target_joint', '/head'), i)
			self.j_l_hand = "{0}_{1}".format(rospy.get_param('~target_joint', '/left_hand'), i) 
			self.j_r_hand = "{0}_{1}".format(rospy.get_param('~target_joint', '/right_hand'), i)
			self.j_l_shoul = "{0}_{1}".format(rospy.get_param('~target_joint', '/left_shoulder'), i)
			self.j_torso = "{0}_{1}".format(rospy.get_param('~target_joint', '/torso'), i)
			self.j_l_ehand = "{0}_{1}".format(rospy.get_param('~target_joint', '/left_elbow'), i)
			self.j_r_ehand = "{0}_{1}".format(rospy.get_param('~target_joint', '/right_elbow'), i)
			break
		except:
			pass
    
    def toRAD(self,angle):
	return angle*almath.TO_RAD

    def nao_init(self):
	#NAO CONNECT AND INIT
	# PORT AND IP NAO ROBOT
	#ip = rospy.get_param('~ip', '127.0.0.1')
	#port = int(rospy.get_param('~port', '51888'))
	ip = rospy.get_param('~ip', '10.104.16.53')
	port = int(rospy.get_param('~port', '9559'))

	self.al = ALProxy("ALAutonomousLife", ip, port)
	self.postureProxy = ALProxy("ALRobotPosture", ip, port)
	self.tts = ALProxy("ALTextToSpeech", ip, port)
	self.motionProxy = ALProxy("ALMotion", ip, port)
	self.behavior = ALProxy("ALBehaviorManager",ip,port)
	self.mistake = almath.Pose2D(self.motionProxy.getRobotPosition(False))
	#self.audioProxy = ALProxy("ALSpeechRecognition",ip,port)
	#self.audioProxy.setLanguage("English")
	#vocabulary = ["go"]
	#self.audioProxy.setVocabulary(vocabulary, False)
	#self.al.setState("disabled")
	#self.postureProxy.goToPosture("StandInit", 0.5)	
	#ALProxy("ALBasicAwareness", ip, port).stopAwareness()
	#self.motionProxy.setFallManagerEnabled(False)
	self.motionProxy.setExternalCollisionProtectionEnabled("All",False)

    def nao_hello(self):
#Nao hello module
	#self.postureProxy.goToPosture("StandInit",1.0)
	self.behavior.stopAllBehaviors()
	self.motionProxy.setAngles('RShoulderPitch',self.toRAD(-97.3),1.0)
	self.motionProxy.setAngles('RShoulderRoll',self.toRAD(-39.2),1.0)
	self.motionProxy.setAngles('RElbowRoll',self.toRAD(50.8),1.0)
	self.motionProxy.setAngles('RElbowYaw',self.toRAD(8.5),1.0)
	self.motionProxy.setAngles('RWristYaw',self.toRAD(-13.4),1.0)
	self.motionProxy.openHand('RHand')
	thread.start_new_thread((lambda:self.tts.say("Hello my friend")),())
	for i in range(0,2):
		self.motionProxy.angleInterpolation("RShoulderRoll",self.toRAD(-12.2),0.3,True)
		self.motionProxy.angleInterpolation("RShoulderRoll",self.toRAD(39.2),0.3,True)
		self.motionProxy.angleInterpolation("RShoulderRoll",self.toRAD(-50.4),0.3,True)
	
	self.postureProxy.goToPosture("StandInit",1.0)
	self.behavior.runBehavior('nao-14eaf5/BogdAL')


    def nao_sensei(self):
#Nao hello(sensei) module
	self.behavior.stopAllBehaviors()
	self.postureProxy.goToPosture("StandInit",1.0)
	
	self.motionProxy.setAngles(['RShoulderPitch','LShoulderPitch'],[self.toRAD(52.7),self.toRAD(49.1)],1.0)
	self.motionProxy.setAngles(['RShoulderRoll','LShoulderRoll'],[self.toRAD(13.3),self.toRAD(-8.9)],1.0)
	self.motionProxy.setAngles(['RElbowRoll','LElbowRoll'],[self.toRAD(88.3),self.toRAD(-87.6)],1.0)
	self.motionProxy.setAngles(['RElbowYaw','LElbowYaw'],[self.toRAD(68.1),self.toRAD(-60.9)],1.0)
	self.motionProxy.setAngles(['RWristYaw','LWristYaw'],[self.toRAD(-10.3),self.toRAD(11.0)],1.0)
	self.motionProxy.setAngles(['RHand','LHand'],[self.toRAD(-10.3),self.toRAD(11.0)],1.0)
	self.motionProxy.openHand('RHand')
	self.motionProxy.openHand('LHand')
	#sequence head
	self.tts.say("Domo Arigato")
	#self.motionProxy.angleInterpolation("HeadPitch",0.0,1.0,True)
	self.motionProxy.angleInterpolation("HeadPitch",self.toRAD(26.5),1.0,True)
	self.motionProxy.angleInterpolation("HeadPitch",0.0,1.0,True)
	
	self.postureProxy.goToPosture("StandInit",1)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]
#!/usr/bin/python
# -*- coding: utf-8 -*-
from naoqi import ALProxy
from naoconfig import *

proxyMo = ALProxy('ALMotion',robot_IP,robot_port)

proxyMo.post.openHand('LHand')
proxyMo.openHand('RHand')

proxyMo.post.closeHand('LHand')
proxyMo.closeHand('RHand')

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]
class NaoMove():
	# Class used to make all movements to Nao
	def __init__(self, parent):
		#Constructor
		
		self.motionProxy = None
		self.postureProxy = None
		self.tts = None
		self.memoryProxy = None
		self.behaviourProxy = None
		
		self.motorOn = False
		self.parent = parent
		
		self.memValue = ["FaceDetected"]
		
		self.headNames = ["HeadYaw", "HeadPitch"]
		self.leftArmNames = ["LShoulderRoll","LShoulderPitch","LElbowRoll","LElbowYaw"]
		self.rightArmNames = ["RShoulderRoll","RShoulderPitch","RElbowRoll","RElbowYaw"]
		
	def connect(self, robotIP, port):
		try:
			self.motionProxy = ALProxy("ALMotion", robotIP , port)
			self.tts = ALProxy("ALTextToSpeech", robotIP, port)
			self.postureProxy = ALProxy("ALRobotPosture", robotIP, port)
			self.memoryProxy = ALProxy("ALMemory", robotIP, port)
			self.behaviourProxy = ALProxy("ALBehaviorManager", robotIP, port)
		except:
			return 1
			
		# If connection is successful, we initialize few variables for Arms	
		self.anglesRight = self.motionProxy.getAngles(self.rightArmNames, True)
		self.anglesLeft = self.motionProxy.getAngles(self.leftArmNames, True)
		return 0
		
		
	def walk(self,x,y,theta,frequency):
		# Test if the stiffness of Nao is set to 1.0
		self.testMotorOn()
		
		# Launch the walk
		self.motionProxy.setWalkTargetVelocity(x, y, theta, frequency)
		
	def stopWalk(self, frequency):
		self.motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, frequency)
		self.motionProxy.waitUntilMoveIsFinished()
		
	def stop(self):
		self.motionProxy.killMove()
		
	def standInit(self):
		self.postureProxy.goToPosture("StandInit", constante.FREQUENCY)
		
	def crouch(self):
		self.postureProxy.goToPosture("Crouch", constante.FREQUENCY)
		
	def runHello(self):
		self.testMotorOn()
		
		self.behaviourProxy.runBehavior("helloBoy")
		
	def openHand(self, hand):
		if hand == constante.RIGHT_HAND:
			self.motionProxy.openHand('RHand')
		else:
			self.motionProxy.openHand('LHand')
	
	def closeHand(self, hand):
		if hand == constante.RIGHT_HAND:
			self.motionProxy.closeHand('RHand')
		else:
			self.motionProxy.closeHand('LHand')
			
	def moveHead(self, angles):
		# Test if the stiffness of Nao is set to 1.0
		self.testMotorOn()
		
		current = self.motionProxy.getAngles(self.headNames, True)
		
		# this boucle is used to dynamically calculate the speed of the movement
		for i in range(len(angles)):
			speed = abs(angles[i]-current[i])/2.0
			
			if speed < 0.05:
				speed = 0.05
			if speed > 0.8:
				speed = 0.8
				
			# Make the movement
			self.motionProxy.setAngles(self.headNames[i], angles[i], speed)
			
	def detectFace(self):
		# Check that it is not already running.
		if (not self.behaviourProxy.isBehaviorRunning("faceDetect")):
			# Launch behavior. This is a blocking call, use post if you do not
			# want to wait for the behavior to finish.
			self.behaviourProxy.post.runBehavior("faceDetect")
		
	def naoMoveTwoArms(self, listPos):
		# Test if the stiffness of Nao is set to 1.0
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]

#.........这里部分代码省略.........

    def moveHead(self, pitch, yaw, radians=False):
        """
        :param pitch: the future pitch of NAO's head
        :param yaw: the future yaw of NAO's head
        :param radians: True if the angles are given in radians. False by default => angles in degree.
        """
        joint_names = ["HeadYaw", "HeadPitch"]
        if not radians:
            yaw = math.radians(yaw)
            pitch = math.radians(pitch)
        angles = [yaw, pitch]
        self.motion_proxy.angleInterpolation(joint_names, angles, 1., True)

    def compareToLeftHandPosition(self, coord, must_print=False):
        """
        :param coord: the 6D coordinates to compare with the left hand position
        :type coord: list
        :return: the difference of position from the hand to the given coordinates [x-axis, y-axis]
        :rtype: np.array
        """
        hand_coord = self.getLeftHandPosition()
        if must_print:
            print coord
            print hand_coord
        return geom.vectorize(hand_coord[0:2], coord[0:2])

    def playDisc(self, hole_coordinates):
        """
        :param hole_coordinates: The 6D translation + rotation vector of the hole
        """
        self.stand()
        self.setLeftHandPosition(hole_coordinates, mask=63)
        self.motion_proxy.openHand("LHand")
        self.motion_proxy.closeHand("LHand")
        self.setLeftArmAlongsideBody()

    def setLeftArmRaised(self, secure=False):
        """
        Raise NAO's left arm to the sky
        """
        self.stand()
        if secure:
            self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_BOTTOM, 2., True)
            self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_TOP, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, RAISED, 2., True)

    def setLeftArmAlongsideBody(self):
        """
        Move the left arm of NAO alongside his body.
        """
        self.stand()
        self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_TOP, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_BOTTOM, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, ARM_ALONGSIDE_BODY, 2., True)

    def setRightArmAlongsideBody(self):
        """
        Move the left arm of NAO alongside his body.
        """
        self.stand()
        self.motion_proxy.angleInterpolation(RARM_CHAIN, ARM_ALONGSIDE_BODY_R, 2., True)

    def setLeftArmToAskingPosition(self):
        """
        Move the left arm of NAO in a "asking disc" position, and open his left hand.

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]
class Tracker():

    def __init__(self):
	rospy.init_node('tracker_mykinect', anonymous=True)
	self.myFrame = 'kinect_link'
	self.r = rospy.Rate(20)		#rospy.Rate(1) = 1Hz

#Skeleton Listener
	self.tf = tf.TransformListener()
#Nao init and connect
	self.nao_init()

	rospy.loginfo("Start tracking for 3s...")
        rospy.sleep(3.0)
	rospy.loginfo("Tracking started!")
	self.hoh = 0.86 #Nao Hight in meters
	for i in range(1, 4):	
#Get all nessecery joins. (Right side = Left side if you stand front to sensor)		
		try:
			self.j_head = "{0}_{1}".format(rospy.get_param('~target_joint', '/head'), i)
			self.j_l_hand = "{0}_{1}".format(rospy.get_param('~target_joint', '/left_hand'), i) 
			self.j_r_hand = "{0}_{1}".format(rospy.get_param('~target_joint', '/right_hand'), i)
			self.j_l_shoul = "{0}_{1}".format(rospy.get_param('~target_joint', '/left_shoulder'), i)
			self.j_torso = "{0}_{1}".format(rospy.get_param('~target_joint', '/torso'), i)
			self.j_l_ehand = "{0}_{1}".format(rospy.get_param('~target_joint', '/left_elbow'), i)
			self.j_r_ehand = "{0}_{1}".format(rospy.get_param('~target_joint', '/right_elbow'), i)
			break
		except:
			pass
    
    def toRAD(self,angle):
	return angle*almath.TO_RAD

    def nao_init(self):
	#NAO CONNECT AND INIT
	# PORT AND IP NAO ROBOT
	ip = rospy.get_param('~ip', '10.104.16.50')
	port = int(rospy.get_param('~port', '9559'))

	self.al = ALProxy("ALAutonomousLife", ip, port)
	self.postureProxy = ALProxy("ALRobotPosture", ip, port)
	self.tts = ALProxy("ALTextToSpeech", ip, port)
	self.motionProxy = ALProxy("ALMotion", ip, port)
	self.al.setState("disabled")
	self.postureProxy.goToPosture("StandInit", 0.5)	

    def nao_hello(self):
#Nao hello module
	self.postureProxy.goToPosture("Standing",1.0)
	self.motionProxy.setAngles('RShoulderPitch',self.toRAD(-97.3),1.0)
	self.motionProxy.setAngles('RShoulderRoll',self.toRAD(-39.2),1.0)
	self.motionProxy.setAngles('RElbowRoll',self.toRAD(50.8),1.0)
	self.motionProxy.setAngles('RElbowYaw',self.toRAD(8.5),1.0)
	self.motionProxy.setAngles('RWristYaw',self.toRAD(-13.4),1.0)
	self.motionProxy.openHand('RHand')
	self.tts.say("Hello my friend")
	for i in range(0,2):
		self.motionProxy.angleInterpolation("RShoulderRoll",self.toRAD(-12.2),0.3,True)
		self.motionProxy.angleInterpolation("RShoulderRoll",self.toRAD(39.2),0.3,True)
		self.motionProxy.angleInterpolation("RShoulderRoll",self.toRAD(-50.4),0.3,True)
	self.postureProxy.goToPosture("Standing",1.0)


    def nao_sensei(self):
#Nao hello(sensei) module
	self.postureProxy.goToPosture("Standing",1.0)
	self.motionProxy.setAngles(['RShoulderPitch','LShoulderPitch'],[self.toRAD(52.7),self.toRAD(49.1)],1.0)
	self.motionProxy.setAngles(['RShoulderRoll','LShoulderRoll'],[self.toRAD(13.3),self.toRAD(-8.9)],1.0)
	self.motionProxy.setAngles(['RElbowRoll','LElbowRoll'],[self.toRAD(88.3),self.toRAD(-87.6)],1.0)
	self.motionProxy.setAngles(['RElbowYaw','LElbowYaw'],[self.toRAD(68.1),self.toRAD(-60.9)],1.0)
	self.motionProxy.setAngles(['RWristYaw','LWristYaw'],[self.toRAD(-10.3),self.toRAD(11.0)],1.0)
	self.motionProxy.setAngles(['RHand','LHand'],[self.toRAD(-10.3),self.toRAD(11.0)],1.0)
	self.motionProxy.openHand('RHand')
	self.motionProxy.openHand('LHand')
	#sequence head
	self.tts.say("Domo Arigato")
	#self.motionProxy.angleInterpolation("HeadPitch",0.0,1.0,True)
	self.motionProxy.angleInterpolation("HeadPitch",self.toRAD(26.5),1.0,True)
	self.motionProxy.angleInterpolation("HeadPitch",0.0,1.0,True)
	
	self.postureProxy.goToPosture("Standing",1)
	
    def hello_process(self):
#HELLO module
	self.count = 0
	self.side = 0		#left 0 right 1
	while not rospy.is_shutdown():
# Get all nessecery vectors
		trans_head, rot= self.tf.lookupTransform('/openni_depth_frame', self.j_head, rospy.Duration())
		vec_head = Vector3(*trans_head)
		trans_hand, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_l_hand, rospy.Duration())
		vec_hand = Vector3(*trans_hand)
		trans_shoul, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_l_shoul, rospy.Duration())
		vec_shoul = Vector3(*trans_shoul)
		trans_torso, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_torso, rospy.Duration())
		vec_torso = Vector3(*trans_torso)
# IF right hand upper that torso
		if (vec_hand.z > vec_torso.z):
	#left side
			if (not self.side):
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]
# -*- encoding: UTF-8 -*-

from naoqi import ALProxy

IP = '127.0.0.1'
PORT = 49340

motion_proxy = ALProxy("ALMotion",IP,PORT)
motion_proxy.openHand('LHand')
motion_proxy.openHand('RHand')

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import openHand [as 别名]
class CoreRobot(object):

    def __init__(self):
        self.animated_speech_proxy = None
        self.motion_proxy = None
        self.posture_proxy = None
        self.awareness_proxy = None
        self.autonomous_move_proxy = None
        self.autonomous_life_proxy = None

    def connect(self, host, port):
        """Takes connection params and builds list of ALProxies"""
        print 'Core - Connecting to robot on {0}:{1}...'.format(host, port)
        try:
            self.animated_speech_proxy = ALProxy("ALAnimatedSpeech", host, port)
            self.motion_proxy = ALProxy("ALMotion", host, port)
            self.posture_proxy = ALProxy("ALRobotPosture", host, port)
            self.awareness_proxy = ALProxy("ALBasicAwareness", host, port)
            self.autonomous_move_proxy = ALProxy("ALAutonomousMoves", host, port)
            self.autonomous_life_proxy = ALProxy("ALAutonomousLife", host, port)
        except Exception as exception: # pylint: disable=broad-except
            raise Exception('Could not create proxy:{0}', format(exception))
        self.set_autonomous_life(False)

    def say(self, animated_speech):
        self.animated_speech_proxy.say(animated_speech) 

    def move(self, rotation, distance):
        motion = self.motion_proxy
        motion.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
        motion.setMotionConfig([["ENABLE_STIFFNESS_PROTECTION", True]])
        motion.setCollisionProtectionEnabled('Arms', True)
        motion.setExternalCollisionProtectionEnabled('All', True)
        motion.moveTo(0, 0, rotation)
        motion.moveTo(distance, 0, 0)
        
    def open_hand(self, hand):
        self.motion_proxy.openHand(hand)
 
    def close_hand(self, hand):
        self.motion_proxy.closeHand(hand)

    def set_stiffness(self, joint, stiffness):
        print 'Setting {0} to stiffness {1}...'.format(joint, stiffness)
        self.motion_proxy.stiffnessInterpolation(joint, stiffness, 1.0) 
        
    def set_joint_angle(self, joint, angle_degrees, speed=0.1):
        print 'Setting {0} to {1} angle in degrees...'.format(joint, angle_degrees)
        self.motion_proxy.angleInterpolationWithSpeed(joint, math.radians(angle_degrees), speed)
        
    def set_pose(self, pose):
        self.posture_proxy.goToPosture(pose, 0.5) 
      
    def set_autonomous_life(self, set_on):
        self.print_sub_system_update(set_on, 'Autonomous Life')
        target_state = 'solitary' if set_on else 'disabled'
        self.autonomous_life_proxy.setState(target_state)
        
    def get_autonomous_life_state(self):
        return self.autonomous_life_proxy.getState()
  
    def set_autonomous_moves(self, set_on):
        self.print_sub_system_update(set_on, 'Autonomous Moves')
        target_state = 'backToNeutral' if set_on else 'none'
        self.autonomous_move_proxy.setBackgroundStrategy(target_state)
        
    def set_awareness(self, set_on):
        self.print_sub_system_update(set_on, 'Basic Awareness')
        if set_on:
            self.awareness_proxy.startAwareness() 
        else:
            self.awareness_proxy.stopAwareness()

    def set_breathing(self, set_on):
        self.print_sub_system_update(set_on, 'body breathing')
        self.motion_proxy.setBreathEnabled('Body', set_on)
        self.print_sub_system_update(set_on, 'arm breathing')
        self.motion_proxy.setBreathEnabled('Arms', set_on)

    def set_move_arms_enabled(self, left_arm, right_arm):
        self.motion_proxy.setMoveArmsEnabled(left_arm, right_arm)

    @staticmethod
    def print_sub_system_update(set_on, sub_process):
        on_off = ['off', 'on']
        print 'Turning {0} {1}...'.format(on_off[set_on], sub_process)