Python ALProxy.stiffnessInterpolation方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stiffnessInterpolation [as 别名]
def end():
	global motionProxy
	global post
	global sonarProxy
	global memoryProxy
	pNames = "Body"
	post.goToPosture("Crouch", 1.0)
	time.sleep(1.0)
	pStiffnessLists = 0.0
	pTimeLists = 1.0
	proxy = ALProxy("ALMotion",IP, 9559)
	proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
	#tts.say("exit")
	print
	print "This is the END"
	cameraProxy.unsubscribe(videoClient)
	sys.exit(0)

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

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

    names.append("LShoulderPitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.05978, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.03064, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 1.41891, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LShoulderRoll")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.11808, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.82525, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.33130, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LElbowYaw")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.02152, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.11969, [ 3, -0.22222, 0.09818], [ 3, 0.22222, -0.09818]], [ -1.39138, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LElbowRoll")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.04751, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.06132, [ 3, -0.22222, 0.01381], [ 3, 0.22222, -0.01381]], [ -1.02620, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LWristYaw")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00456, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.01070, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.01070, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LHand")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00021, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00020, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00020, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RShoulderPitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.07367, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.05220, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 1.41286, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RShoulderRoll")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.16571, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.80539, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.33445, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RElbowYaw")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.03379, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.09660, [ 3, -0.22222, -0.13039], [ 3, 0.22222, 0.13039]], [ 1.39130, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RElbowRoll")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.05527, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.05833, [ 3, -0.22222, -0.00307], [ 3, 0.22222, 0.00307]], [ 1.02782, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RWristYaw")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.01990, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00456, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00456, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RHand")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00017, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00016, [ 3, -0.22222, 0.00000], [ 3, 0.22222, -0.00000]], [ 0.00015, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LHipYawPitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00004, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00311, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.00149, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LHipRoll")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00004, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00464, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00464, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LHipPitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00004, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.43561, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.43561, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LKneePitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.00004, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.69946, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.69793, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LAnklePitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.00004, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.34979, [ 3, -0.22222, 0.00153], [ 3, 0.22222, -0.00153]], [ -0.35133, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("LAnkleRoll")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00004, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00004, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00004, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RHipRoll")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00004, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.00303, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00158, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RHipPitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.00311, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.43110, [ 3, -0.22222, 0.00920], [ 3, 0.22222, -0.00920]], [ -0.44030, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RKneePitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.02297, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.69955, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.69341, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RAnklePitch")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ -0.00303, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ -0.34664, [ 3, -0.22222, 0.00460], [ 3, 0.22222, -0.00460]], [ -0.35124, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append("RAnkleRoll")
    times.append([ 0.33333, 1.00000, 1.66667])
    keys.append([ [ 0.00618, [ 3, -0.11111, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00004, [ 3, -0.22222, 0.00000], [ 3, 0.22222, 0.00000]], [ 0.00004, [ 3, -0.22222, 0.00000], [ 3, 0.00000, 0.00000]]])

    try:
      # uncomment the following line and modify the IP if you use this script outside Choregraphe.
      motion = ALProxy("ALMotion", "192.168.105.15", 9559)
      motion.stiffnessInterpolation("Body", 1.0, 0.1)
      motion.angleInterpolationBezier(names, times, keys);
    except BaseException, err:
      print err  

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stiffnessInterpolation [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 stiffnessInterpolation [as 别名]
class BallTracker:
        
    def __init__(self, glade_file_path=c.GLADE_FILE_PATH):
	self.glade_file_path = glade_file_path
            
	# Gtk Builder Init
	self.builder = Gtk.Builder()
	self.builder.add_from_file(self.glade_file_path)
	self.builder.connect_signals(self)

	# Add UI Components
	self.window = self.builder.get_object("ballWindow")

	# Show UI
	self.window.show_all()

	self.myBroker = ALBroker("myBroker","0.0.0.0",0,"10.0.2.3",9559)
        self.motion   = ALProxy("ALMotion")
        self.tracker  = ALProxy("ALRedBallTracker")
        self.vision   = ALProxy("ALVideoDevice")
        self.tts      = ALProxy("ALTextToSpeech")
        self.currentCamera = 0
        self.setTopCamera(self)
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []

    ### Destroy GUI
    def destroy(self, widget):
	print "destroyed"
	Gtk.main_quit()


    def __del__(self, widget):
	self.tracker.stopTracker(self)
	pass

    # If Nao has ball returns True
    def hasBall(self, widget):
        self.checkForBall(self)
        time.sleep(0.5)
        if self.checkForBall(self):
	    print "I see the Ball"
            return True
        else:
	    print "Sorry I cant find the ball"
            return False

    # Nao scans around for the redball
    def searchBall(self, widget):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall(self):
                self.turnToBall(self)
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall(self):
                        self.turnToBall(self)
                        return


    # Nao walks close to ball
    def walkToBall(self, widget):
        ballPosition = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosition[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.turnToBall(self)
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.turnToBall(self)
                self.walkToBall(self)
                break
            #dist = self.getDistance()
            # if dist < 0.1:
            #     self.motion.stopWalk()
            #     self.turnToBall()
            #     self.safePosition()
            #     time.sleep(1)
            #     self.tts.say("You have 5 seconds to move the ball")
            #     time.sleep(5)
            #     # naoWalkToPosition()
            #     self.walkToPosition()
            #     self.setTopCamera()
            #     break
                return


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

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

	def __init__(self):
		IP, PORT = "127.0.0.1", 9559
		self.vx = 0.0
		self.vy = 0.0
		self.vth = 0.0
		self.dt = 0.227
		self.joints = []
		self.io_file = IO_file()
		self.is_recording = False
		self.postureProxy = ALProxy("ALRobotPosture", IP, PORT)
		self.motionProxy = ALProxy("ALMotion", IP, PORT)
		self.motionProxy.setWalkArmsEnabled(True, True)
		self.motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", False]])
		self.motionProxy.setFallManagerEnabled(False)

	def stiffness(self, go_rigide):
		parametre = 1.0 if go_rigide else 0.0
		self.motionProxy.stiffnessInterpolation("Body", parametre, 1.0)

	def go_debout(self):
		self.stiffness(True)
		self.postureProxy.goToPosture("Stand", 0.8)

	def go_assis(self):
		self.stiffness(True)
		self.postureProxy.goToPosture("Sit", 0.8)
		self.stiffness(False)

	def go_ventre(self):
		self.stiffness(True)
		self.postureProxy.goToPosture("LyingBelly", 0.8)
		self.stiffness(False)

	def go_dos(self):
		self.stiffness(True)
		self.postureProxy.goToPosture("LyingBack", 0.8)
		self.stiffness(False)

	def go_dos(self):
		self.stiffness(True)
		self.postureProxy.goToPosture("Crouch", 0.8)
		self.stiffness(False)

	def go_move(self):
		self.motionProxy.setWalkTargetVelocity(self.vx, self.vy, self.vth, 0.8)

	def set_vx(self, vx):	self.vx = vx
	def set_vy(self, vy):	self.vy = vy
	def set_vth(self, vth):	self.vth = vth

	def update_joints(self):	self.joints = self.motionProxy.getAngles("Body", False)

	def save_joints(self, write):
		self.update_joints()
		if write: fichier = self.io_file.init_write_joints()
		self.io_file.add_joints(self.joints, self.dt)
		if write: self.io_file.write_joints(fichier)

	def record(self):
		self.is_recording = not self.is_recording
		if self.is_recording:
			self.recorder = Recorder(self)
			self.recorder.continuer = True
			self.recorder.start()
		else:
			self.recorder.continuer = False
			self.recorder.join()

	def apply_joints_from_file(self, only_last):
		temps, angles = self.io_file.read_joints(only_last)
		if len(angles) == 0 : return
		self.motionProxy.angleInterpolation("Body", angles, temps, True)

	def move_head(self, vx, vy):
		self.motionProxy.angleInterpolation(["HeadYaw", "HeadPitch"], [-0.1 * float(vx), -0.1 * float(vy)], [0.2, 0.2], False)

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

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

            self.logger.info("Uploading file %s to Nao" % filename)
            basename = os.path.basename(filename)
            try:
                ftp.storbinary('STOR %s' % basename, f)
            except:
                self.logger.error("Could not store file (%s) on Nao" % filename)
                f.close()
                ftp.close()
                return False
            self.logger.info("Succesfully uploaded file %s to Nao" % filename)
            f.close()

        ftp.close()
        return True

    def set_stiffness(self,
                      names=['Body', 'Body', 'Body'],
                      stiffnessLists=[0.25, 0.5, 1.0],
                      timeLists=[0.5, 1.0, 1.5]):
        self.set_stifness(names, stiffnessLists, timeLists)

    def set_stifness(self,
                     names=['Body', 'Body', 'Body'],    # Part of the robot to apply to
                     stiffnessLists = [0.25, 0.5, 1.0], # Trajectory of stiffness levels
                     timeLists = [0.5, 1.0, 1.5]):      # Time
        """
        The stiffnessLiss is a list of stiffness levels. Each entry sets the
        stiffnesslevel that should be set at the time specified in the same
        element in the timeLists
        """
        for i in range(len(names)):
            self.__Motion.stiffnessInterpolation(names[i], stiffnessLists[i], timeLists[i])

    def get_range(self, name, radian=False):
        """
        This method wraps the getLimits function of naoqi; it caches the results
        because each call to ALMotion.getLimits takes a lot of time
        """
        if not name in self.__joint_range:
            limits = self.__Motion.getLimits(name)
            self.__joint_range[name] = limits[0]

        val = self.__joint_range[name]
        if not radian:
            val = (val[0] / self.TO_RAD,
                   val[1] / self.TO_RAD)

        return val

    def change_angles(self, names, angles, max_speed, disable_stiffness=False, radians=False):
        """
        This method will change 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

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stiffnessInterpolation [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 stiffnessInterpolation [as 别名]
print "================ Choregraphe's Initialization ================"
print 'Enter your NAO IP'
naoIP = raw_input()
#naoIP = map(str,naoIP.split())
print 'Enter your NAO port'
naoPort = raw_input()
naoPort=int(naoPort)
#naoPort = map(int,naoPort.split())

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

#Go to the posture StandInitZero
posture = 'StandZero'
print 'Posture Initialization : ' + posture
motionProxy.stiffnessInterpolation('Body', 1.0, 1.0)
postureProxy.goToPosture(posture,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_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=[]
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)
print "================ Handles Initialization ================"
commandAngles = motionProxy.getAngles('Body', False)
print '========== NAO is listening =========='

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

    def __init__(self, name):
        ALModule.__init__(self, name)
        self.name = name
        self.tts = ALProxy("ALTextToSpeech")
        self.asr = ALProxy("ALSpeechRecognition")
        self.memory = ALProxy("ALMemory")
        self.posture = ALProxy("ALRobotPosture")
        self.motion = ALProxy("ALMotion")
        self.minimumPeople = 1
        self.numbers = ["1", "2", "3", "4", "5", "6", "7", "8", "9"]
        self.many = ["many", "lot", "all", "lots", "everybody"]
        self.wordRecognizedCallback = None
        self.wordRecognizedMinimumConfidence = 0.0
        self.lastPhoto = False
        self.pickedUp = False
        self.placed = False
        self.framing = ""

    def start(self):
        self.posture.goToPosture("StandInit", 0.5)
        self.waitForPhotoWord()

    def stop(self):
        try:
            self.memory.unsubscribeToEvent("WordRecognized", self.name)
        except RuntimeError:
            pass
        self.tts.say("You terminated me!")
        self.posture.goToPosture("Sit", 0.5)
        self.motion.stiffnessInterpolation("Body", 0.0, 1.0)
 
    def waitForWords(self, triggerWords, callback, minimumConfidence=0.5):
        try:
            self.memory.unsubscribeToEvent("WordRecognized", self.name)
        except:
            pass
        self.asr.setAudioExpression(False)
        self.asr.setVocabulary(triggerWords, True)
        self.wordRecognizedCallback = callback
        self.wordRecognizedMinimumConfidence = minimumConfidence
        self.memory.subscribeToEvent("WordRecognized", self.name, "onWordRecognized")

    def onWordRecognized(self, eventName, value, subscriberIdentifier):
        recognizedWord = value[0]
        confidence = value[1]
        print "onWordRecognized: '%s' with confidence %f" % (recognizedWord, confidence)

        if confidence < 0.5:
            return
        else:
            self.memory.unsubscribeToEvent("WordRecognized", self.name)
            self.wordRecognizedCallback(recognizedWord)

    def waitForPhotoWord(self):
        triggers = ["photo", "photograph", "picture", "shot", "portrait"]
        self.waitForWords(triggers, self.onPhotoWordRecognized, 0.3)

    def onPhotoWordRecognized(self, recognizedWord):
        print "onPhotoWordRecognized"
        self.tts.say("Of course!")
        HeadMove.start(self.askPeopleNumber)

    def askPeopleNumber(self):
        print "askPeopleNumber"
        self.tts.say("How many people will be on the photo?")
        self.waitForWords(self.numbers + self.many, self.onPeopleNumberWordRecognized, 0.5)

    def onPeopleNumberWordRecognized(self, recognizedWord):
        print "onPhotoWordRecognized"

        reactions = None
        if (recognizedWord in self.many):
            self.minimumPeople = 2
        else:
            self.minimumPeople = int(recognizedWord)

            if self.minimumPeople == 1:    
                reactions = ["It is just you and me then!",
                             "I love taking photos of you!",
                             "Let's make a very personal photo."]
            elif self.minimumPeople == 2:
                reactions = ["You make such a cute couple!",
                             "Let's capture this moment."]
            elif self.minimumPeople == 3:
                reactions = ["It is going to be an incredible trio!",
                             "Go team!"]
        if not reactions:
            reactions = ["Great! I love group shots!",
                         "That's one big family!",
                         "Let's see if we can fit that many!"]

        reaction = random.choice(reactions)
        self.tts.say(reaction)

        questions = ["Where do you want to take the picture?",
                     "Would you like to do it here?"]
        self.tts.say(random.choice(questions))
        self.decideOnNextPhoto()
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stiffnessInterpolation [as 别名]
    
    if (numJoints == 26):
        angles = allAngles
    elif (numJoints == 22):  # no hands (e.g. simulator)
        angles = allAngles[0:6] + allAngles[8:24]
    else:
        print "error in numJoints"
            
    try:
        motionProxy.angleInterpolation('Body', angles, 1.5, True);

    except RuntimeError,e:
        print "An error has been caught"
        print e

    motionProxy.stiffnessInterpolation('Body',0, 0.5)
    
##################################################################################
## Allows Nao to move in a certain direction with a certain speed.
################################################################################
def Move(dx, dtheta=0, dy=0 ,freq=1):
    """"
    dx = forward speed, dtheta = rotational speed,
    dy = sidewards speed, freq = step frequency.
    Allows Nao to move in a certain direction
    with a certain speed.
    """
    
    motionProxy.setWalkTargetVelocity(dx, dy, dtheta, freq)

##################################################################################

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

#.........这里部分代码省略.........
			cv.Erode(thresholded_img,thresholded_img, None, closing)
			cv.Dilate(thresholded_img,thresholded_img, None, closing)
			

			storage = cv.CreateMemStorage(0)
			contour = cv.FindContours(thresholded_img, storage, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE)
			points = [] 

			d=[]
			data=[]
			while contour:			
				# Draw bounding rectangles
				bound_rect = cv.BoundingRect(list(contour))
				contour = contour.h_next()
				# for more details about cv.BoundingRect,see documentation
				pt1 = (bound_rect[0], bound_rect[1])
				pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
				points.append(pt1)
				points.append(pt2)
				cv.Rectangle(cvImg, pt1, pt2, cv.CV_RGB(255,0,0), 1)
				lastx=posx
				lasty=posy
				posx=cv.Round((pt1[0]+pt2[0])/2)
				posy=cv.Round((pt1[1]+pt2[1])/2)
				data.append([posx,posy])
				d.append(math.sqrt(pt1[0]**2+pt2[0]**2))
				d.append(math.sqrt(pt1[1]**2+pt2[1]**2))

			cvImg,error,centroid,labels = clustering(data,cvImg,nframe,error,K)
			# Update the closing size towards the number of found labels
			if labels.size<2:
				closing=1
			if labels.size<6:
				closing = 2
			if labels.size>10:
				closing=3
			if closing < 1:
				closing = 0
			if centroid.size!=0:
		
				uh = 0
				try:
					x = int(centroid[0][0])
					y = int(centroid[0][1])
				except:
					print "NaN"
				l=memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
				# Same thing for right.
				r=memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
				kh = 30.0/(imageWidth/2)
				km = 0.2
				uh = -kh*(x - imageWidth/2)
				um =  km*(math.sqrt(l**2+r**2) - 0.5)
				print "um: ",um

				if (uh>4 or uh<-4):
					motionProxy.moveTo(0.0, 0.0, uh*almath.TO_RAD)
					tu = time.time()
				if (l>0.5 and r>0.5):
					motionProxy.moveTo(um, 0.0, 0.0)
					tu = time.time()


				else:
					print "-------------------------"
					print "K = 1"
					K=1
					tstp = time.time()
				print l,r

				# quand il est proche mettre K=1, on admet qu il n y a plus de parasites
				
				tact = tstp - tu

				if tact>5:
					found=False
					tts.say("You can't hide forever")
					post.goToPosture("StandInit", 1.0)

				#print "diff",x-imageWidth/2
				#print imageWidth,imageHeight
				

			cv.ShowImage("Real",cvImg)
			cv.ShowImage("Threshold",thresholded_img2)
			cv.WaitKey(1)

	except KeyboardInterrupt:
		pNames = "Body"
		post.goToPosture("Crouch", 1.0)
		time.sleep(1.0)
		pStiffnessLists = 0.0
		pTimeLists = 1.0
		proxy = ALProxy("ALMotion",IP, 9559)
		proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
		#tts.say("exit")
		print
		print "Interrupted by user, shutting down" 
		cameraProxy.unsubscribe(videoClient)
		sys.exit(0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stiffnessInterpolation [as 别名]
            walk_proxy.setStiffness(1.0)
        elif nao_action == 2:
            walk_proxy.setStiffness(0.0)
        elif nao_action == 3:
            walk_proxy.initPosition()
        elif nao_action == 4:
            walk_proxy.stopWalking()
        elif nao_action == 5:
            walk_proxy.walk()
        elif nao_action == 6:
            walk_proxy.setStiffness(0.5)
        elif nao_action == 7:
            walk_proxy.setStiffness(1.0)
            walk_proxy.initPosition()
        elif nao_action == 8:
            motion_proxy.stiffnessInterpolation("Body", 1.0, 0.1)
            motion_proxy.setWalkArmsEnabled(False, False)
            # enable motion whe lifted in the air
            motion_proxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", False]])
            motion_proxy.walkInit()

            # (X length, Y length, theta, frequency)
            motion_proxy.walkTo(0.8, 0.0, 0.0);
        elif nao_action == 9:
            # reset stiffness and angles using motion proxy,
            # otherwise it doesn't work well later
            motion_proxy.stiffnessInterpolation("Body", 0.0, 1.0)
            numAngles = len(motion_proxy.getJointNames("Body"))
            angles = [0.0] * numAngles
            motion_proxy.angleInterpolationWithSpeed ("Body", angles, 0.3)

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

#.........这里部分代码省略.........
			minPosLeftx = -MAXPOSX
			maxPosRightx = MAXPOSX
			minPosRightx = -MAXPOSX
			
		if handSide == constante.RIGHT_HAND:
			#ElbowRoll
			self.setAngleRight(angles, ELBOWROLL, 0.0, 1.5, 75, -40, palmPosz)
			
			#ShoulderRoll
			if palmPosx > maxPosRightx:
				if angles[0] <= -1.5:
					angles[0] = -1.5
				angles[0] -= 0.1
			elif palmPosx < minPosRightx:
				if angles[0] >= 0.0:
					angles[0] = 0.0
				angles[0] += 0.1
			
		else:
			#handSide = left
			
			#ShoulderRoll
			if palmPosx > maxPosLeftx:
				if angles[0] >= 1.5:
					angles[0] = 1.5
				angles[0] -= 0.1
			elif palmPosx < minPosLeftx:
				if angles[0] <= 0.0:
					angles[0] = 0.0
				angles[0] += 0.1
				
			#ElbowRoll
			self.setAngleLeft(angles, ELBOWROLL, -1.5, 0.0, 75, -40, palmPosz)
			
		#ShoulderPitch	
		self.setAngleLeft(angles, SHOULDERPITCH, -1.5, 1.5, 350, 150, palmPosy)
		
		if 0.78 > normal > -0.78:
			angles[3] = -normal
			
		for i in range(len(angles)):
			speed = abs(angles[i]-current[i])/2.0
			if speed < 0.05:
				speed = 0.05
			if speed > 0.5:
				speed = 0.5
				
			name = self.rightArmNames[i]
			if handSide == constante.LEFT_HAND:
				name = self.leftArmNames[i]
				
			# Make the movement
			self.motionProxy.setAngles(name, angles[i], speed)
	
	def setAngleRight(self, angles, indice, valueMin, valueMax, posMax, posMin, palmPos):
		if palmPos > posMax:
			if angles[indice] >= valueMax:
				angles[indice] = valueMax
			angles[indice]+= 0.1
		elif palmPos < posMin:
			if angles[indice] <= valueMin:
				angles[indice] = valueMin
			angles[indice] -= 0.1
		
	def setAngleLeft(self, angles, indice, valueMin, valueMax, posMax, posMin, palmPos):
		if palmPos > posMax:
			if angles[indice] <= valueMin:
				angles[indice] = valueMin
			angles[indice] -= 0.1
		elif palmPos < posMin:
			if angles[indice] >= valueMax:
				angles[indice] = valueMax
			angles[indice] += 0.1
						
	def setCollisionEnabled(self, chainName):
		#activate "Arms" anticollision
		enable    = True
		isSuccess  = self.motionProxy.setCollisionProtectionEnabled(chainName, enable)
		return isSuccess	
				
	def turnMotorOn(self):
		#Turn on the nao's motors
		names  = 'Body'
		stiffnessLists  = 1.0
		timeLists  = 1.0
		self.motionProxy.stiffnessInterpolation(names, stiffnessLists, timeLists)
		self.motorOn = True
		
	def testMotorOn(self):
		if not self.motorOn:
			self.turnMotorOn()
			
	def stiffnessOff(self):
		names  = 'Body'
		stiffnessLists  = 0.0
		timeLists  = 1.0
		self.motionProxy.stiffnessInterpolation(names, stiffnessLists, timeLists)
		self.motorOn = False
		
		self.behaviourProxy.stopAllBehaviors()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stiffnessInterpolation [as 别名]
class NaoWalk():
    # INCOMPLETE AT: walkUpToBall(), include MCC-CALL and delete walkToPosition()

    # Usage description:
    # When the NaoWalk Phase started, call
    #       hasBall()
    #
    # to determine if NAO has the Redball in vision.
    # If not, call
    #       retrieveBall()
    #
    # to find and turn to the ball
    # If NAO has a ball in vision, call
    #       walkUpToBall()
    #
    # NAO will walk up closely to the Redball and call the MCC to
    # request the NXT to move to his next position
    # After the NXT left his initial Position, call
    #       walkToPosition()
    #
    # and NAO makes his few last steps.
    #
    # Repeat this progress until NAO has reached
    # the target area.

    def __init__(self):
        self.myBroker = ALBroker("myBroker","0.0.0.0",0,"194.95.174.146",9559)
        self.motion = ALProxy("ALMotion")
        self.tracker = ALProxy("ALRedBallTracker")
        self.vision = ALProxy("ALVideoDevice")
        self.tts = ALProxy("ALTextToSpeech")
        self.currentCam = 0
        self.__setTopCamera()
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []
        # ALModule.__init__(self,name)

    def __del__(self):
        self.tracker.stopTracker()
        pass

    # determines whether NAO sees a redball or not
    # returns true is he sees one, false if not
    def hasBall(self):
        self.__checkForBall()
        time.sleep(0.5)
        if self.__checkForBall():
            return True
        else :
            return False

    # NAO scans his environment for the redball
    # after calling, NAO either has a redball in vision or there is none in
    # his range of sight.
    # Maybe include MCC-CALL after failure
    def retrieveBall(self):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall():
                self.__turnToBall()
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall():
                        self.__turnToBall()
                        return

    # lets the nao walk close up to the ball
    # Redball in vision is mandatory to call this method!
    # !! NEED TO INCLUDE MCC-CALL TO MOVE NXT TO NEXT POSITION IN LINE 85 !!
    def walkUpToBall(self):
        ballPosi = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosi[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.__turnToBall()
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.__turnToBall()
                self.walkUpToBall()
                break
            dist = self.__getDistance()
            if dist < 0.65:
                self.motion.stopWalk()
                self.__turnToBall()
                self.__safePosition()
#                    # !!
#                    # meldung ans mcc dass nxt weiterlaufen soll
#.........这里部分代码省略.........