self.armControl.armOut()
self.armControl.loose(Torso_X,Torso_Y,Height)
self.armControl.armsBack()
self.tts.say("I succeed!")
Focus.headRest(self.IP, self.PORT)
self.motion.rest()
HeadMoveExitFlagGlobal = True
StateChangeExitFlagGlobal = True
else:
self.ReachableFlag = False
print 'TorsoXY is not right in REACHABLE!'
print 'Torso_X', Torso_X
print 'Torso_Y', Torso_Y
time.sleep(1)
threads=[]
IP="127.0.0.1"
PORT=9559
Focus.Initialize(IP, PORT)
# Create new threads
headMove = HeadMove(1, "HeadMove",IP,PORT)
headMove.start()
threads.append(headMove)
stateChange = StateChange(2,"StateChange",IP,PORT)
stateChange.start()
threads.append(stateChange)
# Start Threads
for t in threads:
t.join()
print 'all thread have stopped!'
