Python ALProxy.setAngles方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
class Nao:
	def __init__(self,ip,port):
		self.tts = ALProxy("ALTextToSpeech", ip, port)
		self.motion = ALProxy("ALMotion", ip, port)
		self.posture = ALProxy("ALRobotPosture", ip, port)
		self.memory = ALProxy("ALMemory")
		self.speech = ALProxy("ALSpeechRecognition",ip,port)
		
		#Word Recognition
		self.speech.setLanguage("English") 
		wordList=["hello","goodbye","yes","no", "exit", "sit down"]
		self.speech.setWordListAsVocabulary(wordList)
		self.memory.subscribeToEvent("WordRecognized","pythonModule", "onSpeechRecognized") #  event is case sensitive !


	def sit_down(self):
		self.posture.goToPosture("Sit", 0.3)
		time.sleep(0.5)
		self.motion.setStiffnesses("Body", 0.3)
		fractionMaxSpeed=0.1
		self.motion.setAngles(["LArm"],[ 0.96, 0.03,-0.71,-1.20, 0.00, 0.30],fractionMaxSpeed)
		self.motion.setAngles(["RArm"],[ 0.96,-0.05, 0.71, 1.20, 0.00, 0.30],fractionMaxSpeed)
		self.motion.setAngles(["RLeg"],[-0.84,-0.30,-1.50, 1.02, 0.92, 0.00],fractionMaxSpeed)
		self.motion.setAngles(["LLeg"],[-0.84, 0.30,-1.50, 1.02, 0.92, 0.00],fractionMaxSpeed)
		time.sleep(0.5)
		self.motion.setStiffnesses("Body", 0.0)
		time.sleep(0.25)
		
		
	def __del__(self):
		print "memory.unsubscribeToEvent(WordRecognized,pythonModule)"
		self.memory.unsubscribeToEvent("WordRecognized","pythonModule") #  event is case sensitive !		

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
class Nao:
	def __init__(self,ip,port):
		self.tts = ALProxy("ALTextToSpeech", ip, port)
		self.motion = ALProxy("ALMotion", ip, port)
		self.posture = ALProxy("ALRobotPosture", ip, port)
		self.memory = ALProxy("ALMemory")
		self.speech = ALProxy("ALSpeechRecognition",ip,port)

		#Word Recognition
		self.speech.setLanguage("English") 
		wordList=["hello","goodbye","yes","no", "exit", "sit down"]
		pythonModule.WordDetection(wordList)


	def sit_down(self):
		self.posture.goToPosture("Sit", 0.3)
		time.sleep(0.5)
		self.motion.setStiffnesses("Body", 0.3)
		fractionMaxSpeed=0.1
		self.motion.setAngles(["LArm"],[ 0.96, 0.03,-0.71,-1.20, 0.00, 0.30],fractionMaxSpeed)
		self.motion.setAngles(["RArm"],[ 0.96,-0.05, 0.71, 1.20, 0.00, 0.30],fractionMaxSpeed)
		self.motion.setAngles(["RLeg"],[-0.84,-0.30,-1.50, 1.02, 0.92, 0.00],fractionMaxSpeed)
		self.motion.setAngles(["LLeg"],[-0.84, 0.30,-1.50, 1.02, 0.92, 0.00],fractionMaxSpeed)
		time.sleep(0.5)
		self.motion.setStiffnesses("Body", 0.0)
		time.sleep(0.25)
		
		
	def __del__(self):

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
def init(ip='127.0.0.1', port=9559):
    global myBroker
    myBroker = ALBroker('myBroker', '0.0.0.0', 0, ip, port)
    motion = ALProxy("ALMotion")
    motion.setStiffnesses('Body', .3)
    motion.setAngles('Body', motion.getAngles('Body', True), 1)
    time.sleep(1)
    motion.setStiffnesses('Body', 0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
class TestModule(ALModule):
    """A module used for testing Nao's functionality"""

    def __init__(self, var_name):
        """Define the module's attributes and initialize their value"""

        # Request the parent module to initialize and register the module
        ALModule.__init__(self, var_name)

        # Declare the memory
        global memory
        memory = ALProxy("ALMemory")

        # Initialize a motion module
        self.motion = ALProxy("ALMotion")
        # Initialize a posture module
        self.posture = ALProxy("ALRobotPosture")

    def initialize(self):
        """Initialize the inner modules of the module"""

        # Wake the robot up
        self.motion.wakeUp()
        # Enable the whole body motion
        self.motion.wbEnable(True)

    def shutdown(self):
        """Shut the module down gracefully"""

        # Disable the whole body motion
        self.motion.wbEnable(False)
        # Put the robot to rest
        self.motion.rest()

    def kick(self):
        """Define the procedure to follow to accomplish a kick"""

        # Go to stand init posture
        self.posture.goToPosture("StandInit", 0.5)

        # Define the joints we are interested in
        hip_joints = ["RHipRoll", "LHipRoll"]

        # Get the angles for HipRolls
        angles = self.motion.getAngles(hip_joints, True)

        # Modify the value of the hip rolls
        angles[0] -= 10 * almath.TO_RAD
        angles[1] += 10 * almath.TO_RAD

        # Set the angles to have Nao stand on one foot
        self.motion.setAngles(hip_joints, angles, 0.2)

        # Wait for some time
        sleep(5)

        # Go back to stand init posture
        self.posture.goToPosture("StandInit", 0.5)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
class FaceTracker():
    
    def __init__(self, use_nao, ip = "10.0.1.3", port = 9559):
        self.use_nao = use_nao
        if use_nao:
            self.faceProxy = ALProxy("ALFaceTracker", ip, port)
            self.motion = ALProxy("ALMotion", ip, port)
        
    def start_tracking(self):
        if self.use_nao:
            print "StartTracking"
            self.motion.setStiffnesses("Head", 1.0)
            self.faceProxy.startTracker()
            self.faceProxy.setWholeBodyOn(True)
    
    def stop_tracking(self):
        if self.use_nao:
            self.faceProxy.stopTracker()
            self.motion.setStiffnesses("Head", 0.0)
            self.to_default_pos()
            print "Tracking stopped"
    
    def to_default_pos(self):
        if self.use_nao:
            self.motion.setStiffnesses("Head", 0.5)
            self.motion.setAngles("HeadYaw", 0.0, 0.1)
            self.motion.setAngles("HeadPitch", -0.25, 0.1)
#            self.motion.setStiffnesses("Head", 0)
    
    def shake_no(self):
        if self.use_nao:
            names = "HeadYaw"
            currentAngle = self.motion.getAngles("Head", True)[0]
    
            angles = [0.25, 0, -0.25, 0, 0.25, currentAngle]
            angles = [currentAngle+0.25, currentAngle, currentAngle-0.25, currentAngle, currentAngle+0.25, currentAngle]
            times = [(i/len(angles))+0.2 for i in np.arange(1, len(angles)+1)]
                    
            self.faceProxy.stopTracker()
            print "no"
            self.motion.setStiffnesses("Head", 1.0)
            self.motion.angleInterpolation(names, angles, times, True)
            self.motion.setStiffnesses("Head", 0.0)
            self.start_tracking()
    
    def shake_yes(self):
        if self.use_nao:
            names = "HeadPitch"        
            currentAngle = self.motion.getAngles("Head", False)[1]
            angles = [currentAngle+0.15, currentAngle]
            times = [i/len(angles)*0.5 for i in np.arange(1, len(angles)+1)]
            
            self.faceProxy.stopTracker()
            self.motion.setStiffnesses("Head", 1.0)
            self.motion.angleInterpolation(names, angles, times, True)
            self.motion.setStiffnesses("Head", 0.0)
            self.start_tracking()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [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 setAngles [as 别名]
def test_retraction(ip, kicking_leg, direction, lambd=5, dmax=50):
    """
    kicking_leg: LLeg or RLeg
    direction: a direction vector as python list
    """
    # create a motion proxy
    import sys
    sys.path.append("SDK")
    from naoqi import ALProxy
    mp = ALProxy("ALMotion", ip, 9559)
    import CoM
    fk = CoM.CenterOfMass(ip, 9559)
    import ik_jacobian
    import time
    # set the other leg's name
    other_leg = "RLeg" if kicking_leg == "LLeg" else "LLeg"

    mp.setStiffnesses("Body", 1)
    normalPose(mp, True)

    
    kick_pos = [i[0,0] for i in fk.get_locations_dict(other_leg)[ kicking_leg[0] +
        "AnkleRoll"]]
    kick_pos = kick_pos[:-1]
    print "kickpos", kick_pos
    other_pos = [0,0,0]
    contact_point ,point = find_point("This is irrelevant!", direction, kick_pos, other_pos)
    print contact_point
    print point
    print "point", point
    print "contact", contact_point

    angle_list = ik_jacobian.set_position(ip, kicking_leg, np.matrix(point).T,
                                          lambd=lambd, dmax=dmax)
    joints = []
    angles = []
    for joint in angle_list:
        joints = joints + [joint]
        angles = angles + [angle_list[joint]]
    mp.setAngles(joints, angles, 0.3)
    #time.sleep(3)
    joints = []
    angles = []
    contact_point = np.matrix(contact_point).T
    contact_point -=30
    angle_list = ik_jacobian.set_position(ip, kicking_leg,
                                          contact_point,
                                          lambd=lambd, dmax=dmax)
    for joint in angle_list:
        joints = joints + [joint]
        angles = angles + [angle_list[joint]]
    mp.setAngles(joints, angles, 1)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
class HeadController():
    def __init__(self,ip,port):
        self.motion = ALProxy("ALMotion",ip,port)
        self.motion.setStiffnesses("Head",1.0)
        # contains 5 hard coded head yaw and pitch angles
        self.angles = [[-0.829349559873, -0.0117052046163],[-0.334127543218, 0.0594786450711],[-0.003150604247 , 0.047208706762],[0.0499896272058 ,0.437312890742],[ 0.524700755346 , 0.0525692697978]]
        self.num_objects = 5
        self.cur_object = 0

    def moveHeadToNextObject(self):
        self.motion.setAngles("Head",self.angles[self.cur_object],0.1)
        self.cur_object = (self.cur_object+1)%self.num_objects
        time.sleep(2) # give the robot some time to stabilize, that the image is not blurry

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
def main(robotIP, PORT=9559):

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    #####################
    ## Enable arms control by move algorithm
    #####################
    motionProxy.setMoveArmsEnabled(True, True)
    #~ motionProxy.setMoveArmsEnabled(False, False)

    #####################
    ## FOOT CONTACT PROTECTION
    #####################
    #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    #####################
    ## get robot position before move
    #####################
    initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    X = 0.0
    Y = 0.0
    Theta = math.pi/2.0
    motionProxy.post.moveTo(X, Y, Theta)
    motionProxy.setAngles("HeadYaw", 0.6, 0.6)
    # wait is useful because with post moveTo is not blocking function
    motionProxy.waitUntilMoveIsFinished()

    #####################
    ## get robot position after move
    #####################
    endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    #####################
    ## compute and print the robot motion
    #####################
    robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition
    # return an angle between ]-PI, PI]
    robotMove.theta = m.modulo2PI(robotMove.theta)
    print "Robot Move:", robotMove

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
    def run(self, rs):
      if self.graspCB is not None and self.graspCB.check():
        print 'grasping pose'
        self.graspCB = None
        self.isRunning = True
        self.offset_X_box_hd = self.offset_X_box_hd_grasp 

      if self.stopCB is not None and self.stopCB.check():
        print 'stopping'
        self.stopCB = None
        self.isRunning = True
        self.hsCB = stopMotion(robots, qpsolver, postureTask1, None, rbdList(romeo.mbc.q))
        try:
          PORT = 9559
          robotIP = "198.18.0.1"
          motionProxy = ALProxy("ALMotion", robotIP, PORT)
        except Exception,e:
          print "Could not create proxy to ALMotion"
          print "Error was: ",e
          sys.exit(1)

        # Close the hand 
        handName  = 'RHand'
        #motionProxy.closeHand(handName)
        motionProxy.setStiffnesses(handName, 1.0)
        angle  = 0.10
        fractionMaxSpeed  = 0.2
        motionProxy.setAngles(handName, angle, fractionMaxSpeed)
        time.sleep(1.0)

        # Motion to raise the box
        chainName = "RArm"
        frame     = 0 # TORSO
        useSensor = True
        # Get the current position of the chainName in the same frame
        current = motionProxy.getPosition(chainName, frame, useSensor)

        target = [
            current[0] + 0.00,
            current[1] + 0.00,
            current[2] + 0.09,
            current[3] + 0.0,
            current[4] + 0.0,
            current[5] + 0.0]

        fractionMaxSpeed = 0.3
        axisMask         = 7 # just control position
        motionProxy.setPositions(chainName, frame, target, fractionMaxSpeed, axisMask)
        time.sleep(1.0)

        self.fsm = self.waitHS

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
def main(robotIP, PORT = 9559):
    global yaw
    global pitch
    thread.start_new_thread(DataReceiver,())
    motion = ALProxy("ALMotion", robotIP, PORT)
    motion.setStiffnesses("Head", 1.0)
    #motion.setAngles(["HeadYaw"],1.0,1.0);
    avgYaw=MovingAverage(AVG_WINDOW_SIZE)
    avgPitch=MovingAverage(AVG_WINDOW_SIZE)
    while True:
        #print("************************* Head:"+msg)
        yaw2=avgYaw.add(yaw)
        pitch2=avgPitch.add(pitch)
#        print(str(pitch2) + " .. " + str(yaw2))
        motion.setAngles(["HeadYaw", "HeadPitch"], [yaw2,pitch2], 0.5)
        time.sleep(0.01)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
def main(robotIP, PORT = 9559):
    motion = ALProxy("ALMotion", robotIP, PORT)
    motion.setStiffnesses("LShoulder", 1.0)
    print motion.getBodyNames("Body")
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    avgYaw=MovingAverage(AVG_WINDOW_SIZE)
    avgPitch=MovingAverage(AVG_WINDOW_SIZE)
    with closing(sock):
         sock.bind(("", PORT_CONST_HEAD_Action))
         while True:
             msg = sock.recv(bufsize)
             #print("************************* Head:"+msg)
             a=-math.radians(float(msg.split(",")[1]))
             b=math.radians(float(msg.split(",")[0]))
             a=clamp(a,-math.pi,math.pi)
             b=clamp(b,-math.pi,math.pi)
#            yaw=avgYaw.add(yaw)
#            pitch=avgPitch.add(pitch)
#             print(str(pitch) + " .. " + str(yaw))
             motion.setAngles(["LShoulderRoll", "LShoulderPitch"], [a,b], 0.3)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
def main(robotIP, PORT=9559):

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Initialize the move process.
    # Check the robot pose and take a right posture.
    # This is blocking called.
    motionProxy.moveInit()

    testTime = 10.0 # seconds
    t  = 0.0
    dt = 0.2

    while t<testTime:
        # WALK
        X         = random.uniform(0.4, 1.0)
        Y         = random.uniform(-0.4, 0.4)
        Theta     = random.uniform(-0.4, 0.4)
        Frequency = random.uniform(0.5, 1.0)
        try:
            motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
        except Exception, errorMsg:
            print str(errorMsg)
            print "This example is not allowed on this robot."
            exit()

        # JERKY HEAD
        motionProxy.setAngles("HeadYaw", random.uniform(-1.0, 1.0), 0.6)
        motionProxy.setAngles("HeadPitch", random.uniform(-0.5, 0.5), 0.6)

        t = t + dt
        time.sleep(dt)

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

    CALIBRATION_LIMIT = 50
    GAIN = 400

    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 toRAD(self, angle):
        return angle * almath.TO_RAD

    def __init__(self):
        self.calibration = True
        self.vector = None
        self.rot = None
        self.msg_count = 0
        rospy.init_node("imitate_mykinect", anonymous=True)
        self.myFrame = "imitate_nao"
        # Skeleton Listener
        self.tf = tf.TransformListener()
        self.nao_init()
        self.r = rospy.Rate(20)  # rospy.Rate(1) = 1Hz
        rospy.loginfo("Start tracking for 3s...")
        rospy.sleep(3.0)
        rospy.loginfo("Tracking started!")
        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_r_shoul = "{0}_{1}".format(rospy.get_param("~target_joint", "/right_shoulder"), 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)
                self.j_torso = "{0}_{1}".format(rospy.get_param("~target_joint", "/torso"), i)
                break
            except:
                pass

    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", "35137"))
        # 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.al.setState("disabled")
        self.postureProxy.goToPosture("StandInit", 0.5)

    def nao_imitate(self):
        while not rospy.is_shutdown():
            trans_hand, rot_hand = self.tf.lookupTransform("/openni_depth_frame", self.j_l_hand, rospy.Duration())
            vec_hand = Vector3(*trans_hand)
            trans_torso, rot = self.tf.lookupTransform("/openni_depth_frame", self.j_torso, rospy.Duration())
            vec_torso = Vector3(*trans_torso)

            trans_handR, rot_handR = self.tf.lookupTransform("/openni_depth_frame", self.j_r_hand, rospy.Duration())
            vec_handR = Vector3(*trans_handR)

            x = vec_torso.x - vec_hand.x
            y = vec_torso.y - vec_hand.y
            z = (vec_torso.z - vec_hand.z) * (-1)
            x = (int)(1000 * x / 2.5)
            y = (int)(-1000 * y / 2.5)
            z = (int)(1000 * (z) / 2.5)

            xr = vec_torso.x - vec_handR.x
            yr = vec_torso.y - vec_handR.y
            zr = (vec_torso.z - vec_handR.z) * (-1)
            xr = (int)(1000 * xr / 2.5)
            yr = (int)(-1000 * yr / 2.5)
            zr = (int)(1000 * (zr) / 2.5)
            # print(x)
            # print(y)
            # print(z)
            # print("-----------------")
            try:
                (w1, w2, w3, w4) = self.nao_imitate_with_kinematic_right_hand(
                    self.get_transform_matrix((xr, yr, zr), (rot_handR[0], rot_handR[1], rot_handR[2]))
                )
                self.motionProxy.setAngles(["RShoulderRoll", "RShoulderPitch"], [w2, w1], 0.1)
                self.motionProxy.setAngles(["RElbowRoll", "RElbowYaw"], [w4, w3], 0.1)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setAngles [as 别名]
nao.setJointAngle('HeadYaw',0.0,1.0);

motionProxy = ALProxy("ALMotion", '127.0.0.1', 54321)

motionProxy.setStiffnesses("Head", 1.0)

angleLists = [0.0,  0.6250,  1.0344,  1.1879,  1.1639,  0.9236,  0.5140,  0.0017,  -0.5529,  -1.1045,  -1.4744,  -1.6131,  -1.6652,  -1.6847,  -1.6920,  -1.6947,  -1.6432,  -1.4852,  -1.2284,  -0.8997,  -0.5282,  -0.1389,  0.2500,  0.6267,  0.9177,  1.0269,  1.0678,  0.9781,  0.7125,  0.3072,  -0.1823,  -0.7049,  -1.1729,  -1.4208,  -1.5138,  -1.5486,  -1.4616,  -1.2147,  -0.8401,  -0.3870,  0.0993,  0.5844,  1.0193,  1.2612,  1.3519,  1.3859,  1.3030,  1.0529,  0.6673,  0.1979,  -0.3065,  -0.8090,  -1.2029,  -1.3506,  -1.4060,  -1.4268,  -1.2883,  -0.9879,  -0.5706,  -0.0886,  0.4136,  0.9044,  1.2527,  1.3834,  1.4324,  1.4238,  1.2479,  0.9166,  0.4789,  -0.0135,  -0.5183,  -1.0026,  -1.3130,  -1.4294,  -1.4731,  -1.4401,  -1.2409,  -0.8938,  -0.4483,  0.0455,  0.5471,  1.0171,  1.3045,  1.4123,  1.4527,  1.4084,  1.1985,  0.8437,  0.3939,  -0.1011,  -0.6021,  -1.0551,  -1.3100,  -1.4056,  -1.4415, -1.3751, -1.1438, -0.7742, -0.3166, 0.1804, 0.6791, 1.1061, 1.3132, 1.3908, 1.4200, 1.3209, 1.0579, 0.6658, 0.1961, -0.3043, -0.8005, -1.1877, -1.3329, -1.3874, -1.4078, -1.2638, -0.9569, -0.5338, -0.0474, 0.4578, 0.9502, 1.2873, 1.4137, 1.4611, 1.4430, 1.2579, 0.9205, 0.4799, -0.0127, -0.5159, -0.9979, -1.3063, -1.4220, -1.4654, -1.4331, -1.2346, -0.8878, -0.4422, 0.0518, 0.5539, 1.0226, 1.3069, 1.4135, 1.4534, 1.4072, 1.1953, 0.8394, 0.3891, -0.1059, -0.6064, -1.0575, -1.3094, -1.4038, -1.4392, -1.3706, -1.1370, -0.7658, -0.3073, 0.1900, 0.6887, 1.1127, 1.3141, 1.3896, 1.4179, 1.3151, 1.0484, 0.6538, 0.1827, -0.3181, -0.8138, -1.1966, -1.3402, -1.3940, -1.4142, -1.2673, -0.9574, -0.5322, -0.0446, 0.4610, 0.9533, 1.2890, 1.4149, 1.4621, 1.4439, 1.2587, 0.9213, 0.4807, -0.0120, -0.5153, -0.9975, -1.3063, -1.4221, -1.4656, -1.4337, -1.2357, -0.8893, -0.4439, 0.0500, 0.5521, 1.0214, 1.3067, 1.4137, 1.4538, 1.4082, 1.1971, 0.8416, 0.3916, -0.1034, -0.6040, -1.0559, -1.3092, -1.4043, -1.4399, -1.3722, -1.1397, -0.7691, -0.3110, 0.1862, 0.6849, 1.1102, 1.3139, 1.3902, 1.4189, 1.3176, 1.0524, 0.6589, 0.1884, -0.3122, -0.8081, -1.1928, -1.3370, -1.3911, -1.4114, -1.2657, -0.9571, -0.5328, -0.0457, 0.4598, 0.9522, 1.2884, 1.4145, 1.4618, 1.4435, 1.2584, 0.9210, 0.4804, -0.0123, -0.5156, -0.9976, -1.3063, -1.4221, -1.4655, -1.4334, -1.2352, -0.8886, -0.4432, 0.0508, 0.5529, 1.0219, 1.3068, 1.4136, 1.4537, 1.4078, 1.1963, 0.8407, 0.3905, -0.1044, -0.6050, -1.0566, -1.3093, -1.4041, -1.4396, -1.3715, -1.1385, -0.7677, -0.3094, 0.1879, 0.6866, 1.1113, 1.3140, 1.3900, 1.4185, 1.3165, 1.0507, 0.6567, 0.1860, -0.3147, -0.8106, -1.1944, -1.3384, -1.3924, -1.4126, -1.2664, -0.9573, -0.5325, -0.0452, 0.4603, 0.9527, 1.2887, 1.4147]

timeLists = np.linspace(0.1,  30.0,  num=300).tolist()

names = ["HeadYaw"]

isAbsolute = True

#motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute)

nao.setJointAngle('HeadYaw',0.0,1.0);
i = 0
for a in angleLists:
   names = ["HeadYaw"]
   motionProxy = ALProxy("ALMotion", '127.0.0.1', 54321)
   motionProxy.setStiffnesses("Head", 1.0)
   isAbsolute = True
   print a
   #motionProxy.angleInterpolation(names, a, 0.1, isAbsolute) # blocking call, use when complete trajectory is known
   motionProxy.setAngles(names, a, 1.0) # non-blocking call, reactive control
   time.sleep(0.2)
   i += 1