Python ALProxy.moveToward方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [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 walk process.
    # Check the robot pose and take a right posture.
    # This is blocking called.
    motionProxy.moveInit()

    # TARGET VELOCITY
    X         = 1.0
    Y         = 0.0
    Theta     = 0.0
    Frequency = 1.0

    # Default walk (MaxStepX = 0.04 m)
    try:
        motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
    except Exception, errorMsg:
        print str(errorMsg)
        print "This example is not allowed on this robot."
        exit()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [as 别名]
def MoveAction_MoveFor(robotIP, PORT=9559):
    # 前进
    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)


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

    #####################
    ## Enable arms control by Motion 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]])

    #TARGET VELOCITY
    X = 0.8
    Y = 0.0
    Theta = 0.0
    Frequency =1.0 # max speed
    try:
        motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
    except Exception, errorMsg:
        print str(errorMsg)
        print "This example is not allowed on this robot."
        exit()

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

    # TARGET VELOCITY
    X         = 1.0
    Y         = 0.0
    Theta     = 0.0
    Frequency = 1.0

    # Defined a limp walk
    try:
        motionProxy.moveToward(X, Y, Theta,[["Frequency", Frequency],
                                            # LEFT FOOT
                                            ["LeftStepHeight", 0.02],
                                            ["LeftTorsoWy", 5.0*almath.TO_RAD],
                                            # RIGHT FOOT
                                            ["RightStepHeight", 0.005],
                                            ["RightMaxStepX", 0.001],
                                            ["RightMaxStepFrequency", 0.0],
                                            ["RightTorsoWx", -7.0*almath.TO_RAD],
                                            ["RightTorsoWy", 5.0*almath.TO_RAD]] )
    except Exception, errorMsg:
        print str(errorMsg)
        print "This example is not allowed on this robot."
        exit()

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

    # Wake up robot
    motionProxy.wakeUp()

# 	motionProxy.moveInit()

    # Example showing the use of moveToward
    # The parameters are fractions of the maximums
    # Here we are asking for full speed forwards, 即x = 1.0
    x     = 1.0
    y     = 0.0
    theta = 0.0
    frequency = 1.0
    motionProxy.moveToward(x, y, theta, [["Frequency", frequency]])

    # If we don't send another command, he will move forever
    # Lets make him slow down(step length) and turn after 3 seconds
    time.sleep(3) # 延时3秒运动

    x     = 0.5
    theta = 0.6
    motionProxy.moveToward(x, y, theta, [["Frequency", frequency]])

    # Lets make him slow down(frequency) after 3 seconds
    time.sleep(3)
    frequency = 0.5 # 频率1为最快, 0.5表示50%的频率运动
    motionProxy.moveToward(x, y, theta, [["Frequency", frequency]])

    # Lets make him stop after 3 seconds
    time.sleep(3)
    motionProxy.stopMove()

	# 详细的Move Config配置
	##################################
	#
    # TARGET VELOCITY
    X         = 1.0
    Y         = 0.0
    Theta     = 0.0
    Frequency = 1.0

    # Defined a limp walk
    try:
        motionProxy.moveToward(X, Y, Theta,[["Frequency", Frequency],
                                            # LEFT FOOT
                                            ["LeftStepHeight", 0.02],
                                            ["LeftTorsoWy", 5.0*almath.TO_RAD],
                                            # RIGHT FOOT
                                            ["RightStepHeight", 0.005],
                                            ["RightMaxStepX", 0.001],
                                            ["RightMaxStepFrequency", 0.0],
                                            ["RightTorsoWx", -7.0*almath.TO_RAD],
                                            ["RightTorsoWy", 5.0*almath.TO_RAD]] )
    except Exception, errorMsg:
        print str(errorMsg)
        print "This example is not allowed on this robot."
        exit()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [as 别名]
def main(robotIP, port=9559):
    sonar = Sonar(robotIP, port)
    controler = Controller()
    motionProxy = ALProxy("ALMotion", robotIP, port)
    postureProxy = ALProxy("ALRobotPosture", robotIP, port)
    frame = motion.FRAME_ROBOT # maybe test TORSO?

    # Get ready, NAO!
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)

    start_t = time.time()

    # Start walking (sonar appears to start reading correct values only after this)
    motionProxy.moveToward(0.01, 0, 0)
    time.sleep(0.5)

    # Main loop
    while (time.time() - start_t <= 180):
        # make course corrections
        # async walk
        # little sleep

        # Sensor reading step
        x_velocity = 1
        y_velocity = 0
        theta_correction = 0

        lread, rread = sonar.getSampleReading(n_readings=10)

        # Course correction decision step (theta is positive counterclockwise)
        print("Sonar readings (lread, rread) = (%.2f, %.2f)" % (lread, rread))
        theta_correction = controler.compute(lread=lread, rread=rread)
        print("Theta %.2f" % theta_correction)

        # Course correction execution step
        motionProxy.moveToward(x_velocity, y_velocity, theta_correction)

        time.sleep(0.5)

    # Stop motion
    motionProxy.stopMove()

    # Just to indicate we're finished
    motionProxy.rest()

    print("NAOvigation script finished.")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [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 moveToward [as 别名]
def main(robotIP1,robotIP2, PORT=9559):

    motionProxy1  = ALProxy("ALMotion", robotIP1, PORT)
    postureProxy1 = ALProxy("ALRobotPosture", robotIP1, PORT)
    motionProxy2  = ALProxy("ALMotion", robotIP2, PORT)
    postureProxy2 = ALProxy("ALRobotPosture", robotIP2, PORT)
    # Wake up robot
    motionProxy1.wakeUp()
    motionProxy2.wakeUp()
    # Send robot to Stand Init
    postureProxy1.goToPosture("StandInit", 0.5)
    postureProxy2.goToPosture("StandInit", 0.5)
    lm = Leap.Controller()
    
    while True:
        frame = lm.frame()
        y = 0.0
        p = 0.0
        for fin in frame.fingers:
            if fin.hand.is_left and fin.type == fin.TYPE_INDEX:
                y=fin.direction.yaw
                p=fin.direction.pitch
                print y
        # TARGET VELOCITY
        X         = sin(2*p)
        Y         = 0.0
        Theta     = -sin(y)
        Frequency = 4

        try:
            motionProxy1.moveToward(X, Y, Theta)
            motionProxy2.moveToward(X, Y, Theta)
        except Exception, errorMsg:
            print str(errorMsg)
            print "This example is not allowed on this robot."
            exit()

        time.sleep(0.05)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [as 别名]
def main(robotIP, port=9559):
    sonar = Sonar(robotIP, port)
    controler = Controller()
    motionProxy = ALProxy("ALMotion", robotIP, port)
    postureProxy = ALProxy("ALRobotPosture", robotIP, port)
    frame = motion.FRAME_ROBOT # maybe test TORSO?
    last_theta = 0
    memory_factor = 0.3

    # Get ready, NAO!
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)

    start_t = time.time()

    # Start walking (sonar appears to start reading correct values only after this)
    motionProxy.moveToward(0.01, 0, 0)
    time.sleep(0.5)

    # Main loop
    while (time.time() - start_t <= 180):
        # Sensor reading step
        x_velocity = 1
        y_velocity = 0
        theta_correction = 0

        lread, rread = sonar.getSampleReading(n_readings=10)

        # Course correction decision step (theta is positive counterclockwise)
        print("Sonar readings (lread, rread) = (%.2f, %.2f)" % (lread, rread))
        theta_correction, v_correction = controler.compute(lread=lread, rread=rread, last_theta=last_theta)
        print("Theta %.2f; Velocity %.2f" % (theta_correction, v_correction))

        # Course correction execution step
        #motionProxy.moveToward(v_correction, y_velocity, theta_correction)
        motionProxy.moveToward(v_correction, y_velocity, (memory_factor * last_theta + (1 - memory_factor) * theta_correction))

        last_theta = theta_correction

        time.sleep(0.5)

    # Stop motion
    motionProxy.moveToward(0, 0, 0) # I found this to be less motion breaking
    motionProxy.stopMove()
    postureProxy.goToPosture("StandInit", 0.2) # make him stand again

    # Just to indicate we're finished
    motionProxy.rest()

    print("NAOvigation script finished.")

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

from manage_joints import *


DEFAULT_FILE_TO_WRITE = "robot_training_data"
DEFAULT_QTD_OF_MOVEMENTS = 10000

naoIP = '127.0.0.1'
naoPort = 59193
motionProxy = ALProxy("ALMotion",naoIP, naoPort)
postureProxy = ALProxy("ALRobotPosture", naoIP, naoPort)
posture = 'StandZero'



postureProxy.goToPosture(posture,1.0)

f = file(DEFAULT_FILE_TO_WRITE, 'w')
motionProxy.moveToward(1,0,0)

for i in range(0, DEFAULT_QTD_OF_MOVEMENTS):
    angles = motionProxy.getAngles('Body', False)
    line = ' '.join(str(x) for x in [i] + angles) + '\n'
    time.sleep(0.1)
    f.write(line)
    print line

f.close()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [as 别名]
def runSimulation(cromossomos):
    idx = 0
    for cromossomo in cromossomos:
        #commands.getoutput('~/Downloads/choregraphe-suite-2.1.4.13-linux64/bin/naoqi-bin -p 50004')
        #subprocess.Popen('ls')
        #subprocess.Popen('/home/gabriel/Downloads/choregraphe-suite-2.1.4.13-linux64/bin/naoqi-bin')
        #subprocess.call('./test.sh&', shell=True)
        vrep.simxFinish(-1)
        clientID=vrep.simxStart('127.0.0.2', 50003, True, True, 5000, 5)

        if clientID != -1:
            print 'Connected to remote API server'
            # vrep.simxLoadScene(clientID,"/home/gabriel/MC906/trabalho/mc906/scenes/test.ttt",1,vrep.simx_opmode_oneshot_wait)
            vrep.simxStartSimulation(clientID,vrep.simx_opmode_streaming)
        else:
            print 'Connection was not successful'
            sys.exit('Could not connect')

        naoIP = "127.0.0.1"
        naoPort = 50004

        motionProxy = ALProxy("ALMotion", naoIP, naoPort)
        postureProxy = ALProxy("ALRobotPosture", naoIP, naoPort)

        # Go to the posture StandInitZero
        posture = 'StandZero'
        motionProxy.stiffnessInterpolation('Body', 1.0, 1.0)

        Head_Yaw = []
        Head_Pitch = []
        L_Hip_Yaw_Pitch = []
        L_Hip_Roll = []
        L_Hip_Pitch = []
        L_Knee_Pitch = []
        L_Ankle_Pitch = []
        L_Ankle_Roll = []
        R_Hip_Yaw_Pitch = []
        R_Hip_Roll = []
        R_Hip_Pitch = []
        R_Knee_Pitchi = []
        R_Ankle_Pitch = []
        R_Ankle_Roll=[];
        L_Shoulder_Pitch = []
        L_Shoulder_Roll = []
        L_Elbow_Yaw = []
        L_Elbow_Roll = []
        L_Wrist_Yaw = []
        R_Shoulder_Pitch = []
        R_Shoulder_Roll = []
        R_Elbow_Yaw=[]
        R_Elbow_Roll = []
        R_Wrist_Yaw = []
        R_H = []
        L_H = []
        R_Hand=[]
        L_Hand=[]
        Body = [Head_Yaw,
                Head_Pitch,
                L_Hip_Yaw_Pitch,
                L_Hip_Roll,
                L_Hip_Pitch,
                L_Knee_Pitch,
                L_Ankle_Pitch,
                L_Ankle_Roll,
                R_Hip_Yaw_Pitch,
                R_Hip_Roll,
                R_Hip_Pitch,
                R_Knee_Pitch,
                R_Ankle_Pitch,
                R_Ankle_Roll,
                L_Shoulder_Pitch,
                L_Shoulder_Roll,
                L_Elbow_Yaw,
                L_Elbow_Roll,
                L_Wrist_Yaw,
                R_Shoulder_Pitch,
                R_Shoulder_Roll,
                R_Elbow_Yaw,
                R_Elbow_Roll,
                R_Wrist_Yaw,
                L_H,
                L_Hand,
                R_H,
                R_Hand]

        get_first_handles(clientID, Body)

        # TARGET VELOCITY
        X         = 1.0
        Y         = 0.0
        Theta     = 0.0
        Frequency = 1.0

        # motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
        postureProxy.applyPosture('StandZero',0.1)
        motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency],
                                             ["MaxStepX", 0.01 + cromossomo[0]*0.079],
                                             ["LeftStepHeight", 0.005 + cromossomo[1]*0.035],
                                             ["RightStepHeight", 0.005 + cromossomo[1]*0.035]])

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

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [as 别名]
class planner_move(object):
    global mark
    mark = []
    def __init__(self, ip):
        self.ears = planner_voice()
        self.sonarProxy = ALProxy("ALSonar", ip, 9559)
        self.memoryProxy = ALProxy("ALMemory", ip, 9559)
        self.motionProxy = ALProxy("ALMotion", ip, 9559)
        self.speechProxy = ALProxy("ALTextToSpeech", ip, 9559)
        self.sensorProxy = ALProxy("ALSensors", ip, 9559)
        #self.compassProxy = ALProxy("ALVisualCompass", ip, 9559)
        self.markProxy = ALProxy("ALLandMarkDetection", ip, 9559)
        self.rpos = ALProxy("ALRobotPosture", ip, 9559)
        self.sonarProxy.subscribe("planner_move.py")
        self.sensorProxy.subscribe("planner_move.py")
        #self.compassProxy.subscribe("planner_move.py")
        self.markProxy.subscribe("planner_move.py", 500, 0.0)
        self.motionProxy.stiffnessInterpolation("Body", 0, 1)
        self.rpos.goToPosture("Stand", 1.0, 0)
        self.motionProxy.moveInit()
        self.queue = Queue.Queue()
        thread = threading.Thread(group = None, target = self.ears.run, name = 'thread1', args = (ip, self.queue), kwargs = {})
        thread.daemon = True
        thread.start()
    
    def main(self):
        self.moveForward()
        dataStructure = self.obstacleDetection()
        return dataStructure
        
    def moveForward(self):
        self.motionProxy.moveToward(.5, .02, -.04)
        return
        
    def obstacleDetection(self):
        while(True):
            front = self.memoryProxy.getData("FrontTactilTouched")
            middle = self.memoryProxy.getData("MiddleTactilTouched")
            rear = self.memoryProxy.getData("RearTactilTouched")
            left = self.memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
            right = self.memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
            #direction = self.memoryProxy.getData("VisualCompass/Deviation")
            detection = self.memoryProxy.getData("LandmarkDetected", 0)
            self.touched(front, middle, rear)
            dataStructure = self.landmark(detection)
            if not self.queue.empty():
                dS = self.commanded()
                if dS:
                    if dataStructure == []:
                        dataStructure = dS
                    else:
                        dataStructure[0] = dS[0]
            if not dataStructure == []:
                self.motionProxy.moveToward(0, 0, 0)
                return dataStructure
            self.footBumper()
            self.leftorright(left, right)
            #self.compass(direction)
        return
            
    def leftorright(self, left, right):
        if left == right:
            if left > .25 and left < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.decision(left, right)
                self.motionProxy.moveToward(.5, .02, -.03)
            return
        if left > right:
            if right > .25 and right < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.motionProxy.moveTo(0, 0, .5)
                self.motionProxy.moveToward(.5, .02, -.03)
            return
        if left < right:
            if left > .25 and left < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.motionProxy.moveTo(0, 0, -.5)
                self.motionProxy.moveToward(.5, .02, -.03)
            return
        
    def turn(self, radians):
        self.motionProxy.moveToward(0, 0, 0)
        self.motionProxy.moveTo(0, 0, radians)
        return
    
    def touched(self, front, middle, rear):
        if front == 1 or middle == 1 or rear == 1:
            self.motionProxy.moveToward(0, 0, 0)
            time.sleep(1)
            self.motionProxy.rest()
            sys.exit()
    
    def decision(self, left, right):
        self.motionProxy.moveTo(0, 0, .5)
        l1 = self.memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
        self.motionProxy.moveTo(0, 0, -1)
        r2 = self.memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
        self.motionProxy.moveTo(0, 0, .5)
        if l1 > r2:
            self.motionProxy.moveTo(0, 0, .5)
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
    @staticmethod
    def distanceFromVisualAngle(visualAngle, size):
        return size / (2.0 * math.tan(visualAngle / 2.0))

    @staticmethod
    def sizeFromVisualAngle(visualAngle, distance):
        return 2.0 * distance * math.tan(visualAngle / 2.0)

    @staticmethod
    def sideSize(side1Size, side2Size, oppositeAngle):
        return math.sqrt(side1Size*side1Size + side2Size*side2Size - 2*side1Size*side2Size*math.cos(oppositeAngle))

    @staticmethod
    def oppositeAngle(oppositeSideSize, side1Size, side2Size):
        return math.acos((side1Size*side1Size + side2Size*side2Size - oppositeSideSize*oppositeSideSize) / (2.0 * side1Size * side2Size))

    def onFaceDetected(self, eventName, value, subscriberIdentifier):
        if len(value) == 0:
            # there were faces and now there are none
            return

        event = FaceDetectedEvent(value[0], value[1][:-1], value[4])
        print "onFaceDetected:", "%d faces on camera %d (%d)" % (len(event.faces), event.cameraId, event.timestamp[0])
        shapeInfos = [ FaceShapeInfo(shapeInfoRaw) for shapeInfoRaw in event.faces ]
        sizes = [ shapeInfo.sizeX for shapeInfo in shapeInfos ]
        averageSize = PictureFramerModule.average(sizes)

        #standardFaceHeight = 0.15
        #standardEyeDistance = 0.045
        #standardMouthWidth = 0.045
        #angle = 0.51 # to compute from face size proportions
        #distance = PictureFramerModule.distanceFromVisualAngle(shapeInfos[0].sizeX, standardFaceHeight)
        #eyeAngle = shapeInfos[0].leftEyeX - shapeInfos[0].rightEyeX
        #eyeDistance = PictureFramerModule.sizeFromVisualAngle(eyeAngle, distance)
        #mouthAngle = shapeInfos[0].leftMouthX - shapeInfos[0].rightMouthX
        #mouthWidth = PictureFramerModule.sizeFromVisualAngle(mouthAngle, distance)
        #angle = math.acos(eyeDistance / standardEyeDistance)
        #targetAngle = self.framingSizes[self.framing]
        #targetDistance = PictureFramerModule.distanceFromVisualAngle(targetAngle, standardFaceHeight)
        #walkingDistance = PictureFramerModule.sideSize(distance, targetDistance, angle)
        #walkingAngle = PictureFramerModule.oppositeAngle(targetDistance, walkingDistance, distance)

        if self.framing == "headshot":
            if self.isTooClose(averageSize):
                self.tts.say("I'm a bit too close")
            elif self.isTooFar(averageSize):
                self.tts.say("I'm a bit too far")
        else:
            if self.isTooClose(averageSize):
                print "too close", shapeInfo.sizeX
                if not self.motion.moveIsActive():
                    self.motion.moveToward(-0.8, 0.0, 0.0)
            elif self.isTooFar(averageSize):
                print "too far",shapeInfo.sizeX
                if not self.motion.moveIsActive():
                    self.motion.moveToward(0.8, 0.0, 0.0)
            else:
                self.motion.stopMove()

        if self.previousFaceEvent:
            elapsedSincePrevious = event.timestamp[0] - self.previousFaceEvent.timestamp[0]

            if elapsedSincePrevious <= 4 and \
                len(event.faces) == len(self.previousFaceEvent.faces):

                missingFaces = self.faceNumber - len(event.faces)
                print "missingFaces", missingFaces
                if missingFaces >= 0 and elapsedSincePrevious >= 2:
                    self.missingFaces(missingFaces)
                    return
                else:
                    self.pictureFramed()

        self.previousFaceEvent = event

    def isTooClose(self, faceSize):
        targetSize = self.framingSizes[self.framing]
        return faceSize >= targetSize + self.framingSizeRange

    def isTooFar(self, faceSize):
        targetSize = self.framingSizes[self.framing]
        return faceSize <= targetSize - self.framingSizeRange

    def pictureFramed(self):
        self.faceDetection.unsubscribe(self.name)
        self.memory.unsubscribeToEvent("FaceDetected", self.name)
        print "Picture is framed and ready to be taken."
        reactions = ["Good, that's good, everybody is in.",
                     "Nice, that's right.",
                     "That should be good now."]
        self.tts.say(random.choice(reactions))
        if self.onPictureFramed:
            self.onPictureFramed()

    def missingFaces(self, missingFaces):
        print "Some faces are missing!"
        reactions = ["We are missing people.",
                     "Come on everybody, I cannot see you.",
                     "You are outside the photo!"]
        self.tts.say(random.choice(reactions))

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

    # Initialization of the needed NAO proxies
    def __init__(self, parameters):

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

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

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

    # Enables the NAO motors. The robot cannot move if its motors are not
    # enabled (a.k.a. stifness on).
    def enableMotors(self):
        try:
          self.motion.stiffnessInterpolation('Body', 1.0, 0.5)
        except Exception as e:
            return self.ret_exc("motion.enableMotors: Unrecognized exception: " + \
                e.message)

        return {'error': None}
  
    # Disables motors (a.k.a. stifness off)
    def disableMotors(self):
        try:
          self.motion.stiffnessInterpolation('Body', 0.0, 0.5)
        except Exception as e:
            return self.ret_exc("motion.disableMotors: Unrecognized exception: " + \
                e.message)

        return {'error': None}

    # Moves the NAO robot by velocities (all inputs are bounded to [-1,+1], where
    # +1 is the maximum positive speed and -1 is the maximum negative speed
    def moveByVelocity(self, x_vel, y_vel, theta_vel):

        if x_vel < -1 or x_vel > 1:
            return self.ret_exc('motion.moveByVelocity: x_vel out of bounds')
        if y_vel < -1 or y_vel > 1:
            return self.ret_exc('motion.moveByVelocity: y_vel out of bounds')
        if theta_vel < -1 or theta_vel > 1:
            return self.ret_exc('motion.moveByVelocity: theta_vel out of bounds')

        try:
            self.motion.moveToward(x_vel, y_vel, theta_vel)
        except Exception as e:
            return self.ret_exc("motion.moveByVelocity: Unrecognized exception: " + \
                e.message)

        return {'error': None}

    # Moves the NAO robot to the specified pose. x & y are meters, theta is
    # in radians
    def moveTo(self, x, y, theta):
    
        if type(x) not in [float, int]:
            return self.ret_exc('motion.moveTo: x not a number')
        if type(y) not in [float, int]:
            return self.ret_exc('motion.moveTo: y not a number')
        if type(theta) not in [float, int]:
            return self.ret_exc('motion.moveTo: theta not a number')

        try:
          self.motion.moveTo(x, y, theta)
        except Exception as e:
            return self.ret_exc("motion.moveTo: Unrecognized exception: " + \
                e.message)

        return {'error': None}

    # Stops the NAO motion. May be a bit harsh. It is suggested to use the
    # moveByVelocity(0,0,0) instead.
    def stop(self):
        try:
            self.motion.stopMove()
        except Exception as e:
            return self.ret_exc("motion.stop: Unrecognized exception: " + \
                e.message)

        return {'error': None}

    # Returns the NAO velocities in the three axes (x,y,theta)
    def getVelocities(self):
        try:
            data = self.motion.getRobotVelocity()
            return {'velocities': data, 'error': None}
        except Exception as e:
            return self.ret_exc("motion.getVelocities: Unrecognized exception: " + \
                e.message)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [as 别名]
    def run(self):
        nNaoMove=ALProxy("ALMotion", self.IP, self.PORT)
        postureProxy = ALProxy("ALRobotPosture", self.IP, self.PORT)
        nNaoMove.wakeUp()
        postureProxy.goToPosture("Stand", 0.5)
        pygame.init()
        screen = pygame.display.set_mode((454, 603), 0, 32)
        background = pygame.image.load(r"F:\NaoTest\NaoSource\mythreads\naonew.png").convert_alpha()
        pygame.display.set_caption("Dragon's dance")
        screen.blit(background, (0, 0))
        pygame.display.update()
        while True:
            for event in pygame.event.get():
                if event.type == QUIT:
                    exit()
                elif event.type == KEYDOWN:
                    if event.key == K_UP:
                        x = 0.7
                        y = 0.0
                        theta = 0.0
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])

                    elif event.key == K_DOWN:
                        x = -0.4
                        y = 0.0
                        theta = 0.0
                        frequency = 0.2
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_LEFT:
                        x = 0.0
                        y = 0.2
                        theta = 0.6
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_RIGHT:
                        x = 0.0
                        y = -0.2
                        theta = -0.6
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_q:
                        nNaoMove.stopMove()
                        time.sleep(1)
                        nNaoMove.rest()
                    elif event.key == K_r:
                        nNaoMove.wakeUp()
                        postureProxy.goToPosture("StandInit", 0.5)
                        time.sleep(1.5)
                    elif event.key == K_s:
                        nNaoMove.stopMove()

                    elif event.key == K_z:
                        # 头部左转
                        nNaoMove.setStiffnesses("Head", 1.0)
                        names = "HeadYaw"
                        changes = 0.5
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_c:
                        # 头部右转
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadYaw"
                        changes = -0.5
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_a:
                        # 向上看
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadPitch"
                        changes = 0.2
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_d:
                        # 向下看
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadPitch"
                        changes = -0.2
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_x:
                        # 头部恢复
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadYaw"
                        angles = 0.0
                        fractionMaxSpeed = 0.2
                        nNaoMove.setAngles(names, angles, fractionMaxSpeed)
                        # 左手运动
                    elif event.key == K_1:
                        changes = -0.25
                        fractionMaxSpeed = 0.1
                        nNaoMove.setStiffnesses("LShoulderPitch", 1)
                        nNaoMove.changeAngles("LShoulderPitch", changes, 0.1)  # (关节，改变的角度，快慢）
                    elif event.key == K_2:
                        changes = 0.25
                        nNaoMove.changeAngles("LShoulderPitch", changes, 0.1)
                    elif event.key == K_3:
                        changes = 0.2
                        nNaoMove.setStiffnesses("LShoulderRoll", 1)
                        nNaoMove.changeAngles("LShoulderRoll", changes, 0.1)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveToward [as 别名]
class NaoMotion():
    def __init__(self, host_name, port):
        self.motionProxy = ALProxy("ALMotion", host_name, port)
        self.postureProxy = ALProxy("ALRobotPosture", host_name, port)
        self.ttsProxy = ALProxy("ALTextToSpeech", host_name, port)

        # self.disable_smart_stiffness()

        # Head Motion
        self.head_joints = ["HeadPitch", "HeadYaw"]
        self.head_angles = []

        # Hand Motion
        self.hand_joints = ["LShoulderRoll", "LShoulderPitch", "LElbowRoll", "LElbowYaw",
                            "RShoulderRoll",
                            "RShoulderPitch",
                            "RElbowRoll",
                            "RElbowYaw"]
        self.hand_angles = []
        self.hand_speed = 0.2

        # Body Motion
        self.head_angles = []
        self.forward_direction = 0
        self.sideward_diretion = 0
        self.rotation = 0
        self.step_frequency = 1.0
        self.walk_duration = 5
        self.turn_duration = 4

        # Is the robot sleeping?
        self.wake_up()

    # If robot is sleeping, wake it up
    def wake_up(self):
        if self.motionProxy.robotIsWakeUp() is False:
            self.motionProxy.wakeUp()
            # time.sleep(3)
            # self.postureProxy.goToPosture("Stand", 0.5)
            time.sleep(3)
            self.set_head()
        else:
            self.set_head()

    # Set the head to look at the user
    def set_head(self):
        self.head_angles = [-0.3145120143890381, -0.013848066329956055]
        self.motionProxy.setAngles(self.head_joints, self.head_angles, 0.1)
        time.sleep(2)

    # Reset the head
    def reset_head(self):
        self.head_angles = [0.06438612937927246, -4.1961669921875e-05]
        self.motionProxy.setAngles(self.head_joints, self.head_angles, 0.1)
        time.sleep(2)

    # Reset the hand
    def reset_hand(self):
        self.hand_angles = [0.2638061046600342, 1.4097040891647339, -1.0123980045318604,
                            -1.3806419372558594, -0.2623560428619385, 1.418992042541504,
                            1.0170841217041016, 1.3774900436401367]
        self.motionProxy.setAngles(self.hand_joints, self.hand_angles, self.hand_speed)

    # Wake up using click gesture
    def click_wake_up(self):
        if self.motionProxy.robotIsWakeUp() is False:
            self.motionProxy.wakeUp()
            self.set_head()
        else:
            self.set_head()

    # Set the robot to sleep
    def sleep_robot(self):
        self.motionProxy.rest()

    def get_smart_stiffness(self):
        print self.motionProxy.getSmartStiffnessEnabled()

    def disable_smart_stiffness(self):
        self.motionProxy.setSmartStiffnessEnabled(False)

    # Motion based on given Gesture
    def gesture_to_motion(self, sign):
        if sign == "Walk":
            self.reset_head()
            self.forward_direction = 1.0
            self.sideward_diretion = 0.0
            self.rotation = 0.0
            self.motionProxy.moveToward(self.forward_direction, self.sideward_diretion,
                                        self.rotation,
                                        [["Frequency", self.step_frequency]])

            time.sleep(self.walk_duration)
            self.motionProxy.stopMove()
            time.sleep(2)

            self.set_head()

        elif sign == "Turn Right":
            self.reset_head()
#.........这里部分代码省略.........