def stopSound( path ):
# Path verarbeiten
path = str(path)
path = path.strip()
print "stop soundfile "+path+""
# Liste der abspielenden Sound durchgehen
for sound in Sounds:
# Gesuchter Vorgang?
if sound[0] == path:
# Verbindung mit ALAudioPlayer herstellen
player = ALProxy('ALAudioPlayer', config.naoIP, config.naoPort, True )
# Abspielvorgang stoppen
try:
player.stop( sound[1] )
except:
return False
# Apsielvorgang wider aus Liste entfernen
Sounds.remove( [sound[0], sound[1]] )
return True
return False
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 = []
def initALModule(self):
print "[Core agent server] - Initialization of Naoqi modules"
print "[Core agent server] - ALMemory proxy initialization"
global prox_memory
prox_memory = ALProxy("ALMemory")
if prox_memory is None:
rospy.logerr("[Core agent server] - Could not get a proxy to ALMemory")
exit(1)
print "[Core agent server] - ALTextToSpeech proxy initialization"
self.prox_tts = ALProxy("ALTextToSpeech")
if self.prox_tts is None:
rospy.logerr("[Core agent server] - Could not get a proxy to ALTextToSpeech")
exit(1)
print "[Core agent server] - ALSoundDetection proxy initialization"
self.prox_sd = ALProxy("ALSoundDetection")
if self.prox_sd is None:
rospy.logerr("[Core agent server] - Could not get a proxy to ALSoundDetection")
exit(1)
print "[Core agent server] - ALSpeechRecognition proxy initialization"
self.prox_sprec = ALProxy("ALSpeechRecognition")
if self.prox_sprec is None:
rospy.logerr("[Core agent server] - Could not get a proxy to ALSpeechRecognition")
exit(1)
print "[Core agent server] - ALAudioRecorder proxy initialization"
self.prox_ar = ALProxy("ALAudioRecorder")
if self.prox_ar is None:
rospy.logerr("[Core agent server]- Could not get a proxy to ALAudioRecorder")
exit(1)
class TactileHeadModule(ALModule):
AudioModule = None
def __init__(self, name, audiomodule):
ALModule.__init__(self, name)
self.AudioModule = audiomodule
# Create a proxy to ALTextToSpeech for later use
self.tts = ALProxy("ALTextToSpeech")
# Subscribe to TouchChanged event:
global memory
memory = ALProxy("ALMemory")
memory.subscribeToEvent("MiddleTactilTouched",
"ReactToTouch",
"onTouched")
def onTouched(self, strVarName, value):
""" This will be called each time a touch
is detected.
"""
# Unsubscribe to the event when talking,
# to avoid repetitions
memory.unsubscribeToEvent("MiddleTactilTouched",
"ReactToTouch")
self.tts.say("D'accord, on arrête de jouer")
self.AudioModule.cs = 0
# Subscribe again to the event
memory.subscribeToEvent("MiddleTactilTouched",
"ReactToTouch",
"onTouched")
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
# Example showing how to get the robot config
robotConfig = motionProxy.getRobotConfig()
for i in range(len(robotConfig[0])):
print robotConfig[0][i], ": ", robotConfig[1][i]
def __init__(self):
self.IP = "localhost" # Replace here with your NaoQi's IP address.
self.PORT = 9559
self.bmanager = ALProxy("ALBehaviorManager", self.IP, self.PORT)
self.poseProxy = ALProxy("ALRobotPose", self.IP, self.PORT)
self.motionProxy = ALProxy("ALMotion", self.IP, self.PORT)
self.memProxy = ALProxy("ALMemory",self.IP,self.PORT)
def start_sound_track(self, msg):
self.__proxyTTS = ALProxy("ALAnimatedSpeech", self.__ip, self.__port)
# set the local configuration
sayconfig = {"bodyLanguageMode":"contextual"}
self.__proxyTTS.say("Can you help me find you by clapping your hand?", sayconfig)
self.__proxyMotion = ALProxy("ALMotion", self.__ip, self.__port)
#initialise microphone
#self.__audioProxy = ALProxy("ALAudioDevice", self.__ip, self.__port)
#initialise soundsourcelocalisation
self.__sslProxy = ALProxy("ALSoundLocalization", self.__ip, self.__port)
#initialise almemory
self.__memoryProxy = ALProxy("ALMemory", self.__ip, self.__port)
#debugging purpose
#self.__audioProxy.setClientPreferences( self.getName() , 16000, 3, 0 )
#self.__audioProxy.subscribe(self.getName())
#configure sound detection
self.__sslProxy.setParameter("Sensitivity",0.1)
#callback from memory
try:
self.__memoryProxy.unsubscribeToEvent("ALSoundLocalization/SoundLocated","soundtracking")
except:
pass
self.__sslProxy.subscribe("sound_source_locator")
self.__memoryProxy.subscribeToMicroEvent(
"ALSoundLocalization/SoundLocated",
self.getName(),
"AnotherUserDataToIdentifyEvent",
"sound_callback")
class ReactionModule(ALModule):
""" A basic module to test events """
def __init__(self, name):
ALModule.__init__(self, name)
self.name = name
self.memory = ALProxy("ALMemory")
self.motion = ALProxy("ALMotion")
self.memory.subscribeToEvent("ColorDetectedEvent", name, "handleDetection")
def handleDetection(self, key, value, message):
""" A method that handles detection of the color. """
names = ['HeadYaw', 'HeadPitch']
times = [[0.01], [0.01]] # what is the fastest rate?
xStep = 0.03
yStep = 0.022
moveX = -xStep if value[0]>0 else xStep if value[0]<0 else 0.0 # since robot camera has a mirror view, we need to alternate directions
moveY = yStep if value[1]>0 else -yStep if value[1]<0 else 0.0
print moveX, moveY
self.memory.unsubscribeToEvent("ColorDetectedEvent", self.name)
self.motion.angleInterpolation(names, [moveX, moveY], times, False)
self.memory.subscribeToEvent("ColorDetectedEvent", self.name, "handleDetection")
def disconnect(self):
try:
self.memory.unsubscribeToEvent("ColorDetectedEvent", self.getName())
except BaseException, err:
print "Error while disconnecting from color reaction module: " + str(err)
def getImage(self):
"""main method, wait until qr code is found."""
period = 1000
qrCodeProxy = ALProxy("ALBarcodeReader", self.NAO_IP, self.NAO_PORT)
qrCodeProxy.subscribe("Testh_qr", period, 0.0)
detected = False
i = 0
while detected is False:
time.sleep(0.5)
val = self.memory.getData("BarcodeReader/BarcodeDetected")
print val
if val is not None:
if len(val) >= 1:
detected = True
todo = val[0][0]
ac = todo.split(" ", 1)
if len(ac) > 1:
action = self.nao.getAction().get(str(ac[0]))
action(str(ac[1]))
self.memory.insertData("BarcodeReader/BarcodeDetected", "")
else:
action = self.nao.getAction().get(todo)
action()
self.memory.insertData("BarcodeReader/BarcodeDetected", "")
i += 1
if i is 30:
detected = True
def __init__(self, name):
ALModule.__init__(self, name)
self.name = name
#self.tts = ALProxy("ALTextToSpeech")
self.memory = ALProxy("ALMemory")
self.motion = ALProxy("ALMotion")
self.memory.subscribeToEvent("RedBallDetectedEvent", name, "handleBallDetection")
def showNaoImage(IP, PORT):
camProxy = ALProxy("ALVideoDevice", IP, PORT)
resolution = 2 # VGA
colorSpace = 11 # RGB
videoClient = camProxy.subscribe("python_client", resolution, colorSpace, 5)
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = camProxy.getImageRemote(videoClient)
camProxy.unsubscribe(videoClient)
# Now we work with the image returned and save it as a PNG using ImageDraw
# package.
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
# Create a PIL Image from our pixel array.
im = Image.fromstring("RGB", (imageWidth, imageHeight), array)
# Save the image.
im.save("../public/imgNao/live.jpeg", "JPEG")
def showNaoImage(IP, PORT):
camProxy = ALProxy("ALVideoDevice", IP, PORT)
resolution = 2 # VGA
colorSpace = 11 # RGB
videoClient = camProxy.subscribe("python_client", resolution, colorSpace, 5)
t0 = time.time()
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = camProxy.getImageRemote(videoClient)
t1 = time.time()
# Time the image transfer.
print "acquisition delay ", t1 - t0
camProxy.unsubscribe(videoClient)
# Now we work with the image returned and save it as a PNG using ImageDraw
# package.
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
# Create a PIL Image from our pixel array.
im = Image.fromstring("RGB", (imageWidth, imageHeight), array)
nomPhoto = time.strftime("%d-%m-%y_a_%H-%M-%S", time.localtime())
print nomPhoto
# Save the image.
im.save("../public/imgNao/" + nomPhoto + ".jpeg", "JPEG")
def main():
""" Parse command line arguments,
run recordData and write the results
into a csv file
"""
if len(sys.argv) < 2:
nao_ip = ROBOT_IP
else:
nao_ip = sys.argv[1]
motion = ALProxy("ALMotion", nao_ip, 9559)
# Set stiffness on for Head motors
motion.setStiffnesses("Head", 1.0)
# Will go to 1.0 then 0 radian
# in two seconds
motion.post.angleInterpolation(
["HeadYaw"],
[1.0, 0.0],
[1 , 2],
False
)
data = recordData(nao_ip)
# Gently set stiff off for Head motors
motion.setStiffnesses("Head", 0.0)
output = os.path.abspath("record.csv")
with open(output, "w") as fp:
for line in data:
fp.write("; ".join(str(x) for x in line))
fp.write("\n")
print "Results written to", output
class HumanGreeterModule(ALModule):
""" A simple module able to react
to facedetection events
"""
def __init__(self, name):
ALModule.__init__(self, name)
# No need for IP and port here because
# we have our Python broker connected to NAOqi broker
# Create a proxy to ALTextToSpeech for later use
self.tts = ALProxy("ALTextToSpeech")
# Subscribe to the FaceDetected event:
global memory
memory = ALProxy("ALMemory")
memory.subscribeToEvent("FaceDetected","HumanGreeter", "onFaceDetected")
def onFaceDetected(self, *_args):
""" This will be called each time a face is
detected."""
# Unsubscribe to the event when talking,
# to avoid repetitions
memory.unsubscribeToEvent("FaceDetected", "HumanGreeter")
self.tts.say("je suis content")
# Subscribe again to the event
memory.subscribeToEvent("FaceDetected", "HumanGreeter", "onFaceDetected")
def powerOff (self):
tts = ALProxy("ALTextToSpeech", 'nao.local', 9559)
tts.say("即将执行关机操作！")
command1 = 'sudo shutdown -h now'
os.system(command1)
command2 = 'root\r' # here is tha default password of root user
os.system(command2)
def __init__(self, name):
ALModule.__init__(self, name)
try:
self.asr = ALProxy("ALSpeechRecognition")
except Exception as e:
self.asr = None
self.memory = ALProxy("ALMemory")
def getNaoImage(IP, PORT):
camProxy = ALProxy("ALVideoDevice", IP, PORT)
resolution = 2 # 640*480px http://doc.aldebaran.com/2-1/family/robots/video_robot.html#cameraresolution-mt9m114
colorSpace = 11 # RGB colorspace http://doc.aldebaran.com/2-1/family/robots/video_robot.html#cameracolorspace-mt9m114
fps = 5 # can be 0-30 fps
videoClient = camProxy.subscribe("python_client", resolution, colorSpace, fps)
t0 = time.time()
naoImage = camProxy.getImageRemote(videoClient)
t1 = time.time()
camProxy.unsubscribe(videoClient)
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
# Create a PIL Image from our pixel array.
im = Image.fromstring("RGB", (imageWidth, imageHeight), array)
#grab image from PIL and convert to opencv image
img = np.array(im)
img = img[:, :, ::-1].copy()
#im.save(name,"PNG")
print "acquisition delay ", t1 - t0
return img
def connect(self):
# create Proxy to give motion commands
self.motionproxy = ALProxy("ALMotion",self.ip,self.port)
# create Proxy to call behaviors
self.behaviorproxy = ALProxy("ALBehaviorManager", self.ip,self.port)
# create sensor object and share the motion proxy with it (connect(self))
self.sensors = naoSensors.naoSensors()
# give nao object to sensors, since it uses the same motionproxy
self.sensors.connect(self)
self.camera = naoCamera.naoCamera()
self.camera.setAddress(self.ip,self.port)
self.camera.connect()
# read sensors, so that robot can start with current pose
# .. second parameter chooses if the value is read from the sensor or the last command
# THOSE VALUES SHOULD COME FROM sensor object!
# .. change when implementation is done
# SHOULD be OK now (changed to what it should be)
self.headvalues = self.sensors.getHeadValues()
#motionproxy.getAngles(self.headnames,True)
self.armvalues = self.sensors.getRightArmValues()
#self.motionproxy.getAngles(self.armnames,True)
# set wich arm is free for balancing during walking
self.setActiveArm("left")
## set Head to initial position
#self.setHeadAngles(0,0)
# TODO: better if this was "somewhere else"
#self.motionproxy.stiffnessInterpolation("Body",1.0,0.5)
self.motionproxy.stiffnessInterpolation("Head",1.0,0.5)
print "!! press stand-up button !!"
print "!! If robot was already standing at start, just press stand-up button (default = left stick button) once to gain arm control"
class SpeechRecoModule(ALModule):
#""" A module to use speech recognition """
def __init__(self, name):
ALModule.__init__(self, name)
try:
self.asr = ALProxy("ALSpeechRecognition")
except Exception as e:
self.asr = None
self.memory = ALProxy("ALMemory")
def onLoad(self):
from threading import Lock
self.bIsRunning = False
self.mutex = Lock()
self.hasPushed = False
self.hasSubscribed = False
self.BIND_PYTHON("SpeechReco", "onWordRecognized")
def onUnload(self):
from threading import Lock
self.mutex.acquire()
try:
if (self.bIsRunning):
if (self.hasSubscribed):
self.memory.unsubscribeToEvent("WordRecognized", "SpeechReco")
if (self.hasPushed and self.asr):
self.asr.popContexts()
except RuntimeError, e:
self.mutex.release()
raise e
self.bIsRunning = False;
self.mutex.release()
def _validate(self, context):
"""
Component validated
"""
_logger.debug("Validating speech...")
# Register the module as a global in __main__
constants.register_almodule(self._name, self)
# Initialize the module
ALModule.__init__(self, self._name)
# Get the "memory" proxy, to register to callbacks
self._memory = ALProxy("ALMemory")
# Just to be sure...
try:
self._memory.unsubscribeToEvent("WordRecognized", self._name)
except:
_logger.debug("Speech wasn't yet registered")
# Create the proxy
self._recog = ALProxy("ALSpeechRecognition")
self._recog.setLanguage("French")
# We're ready
self._can_recog.set()
_logger.debug("Speech ready")
class naoSensors():
motionproxy = None
headnames = ['HeadYaw', 'HeadPitch']
armnames = ['RShoulderRoll', 'RShoulderPitch', 'RElbowYaw', 'RElbowRoll']
redballproxy = None
memproxy = None
openHand = True
nao = None
def connect(self,nao):
self.nao = nao
self.motionproxy = nao.motionproxy
# exception needed needed to be able to use this code with webots, which does not support redballdetection
# TODO: add class for own balldetection, that will be loaded when using naoqi redballdetection fails
# .. use opencv, see old localization module
# ! IMPORTANT, methods of second implementation MUST be the same as in redballdetection, so this needs no changes at other places!
try:
self.redballproxy = ALProxy("ALRedBallDetection", nao.ip, nao.port)
self.memproxy = ALProxy("ALMemory",nao.ip,nao.port)
# this might not be the best way to start the naoqi redballdetection
# .. after subscribing the nao head processor searches and calculates the
# .. ballposition over and over again, <- and very likely will get hot
self.redballproxy.subscribe("detector")
except RuntimeError,e:
print e
class BoardMemoryMonitor(object):
def __init__(self, args):
self._prefix = 'Device/DeviceList'
self._board = args.board
self._key = args.key
try:
self._mem = ALProxy('ALMemory', args.url, args.port)
self._out = UnbufferedStreamWrapper(stdout)
except RuntimeError as e:
exceptRgx = compile('[^\n\t]+$')
print '\n', 'RuntimeError:', exceptRgx.search(e.args[0]).group(0)
exit(1)
def run(self):
progVersion = self._mem.getData('/'.join([self._prefix,
self._board,
'ProgVersion']))
system('clear')
print 'Monitoring key \'{0}\' on board \'{1}\' [ProgVersion: {2}]'.format(self._key,
self._board,
str(progVersion))
while True:
data = self._mem.getData('/'.join([self._prefix, self._board, self._key]))
self._out.write(str(data))
self._out.write(' \r')
def update_battery(self):
if (self.ip.get()) == "Disconnected":
self.battery_status.set("0 %")
else :
try:
## import naoqi library and creates all the relevant modules that will be used
from naoqi import ALProxy
self.memory = ALProxy("ALMemory", self.ip.get() , 9559)
self.memory.ping()
self.battery = ALProxy("ALBattery", self.ip.get() , 9559)
self.tts = ALProxy("ALTextToSpeech", self.ip.get() , 9559)
self.motion = ALProxy("ALMotion" , self.ip.get() , 9559)
self.posture = ALProxy("ALRobotPosture", self.ip.get(), 9559)
#Sets the status of the battery to the existent value
self.battery_status.set(str(self.battery.getBatteryCharge()) + " %")
# Thread that updates the battery status after specific period of time
#threading.Timer(5.0, self.update_battery).start()
threading.Timer(15.0, self.update_battery).start()
except BaseException:
self.ip.set("Disconnected")
return
def getColour(IP, PORT):
"""
First get an image from Nao, then show it on the screen with PIL.
:param IP:
:param PORT:
"""
myBroker = ALBroker("myBroker",
"0.0.0.0", # listen to anyone
0, # find a free port and use it
IP, # parent broker IP
PORT) # parent broker port
camProxy = ALProxy("ALVideoDevice", IP, PORT)
resolution = 2 # VGA
colorSpace = 11 # RGB
videoClient = camProxy.subscribe("python_client", resolution, colorSpace, 5)
t0 = time.time()
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = camProxy.getImageRemote(videoClient)
t1 = time.time()
# Time the image transfer.
#print "Runde: ", b
camProxy.unsubscribe(videoClient)
# Now we work with the image returned and save it as a PNG using ImageDraw
# package.
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
#Create a PIL Image Instance from our pixel array.
img0= Image.frombytes("RGB", (imageWidth, imageHeight), array)
#frame=np.asarray(convert2pil(img0)[:,:])
#object_rect2=detectColor(img0, RED_MIN,RED_MAX)
frame=detectShape(img0, RED_MIN,RED_MAX)
#frame=selectDetected(object_rect1,frame)
#frame=selectDetected(object_rect2,frame)
# currentImage = path+ "/camImage1cm.jpg"
# cv2.imwrite(currentImage, frame)
cv2.imshow('contour',frame)
cv2.waitKey(0)
cv2.destroyAllWindows()
class FaceDetectionModule(ALModule):
# Déclaration de méthode.
def __init__(self, name):
ALModule.__init__(self, name)
print "[INFO ] FaceDetectionModule initialization"
# Instanciation d'un objet tts de classe ALTextToSpeech.
self.tts = ALProxy("ALTextToSpeech")
self.tts.setLanguage("french")
# Instanciation d'un objet tts de classe ALFaceDetection.
self.fd = ALProxy("ALFaceDetection")
# Variable d'instance.
global memory
# Instanciation d'un objet memory de classe ALMemory.
memory = ALProxy("ALMemory")
# Appel de la methode subsribeToEvent...
memory.subscribeToEvent("FaceDetected", # Sur cet evenement...
"FaceDetection", # ...de cet instance...
"onDetection") # ...declancher l'appel
# ...de cette methode.
print "[INFO ] FaceDetectionModule initialized"
# Méthode appelée sur l'évènement.
def onDetection(self, *_args):
print "[INFO ] FaceDetection: Face detected"
global face_nb
print "[INFO ] FaceDetection initialize face detection process"
learnFaceProcess(self, face_nb)
def start(self):
if self.al.connected():
self.tts.say("You are already connected")
else:
self.networks = self.al.list_networks()
self.tts.say("Here are the Wi Fi networks")
for num, network in enumerate(self.networks, 1):
self.tts.say(network)
self.tts.say("is number %d" % (num,))
time.sleep(0.2)
if len(self.networks) == 0:
self.tts.say("Sorry you are in a wifi free zone")
else:
self.tts.say("Which number Wi Fi network shall I connect to?")
try:
self.memory.unsubscribeToEvent("WordRecognized")
except Exception:
pass
speech_recognition = ALProxy("ALSpeechRecognition", NAO_IP, 9559)
speech_recognition.setLanguage("English")
try:
speech_recognition.setWordListAsVocabulary([str(i) for i in range(1, len(self.networks))])
except Exception:
self.tts.say("Could not set vocabulary")
try:
result = self.memory.subscribeToEvent("WordRecognized", self.module_name, "on_word_recognised")
print "Subscribed to event WordRecognized with package ", self.module_name, " and result ", result
except Exception as e:
print "Failed to subscribe ", e
def getMarkXYZ (IP, portNumber, markData, landmarkSize):
print "0"
currentCamera = "CameraTop"
print "1"
# Retrieve landmark angular size in radians.
angularSize = markData[1][0][0][3]
print "2"
# Compute distance to landmark.
distanceFromCameraToLandmark = landmarkSize / ( 2 * math.tan( angularSize / 2))
print "3"
motionProxy = ALProxy("ALMotion", IP, portNumber)
print "4"
# Retrieve landmark center position in radians.
wzCamera = markData[1][0][0][1]
print "5"
wyCamera = markData[1][0][0][2]
print "6"
# Get current camera position in NAO space.
transform = motionProxy.getTransform(currentCamera, 2, True)
print "7"
transformList = almath.vectorFloat(transform)
robotToCamera = almath.Transform(transformList)
# Compute the rotation to point towards the landmark.
cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, wyCamera, wzCamera)
# Compute the translation to reach the landmark.
cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)
# Combine all transformations to get the landmark position in NAO space.
robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform *cameraToLandmarkTranslationTransform
return robotToLandmark.r1_c4, robotToLandmark.r2_c4, robotToLandmark.r3_c4
def __init__(self, name): #contructor of the class, which takes two parameters, self refers to the instance of the class and the name parameter which is just a string
ALModule.__init__(self, name) #calling of the contructpor of the ALModule
self.tts = ALProxy("ALTextToSpeech", ip, 9559) #proxy creation on the tts module
self.asr = ALProxy("ALSpeechRecognition", ip, 9559) #proxy creation on the asr module
self.memory = ALProxy("ALMemory", ip, 9559) #proxy creation on the memory module
self.num1 = random.randint(1, 10) #here are two integers randomly selected from 1 to 10
self.num2 = random.randint(1, 10)
self.operator = random.choice("-") #here is randomly choosen operator which is then applied to the equation
self.tts.setLanguage("English") #set the the language which NAO uses for talking
if self.operator == "-": #NAO was programmed to create equations which have a positive result
if self.num1 > self.num2: #the numbers are compared in order to asure that the larger number is first
self.result = str(eval(str(self.num1) + self.operator + str(self.num2))) #the result is evaluated and put into a string so NOA can say it
self.operator = " minus " #and so is the operator
self.question = "What is the result of " + str(self.num1) + self.operator + str(self.num2) + "?" #the question is created
else:
self.result = str(eval(str(self.num2) + self.operator + str(self.num1)))
self.operator = " minus "
self.question = "What is the result of " + str(self.num2) + self.operator + str(self.num1) + "?"
else:
self.result = str(eval(str(self.num1) + self.operator + str(self.num2)))
self.operator = " plus "
self.question = "What is the result of " + str(self.num1) + self.operator + str(self.num2) + "?"
print self.question #the question is printed to the terminal
print self.result #the reslt is printed to the terminal
self.tts.say(self.question) #NAO tells the question
self.speech_recognition() #the speech_recognition method is called
class NaoTTS(object):
"""
Nao text-to-speech service
"""
def __init__(self):
"""
Sets up members
"""
self._tts = None
# Authorization to speak
self._can_speak = threading.Event()
self._can_speak.set()
self.__speaking_lock = threading.Lock()
@Validate
def validate(self, context):
"""
Component validated
"""
# Set up the TTS proxy
self._tts = ALProxy("ALTextToSpeech")
@Invalidate
def invalidate(self, context):
"""
Component invalidated
"""
# Stop using the proxy
self._tts = None
# Unlock everything
self._can_speak.set()
def say(self, sentence):
"""
Says the given sentence
:param sentence: Text to say
"""
with self.__speaking_lock:
# Wait to be authorized to speak
self._can_speak.wait()
# Say what we have to
self._tts.say(sentence)
def resume(self):
"""
Allows Nao to speak
"""
self._can_speak.set()
def pause(self):
"""
Forbids Nao to speak
"""
if self._can_speak.is_set():
with self.__speaking_lock:
self._can_speak.clear()
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()
def main(robotIP, port):
names = list()
times = list()
keys = list()
names.append("HeadPitch")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 5.46667, 6
])
keys.append([[0.00302603, [3, -0.111111, 0], [3, 0.222222, 0]],
[0.00302603, [3, -0.222222, 0], [3, 0.2, 0]],
[0.340507, [3, -0.2, 0], [3, 0.2, 0]],
[-0.417291, [3, -0.2, 0.200954], [3, 0.133333, -0.133969]],
[-0.664264, [3, -0.133333, 0], [3, 0.0888889, 0]],
[
0.0459781, [3, -0.0888889, -0.0674959],
[3, 0.0888889, 0.0674959]
], [0.113474, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.0996681, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.102736, [3, -0.0888889, 0], [3, 0.6, 0]],
[-0.00617796, [3, -0.6, 0], [3, 0.177778, 0]],
[-0.0046272, [3, -0.177778, 0], [3, 0, 0]]])
names.append("HeadYaw")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 5.46667, 6
])
keys.append([[-1.10145, [3, -0.111111, 0], [3, 0.222222, 0]],
[1.4097, [3, -0.222222, 0], [3, 0.2, 0]],
[-0.00924597, [3, -0.2, 0.245439], [3, 0.2, -0.245439]],
[-0.254685, [3, -0.2, 0], [3, 0.133333, 0]],
[
0.506179, [3, -0.133333, -0.17641],
[3, 0.0888889, 0.117607]
], [0.627364, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[-0.22554, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.645772, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[-0.217869, [3, -0.0888889, 0], [3, 0.6, 0]],
[0.00456004, [3, -0.6, 0], [3, 0.177778, 0]],
[-0.00930693, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LAnklePitch")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append([[-0.359129, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.762571, [3, -0.222222, 0.0170441], [3, 0.2, -0.0153397]],
[-0.777911, [3, -0.2, 0], [3, 0.2, 0]],
[-0.580025, [3, -0.2, 0], [3, 0.133333, 0]],
[
-0.58923, [3, -0.133333, 0.00115064],
[3, 0.177778, -0.00153418]
], [-0.590764, [3, -0.177778, 0], [3, 0.777778, 0]],
[-0.116757, [3, -0.777778, 0], [3, 0.177778, 0]],
[-0.350546, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LAnkleRoll")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append(
[[-0.282235, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.0598056, [3, -0.222222, -0.00511343], [3, 0.2, 0.00460208]],
[-0.0552035, [3, -0.2, 0], [3, 0.2, 0]],
[-0.139574, [3, -0.2, 0], [3, 0.133333, 0]],
[-0.133438, [3, -0.133333, 0], [3, 0.177778, 0]],
[-0.134972, [3, -0.177778, 0], [3, 0.777778, 0]],
[0.0138264, [3, -0.777778, 0], [3, 0.177778, 0]],
[-0.00709866, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LElbowRoll")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 4.4, 4.8,
5.46667, 6
])
keys.append([[-0.748551, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.901949, [3, -0.222222, 0.129986], [3, 0.2, -0.116988]],
[-1.48947, [3, -0.2, 0], [3, 0.2, 0]],
[-1.05688, [3, -0.2, 0], [3, 0.133333, 0]],
[-1.56157, [3, -0.133333, 0], [3, 0.0888889, 0]],
[-1.08603, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[-1.56464, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[-0.866668, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[-1.56464, [3, -0.0888889, 0], [3, 0.244444, 0]],
[
-1.02314, [3, -0.244444, -0.272962],
[3, 0.133333, 0.148888]
], [-0.299088, [3, -0.133333, 0], [3, 0.222222, 0]],
[
-0.315962, [3, -0.222222, 0.0168739],
[3, 0.177778, -0.0134991]
], [-1.007, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LElbowYaw")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 4.4, 4.8,
5.46667, 6
])
keys.append([[0.89428, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.681137, [3, -0.222222, 0], [3, 0.2, 0]],
[-0.52467, [3, -0.2, -0.156467], [3, 0.2, 0.156467]],
[0.383458, [3, -0.2, 0], [3, 0.133333, 0]],
[-0.760906, [3, -0.133333, 0], [3, 0.0888889, 0]],
[0.0797261, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[-0.83914, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.329768, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[-0.740964, [3, -0.0888889, 0], [3, 0.244444, 0]],
[-0.339056, [3, -0.244444, 0], [3, 0.133333, 0]],
[-0.932714, [3, -0.133333, 0], [3, 0.222222, 0]],
[-0.7471, [3, -0.222222, 0], [3, 0.177778, 0]],
[-1.38708, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LHand")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append([[0.920023, [3, -0.111111, 0], [3, 0.222222, 0]],
[0.920023, [3, -0.222222, 0], [3, 0.2, 0]],
[0.921478, [3, -0.2, 0], [3, 0.2, 0]],
[0.921478, [3, -0.2, 0], [3, 0.133333, 0]],
[0.921478, [3, -0.133333, 0], [3, 0.177778, 0]],
[0.921478, [3, -0.177778, 0], [3, 0.777778, 0]],
[
0.920387, [3, -0.777778, 0.00109094],
[3, 0.177778, -0.000249359]
], [0.255661, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LHipPitch")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append(
[[0.232947,
[3, -0.111111, 0], [3, 0.222222,
0]], [-0.224186, [3, -0.222222, 0], [3, 0.2, 0]],
[-0.21038, [3, -0.2, 0],
[3, 0.2,
0]],
[-0.34077, [3, -0.2, 0],
[3, 0.133333,
0]],
[-0.325429, [3, -0.133333, -0.00115061], [3, 0.177778, 0.00153415]],
[-0.323895, [3, -0.177778, -0.00153415], [3, 0.777778, 0.00671189]],
[0.0764786, [3, -0.777778, 0], [3, 0.177778, 0]],
[-0.44423, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LHipRoll")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append(
[[0.423192, [3, -0.111111, 0], [3, 0.222222, 0]],
[0.31121, [3, -0.222222, 0.0187487], [3, 0.2, -0.0168738]],
[0.294336, [3, -0.2, 0.0168738], [3, 0.2, -0.0168738]],
[-0.050814, [3, -0.2, 0.0230101], [3, 0.133333, -0.01534]],
[-0.0661541, [3, -0.133333, 0], [3, 0.177778, 0]],
[-0.0661541, [3, -0.177778, 0], [3, 0.777778, 0]],
[-0.0492801, [3, -0.777778, -0.016874], [3, 0.177778, 0.00385692]],
[0, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LHipYawPitch")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 3.66667, 5.46667, 6])
keys.append([[-0.113559, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.236279, [3, -0.222222, 0], [3, 0.2, 0]],
[-0.230144, [3, -0.2, 0], [3, 0.2, 0]],
[-0.32065, [3, -0.2, 0], [3, 0.133333, 0]],
[-0.311445, [3, -0.133333, 0], [3, 0.177778, 0]],
[-0.311445, [3, -0.177778, 0], [3, 0.177778, 0]],
[-0.302242, [3, -0.177778, -0.00920312], [3, 0.6, 0.0310605]],
[0.0275685, [3, -0.6, 0], [3, 0.177778, 0]],
[-0.0028562, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LKneePitch")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append([[0.14541, [3, -0.111111, 0], [3, 0.222222, 0]],
[1.11643, [3, -0.222222, -0.0187487], [3, 0.2, 0.0168739]],
[1.13331, [3, -0.2, 0], [3, 0.2, 0]],
[1.01212, [3, -0.2, 0], [3, 0.133333, 0]],
[1.03513, [3, -0.133333, 0], [3, 0.177778, 0]],
[
1.02899, [3, -0.177778, 0.00613659],
[3, 0.777778, -0.0268476]
], [0.0871181, [3, -0.777778, 0], [3, 0.177778, 0]],
[0.699999, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LShoulderPitch")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 4.4, 4.8,
5.46667, 6
])
keys.append([[0.653443, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.934249, [3, -0.222222, 0], [3, 0.2, 0]],
[0.489305, [3, -0.2, -0.0997089], [3, 0.2, 0.0997089]],
[0.589014, [3, -0.2, 0], [3, 0.133333, 0]],
[0.259204, [3, -0.133333, 0], [3, 0.0888889, 0]],
[0.714801, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.245399, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.656511, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.274544, [3, -0.0888889, 0], [3, 0.244444, 0]],
[0.720938, [3, -0.244444, 0], [3, 0.133333, 0]],
[0.437147, [3, -0.133333, 0], [3, 0.222222, 0]],
[1.56004, [3, -0.222222, 0], [3, 0.177778, 0]],
[1.39144, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LShoulderRoll")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 4.4, 4.8,
5.46667, 6
])
keys.append([[1.32687, [3, -0.111111, 0], [3, 0.222222, 0]],
[1.39743, [3, -0.222222, 0], [3, 0.2, 0]],
[0.010696, [3, -0.2, 0], [3, 0.2, 0]],
[0.013764, [3, -0.2, 0], [3, 0.133333, 0]],
[0, [3, -0.133333, 0], [3, 0.0888889, 0]],
[0, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.013764, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0, [3, -0.0888889, 0], [3, 0.244444, 0]],
[
0.061318, [3, -0.244444, -0.0613179],
[3, 0.133333, 0.0334462]
], [1.13052, [3, -0.133333, 0], [3, 0.222222, 0]],
[
0.302157, [3, -0.222222, 0.00105811],
[3, 0.177778, -0.000846486]
], [0.30131, [3, -0.177778, 0], [3, 0, 0]]])
names.append("LWristYaw")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append([[-1.02475, [3, -0.111111, 0], [3, 0.222222, 0]],
[-1.02475, [3, -0.222222, 0], [3, 0.2, 0]],
[-1.02015, [3, -0.2, 0], [3, 0.2, 0]],
[-1.02629, [3, -0.2, 0], [3, 0.133333, 0]],
[-1.02015, [3, -0.133333, 0], [3, 0.177778, 0]],
[-1.02629, [3, -0.177778, 0], [3, 0.777778, 0]],
[1.04461, [3, -0.777778, 0], [3, 0.177778, 0]],
[-0.00130817, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RAnklePitch")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 3.66667, 5.46667, 6])
keys.append([[-0.102805, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.210185, [3, -0.222222, 0.0119322], [3, 0.2, -0.010739]],
[-0.220924, [3, -0.2, 0.010739], [3, 0.2, -0.010739]],
[-0.405004, [3, -0.2, 0.0115035], [3, 0.133333, -0.00766897]],
[-0.412673, [3, -0.133333, 0], [3, 0.177778, 0]],
[-0.412673, [3, -0.177778, 0], [3, 0.177778, 0]],
[-0.421878, [3, -0.177778, 0], [3, 0.6, 0]],
[-0.102805, [3, -0.6, 0], [3, 0.177778, 0]],
[-0.34791, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RAnkleRoll")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 3.66667, 5.46667, 6])
keys.append([
[-0.00456227, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.105806, [3, -0.222222, 0.0017044], [3, 0.2, -0.00153396]],
[-0.10734, [3, -0.2, 0],
[3, 0.2,
0]], [-0.0229703, [3, -0.2, 0],
[3, 0.133333,
0]],
[-0.0245042, [3, -0.133333, 0.000438277], [3, 0.177778, -0.00058437]],
[-0.0260382, [3, -0.177778, 0.000511323], [3, 0.177778, -0.000511323]],
[-0.0275722, [3, -0.177778, 0], [3, 0.6, 0]],
[-0.00456227, [3, -0.6, -0.00754438], [3, 0.177778, 0.00223537]],
[0.00176708, [3, -0.177778, 0], [3, 0, 0]]
])
names.append("RElbowRoll")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 4.4, 4.8,
5.46667, 6
])
keys.append([
[0.727158, [3, -0.111111,
0], [3, 0.222222,
0]], [1.10452, [3, -0.222222, 0], [3, 0.2, 0]],
[0.176453, [3, -0.2, 0.00613674],
[3, 0.2, -0.00613674]], [0.170316, [3, -0.2, 0], [3, 0.133333, 0]],
[0.174919, [3, -0.133333, 0],
[3, 0.0888889,
0]], [0.174919, [3, -0.0888889, 0],
[3, 0.0888889,
0]],
[0.173384, [3, -0.0888889, 0.000767101], [3, 0.0888889, -0.000767101]],
[0.170316, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.173384, [3, -0.0888889, -0.00306793], [3, 0.244444, 0.0084368]],
[0.280764, [3, -0.244444, -0.10738], [3, 0.133333, 0.058571]],
[1.34843, [3, -0.133333, 0], [3, 0.222222, 0]],
[0.273093, [3, -0.222222, 0], [3, 0.177778, 0]],
[1.00676, [3, -0.177778, 0], [3, 0, 0]]
])
names.append("RElbowYaw")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 4.4, 4.8,
5.46667, 6
])
keys.append(
[[0.946436,
[3, -0.111111, 0], [3, 0.222222,
0]], [-0.515466, [3, -0.222222, 0], [3, 0.2, 0]],
[0.728609, [3, -0.2, 0],
[3, 0.2,
0]],
[0.722472, [3, -0.2, 0],
[3, 0.133333,
0]],
[0.730143, [3, -0.133333, -0.00184063], [3, 0.0888889, 0.00122709]],
[0.731675, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.730143, [3, -0.0888889, 0.00153245], [3, 0.0888889, -0.00153245]],
[0.71787, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[0.722472, [3, -0.0888889, 0], [3, 0.244444, 0]],
[-1.22878, [3, -0.244444, 0], [3, 0.133333, 0]],
[-0.0537319, [3, -0.133333, -0.249658], [3, 0.222222, 0.416097]],
[0.768491, [3, -0.222222, -0.266814], [3, 0.177778, 0.213451]],
[1.38706, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RHand")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append([[0.920387, [3, -0.111111, 0], [3, 0.222222, 0]],
[0.920387, [3, -0.222222, 0], [3, 0.2, 0]],
[0.922933, [3, -0.2, 0], [3, 0.2, 0]],
[0.922933, [3, -0.2, 0], [3, 0.133333, 0]],
[0.925478, [3, -0.133333, 0], [3, 0.177778, 0]],
[0.922933, [3, -0.177778, 0], [3, 0.777778, 0]],
[0.971296, [3, -0.777778, 0], [3, 0.177778, 0]],
[0.255665, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RHipPitch")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 3.66667, 5.46667, 6])
keys.append([[0.041361, [3, -0.111111, 0], [3, 0.222222, 0]],
[-0.24243, [3, -0.222222, 0], [3, 0.2, 0]],
[-0.237827, [3, -0.2, 0], [3, 0.2, 0]],
[-0.856028, [3, -0.2, 0], [3, 0.133333, 0]],
[-0.848359, [3, -0.133333, 0], [3, 0.177778, 0]],
[-0.852962, [3, -0.177778, 0], [3, 0.177778, 0]],
[
-0.851427, [3, -0.177778, -0.00153418],
[3, 0.6, 0.00517786]
], [0.041361, [3, -0.6, 0], [3, 0.177778, 0]],
[-0.45, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RHipRoll")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 3.66667, 5.46667, 6])
keys.append([[0.0153604, [3, -0.111111, 0], [3, 0.222222, 0]],
[0.210178, [3, -0.222222, -0.00511381], [3, 0.2, 0.00460242]],
[0.21478, [3, -0.2, 0], [3, 0.2, 0]],
[-0.0889516, [3, -0.2, 0], [3, 0.133333, 0]],
[-0.0843497, [3, -0.133333, 0], [3, 0.177778, 0]],
[-0.0843497, [3, -0.177778, 0], [3, 0.177778, 0]],
[-0.0797476, [3, -0.177778, -0.00460208], [3, 0.6, 0.015532]],
[0.0153604, [3, -0.6, 0], [3, 0.177778, 0]],
[0.00205951, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RKneePitch")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 3.66667, 5.46667, 6])
keys.append([[0.105432, [3, -0.111111, 0], [3, 0.222222, 0]],
[0.619321, [3, -0.222222, -0.0238625], [3, 0.2, 0.0214763]],
[0.640798, [3, -0.2, -0.0214763], [3, 0.2, 0.0214763]],
[1.26974, [3, -0.2, -0.0322144], [3, 0.133333, 0.0214763]],
[1.29121, [3, -0.133333, 0], [3, 0.177778, 0]],
[1.28968, [3, -0.177778, 0], [3, 0.177778, 0]],
[1.31576, [3, -0.177778, 0], [3, 0.6, 0]],
[0.105432, [3, -0.6, 0], [3, 0.177778, 0]],
[0.699999, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RShoulderPitch")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 4.4, 4.8,
5.46667, 6
])
keys.append(
[[-0.970981,
[3, -0.111111, 0], [3, 0.222222,
0]], [0.782382, [3, -0.222222, 0], [3, 0.2, 0]],
[-0.877407, [3, -0.2, 0],
[3, 0.2,
0]], [-0.863599, [3, -0.2, -0.0119653], [3, 0.133333, 0.00797686]],
[-0.81758, [3, -0.133333, -0.00460244], [3, 0.0888889, 0.00306829]],
[-0.814512,
[3, -0.0888889, 0], [3, 0.0888889, 0]],
[-0.819114, [3, -0.0888889, 0], [3, 0.0888889, 0]],
[-0.811444, [3, -0.0888889, 0],
[3, 0.0888889, 0]], [-0.81758, [3, -0.0888889, 0], [3, 0.244444, 0]],
[-0.085862, [3, -0.244444, -0.264028], [3, 0.133333, 0.144016]],
[0.406552, [3, -0.133333, -0.205364], [3, 0.222222, 0.342274]],
[1.55705, [3, -0.222222, 0], [3, 0.177778, 0]],
[1.39697, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RShoulderRoll")
times.append([
0.333333, 1, 1.6, 2.2, 2.6, 2.86667, 3.13333, 3.4, 3.66667, 4.4, 4.8,
5.46667, 6
])
keys.append(
[[-1.40519, [3, -0.111111, 0],
[3, 0.222222,
0]], [-1.30701, [3, -0.222222, -0.0707792],
[3, 0.2, 0.0637013]], [-1.00174, [3, -0.2, 0], [3, 0.2, 0]],
[-1.01248, [3, -0.2, 0.00490891],
[3, 0.133333,
-0.00327261]], [-1.02629, [3, -0.133333, 0], [3, 0.0888889, 0]],
[-1.02015, [3, -0.0888889, 0],
[3, 0.0888889,
0]],
[-1.03856, [3, -0.0888889, 0.00511322], [3, 0.0888889, -0.00511322]],
[-1.05083, [3, -0.0888889, 0.00357938], [3, 0.0888889, -0.00357938]],
[-1.06004, [3, -0.0888889, 0.00920488], [3, 0.244444, -0.0253134]],
[-1.25179, [3, -0.244444, 0], [3, 0.133333, 0]],
[-0.00617796, [3, -0.133333, 0], [3, 0.222222, 0]],
[-0.296104, [3, -0.222222, 0.00650789], [3, 0.177778, -0.00520631]],
[-0.30131, [3, -0.177778, 0], [3, 0, 0]]])
names.append("RWristYaw")
times.append([0.333333, 1, 1.6, 2.2, 2.6, 3.13333, 5.46667, 6])
keys.append([[0.977116, [3, -0.111111, 0], [3, 0.222222, 0]],
[0.974047, [3, -0.222222, 0.00215287], [3, 0.2, -0.00193759]],
[0.964844, [3, -0.2, 0], [3, 0.2, 0]],
[0.966378, [3, -0.2, -0.00153411], [3, 0.133333, 0.00102274]],
[0.98632, [3, -0.133333, 0], [3, 0.177778, 0]],
[0.966378, [3, -0.177778, 0], [3, 0.777778, 0]],
[1.49254, [3, -0.777778, 0], [3, 0.177778, 0]],
[0.000547559, [3, -0.177778, 0], [3, 0, 0]]])
try:
motion = ALProxy("ALMotion", robotIP, port)
motion.angleInterpolationBezier(names, times, keys)
posture = ALProxy("ALRobotPosture", robotIP, port)
#posture.goToPosture("StandInit", 1.0)
except BaseException, err:
print err
def main(robotIP, port, unit_time, stand):
names = list()
times = list()
keys = list()
names.append("HeadPitch")
times.append([0.92, 1.4, 1.44, 1.96])
keys.append([[-0.368202, [3, -0.306667, 0], [3, 0.16, 0]], [-0.144238, [3, -0.16, -0.223964], [3, 0.0133333, 0.0186636]], [0.434081, [3, -0.0133333, 0], [3, 0.173333, 0]], [-0.144238, [3, -0.173333, 0], [3, 0, 0]]])
names.append("HeadYaw")
times.append([0.92, 1.4, 1.44, 1.96])
keys.append([[0.87127, [3, -0.306667, 0], [3, 0.16, 0]], [-0.032256, [3, -0.16, 0], [3, 0.0133333, 0]], [-0.012314, [3, -0.0133333, 0], [3, 0.173333, 0]], [-0.032256, [3, -0.173333, 0], [3, 0, 0]]])
names.append("LElbowRoll")
times.append([0.96, 1.4, 1.48, 1.96])
keys.append([[-0.759288, [3, -0.32, 0], [3, 0.146667, 0]], [-0.427944, [3, -0.146667, -0.212439], [3, 0.0266667, 0.0386253]], [-0.00609404, [3, -0.0266667, 0], [3, 0.16, 0]], [-0.427944, [3, -0.16, 0], [3, 0, 0]]])
names.append("LElbowYaw")
times.append([0.96, 1.4, 1.48, 1.96])
keys.append([[-0.506262, [3, -0.32, 0], [3, 0.146667, 0]], [-1.17509, [3, -0.146667, 0], [3, 0.0266667, 0]], [-0.231677, [3, -0.0266667, 0], [3, 0.16, 0]], [-1.17509, [3, -0.16, 0], [3, 0, 0]]])
names.append("LHand")
times.append([1.4, 1.96])
keys.append([[0.3048, [3, -0.466667, 0], [3, 0.186667, 0]], [0.3048, [3, -0.186667, 0], [3, 0, 0]]])
names.append("LShoulderPitch")
times.append([0.96, 1.4, 1.48, 1.96])
keys.append([[-0.243948, [3, -0.32, 0], [3, 0.146667, 0]], [1.48027, [3, -0.146667, 0], [3, 0.0266667, 0]], [0.917291, [3, -0.0266667, 0], [3, 0.16, 0]], [1.48027, [3, -0.16, 0], [3, 0, 0]]])
names.append("LShoulderRoll")
times.append([0.96, 1.4, 1.48, 1.96])
keys.append([[1.36982, [3, -0.32, 0], [3, 0.146667, 0]], [0.0873961, [3, -0.146667, 0], [3, 0.0266667, 0]], [0.207048, [3, -0.0266667, 0], [3, 0.16, 0]], [0.0873961, [3, -0.16, 0], [3, 0, 0]]])
names.append("LWristYaw")
times.append([1.4, 1.96])
keys.append([[0.145688, [3, -0.466667, 0], [3, 0.186667, 0]], [0.145688, [3, -0.186667, 0], [3, 0, 0]]])
names.append("RElbowRoll")
times.append([0.96, 1.4, 1.48, 1.96])
keys.append([[1.26866, [3, -0.32, 0], [3, 0.146667, 0]], [0.389678, [3, -0.146667, 0], [3, 0.0266667, 0]], [0.506262, [3, -0.0266667, 0], [3, 0.16, 0]], [0.389678, [3, -0.16, 0], [3, 0, 0]]])
names.append("RElbowYaw")
times.append([0.96, 1.4, 1.48, 1.96])
keys.append([[0.628898, [3, -0.32, 0], [3, 0.146667, 0]], [1.17654, [3, -0.146667, 0], [3, 0.0266667, 0]], [0.354312, [3, -0.0266667, 0], [3, 0.16, 0]], [1.17654, [3, -0.16, 0], [3, 0, 0]]])
names.append("RHand")
times.append([1.4, 1.96])
keys.append([[0.3068, [3, -0.466667, 0], [3, 0.186667, 0]], [0.3068, [3, -0.186667, 0], [3, 0, 0]]])
names.append("RShoulderPitch")
times.append([0.96, 1.4, 1.48, 1.96])
keys.append([[0.289967, [3, -0.32, 0], [3, 0.146667, 0]], [1.49569, [3, -0.146667, 0], [3, 0.0266667, 0]], [0.948054, [3, -0.0266667, 0], [3, 0.16, 0]], [1.49569, [3, -0.16, 0], [3, 0, 0]]])
names.append("RShoulderRoll")
times.append([0.96, 1.4, 1.48, 1.96])
keys.append([[-0.04913, [3, -0.32, 0], [3, 0.146667, 0]], [-0.104354, [3, -0.146667, 0], [3, 0.0266667, 0]], [-0.0537319, [3, -0.0266667, 0], [3, 0.16, 0]], [-0.104354, [3, -0.16, 0], [3, 0, 0]]])
names.append("RWristYaw")
times.append([1.4, 1.96])
keys.append([[0.0720561, [3, -0.466667, 0], [3, 0.186667, 0]], [0.0720561, [3, -0.186667, 0], [3, 0, 0]]])
if stand:
names.append("LAnklePitch")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[-0.00361468, [3, -0.173333, 0], [3, 0.16, 0]], [0.0608078, [3, -0.16, -0.017663], [3, 0.133333, 0.0147192]], [0.093532, [3, -0.133333, 0], [3, 0.186667, 0]], [0.093532, [3, -0.186667, 0], [3, 0, 0]]])
names.append("LAnkleRoll")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[0.225266, [3, -0.173333, 0], [3, 0.16, 0]], [0.0621712, [3, -0.16, 0.062147], [3, 0.133333, -0.0517891]], [-0.116542, [3, -0.133333, 0], [3, 0.186667, 0]], [-0.116542, [3, -0.186667, 0], [3, 0, 0]]])
names.append("RHipPitch")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[-0.0199139, [3, -0.173333, 0], [3, 0.16, 0]], [0.435402, [3, -0.16, 0], [3, 0.133333, 0]], [0.131882, [3, -0.133333, 0], [3, 0.186667, 0]], [0.131882, [3, -0.186667, 0], [3, 0, 0]]])
names.append("RHipRoll")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[-0.389282, [3, -0.173333, 0], [3, 0.16, 0]], [-0.488811, [3, -0.16, 0], [3, 0.133333, 0]], [-0.06592, [3, -0.133333, 0], [3, 0.186667, 0]], [-0.06592, [3, -0.186667, 0], [3, 0, 0]]])
names.append("RHipYawPitch")
times.append([1.4, 1.96])
keys.append([[-0.171766, [3, -0.466667, 0], [3, 0.186667, 0]], [-0.171766, [3, -0.186667, 0], [3, 0, 0]]])
names.append("RKneePitch")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[0.729549, [3, -0.173333, 0], [3, 0.16, 0]], [0.0461345, [3, -0.16, 0.149372], [3, 0.133333, -0.124477]], [-0.091998, [3, -0.133333, 0], [3, 0.186667, 0]], [-0.091998, [3, -0.186667, 0], [3, 0, 0]]])
names.append("RAnklePitch")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[-0.489898, [3, -0.173333, 0], [3, 0.16, 0]], [-0.0726446, [3, -0.16, -0.108604], [3, 0.133333, 0.090503]], [0.107422, [3, -0.133333, 0], [3, 0.186667, 0]], [0.107422, [3, -0.186667, 0], [3, 0, 0]]])
names.append("RAnkleRoll")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[0.194007, [3, -0.173333, 0], [3, 0.16, 0]], [0.34383, [3, -0.16, 0], [3, 0.133333, 0]], [0.07214, [3, -0.133333, 0], [3, 0.186667, 0]], [0.07214, [3, -0.186667, 0], [3, 0, 0]]])
names.append("LHipPitch")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[0.298877, [3, -0.173333, 0], [3, 0.16, 0]], [0.397335, [3, -0.16, 0], [3, 0.133333, 0]], [0.138102, [3, -0.133333, 0], [3, 0.186667, 0]], [0.138102, [3, -0.186667, 0], [3, 0, 0]]])
names.append("LHipRoll")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[-0.359491, [3, -0.173333, 0], [3, 0.16, 0]], [-0.077054, [3, -0.16, -0.0868456], [3, 0.133333, 0.0723713]], [0.11816, [3, -0.133333, 0], [3, 0.186667, 0]], [0.11816, [3, -0.186667, 0], [3, 0, 0]]])
names.append("LHipYawPitch")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[-0.358915, [3, -0.173333, 0], [3, 0.16, 0]], [-0.443284, [3, -0.16, 0], [3, 0.133333, 0]], [-0.171766, [3, -0.133333, 0], [3, 0.186667, 0]], [-0.171766, [3, -0.186667, 0], [3, 0, 0]]])
names.append("LKneePitch")
times.append([0.52, 1, 1.4, 1.96])
keys.append([[0.0292604, [3, -0.173333, 0], [3, 0.16, 0]], [0.0147056, [3, -0.16, 0.0145549], [3, 0.133333, -0.012129]], [-0.090548, [3, -0.133333, 0], [3, 0.186667, 0]], [-0.090548, [3, -0.186667, 0], [3, 0, 0]]])
mul = unit_time/2.02
times_unit = [list(np.array(t, dtype=float)*mul) for t in times]
try:
motion = ALProxy("ALMotion", robotIP, port)
motion.angleInterpolationBezier(names, times_unit, keys)
except BaseException, err:
print err
import sys
def main(robotIP):
PORT = 9559
# Create proxy to ALMemory
try:
memoryProxy = ALProxy("ALMemory", robotIP, PORT)
except Exception, e:
print "Could not create proxy to ALMemory"
print "Error was: ", e
# Create proxy to ALSonar
try:
sonarProxy = ALProxy("ALSonar", robotIP, PORT)
except Exception, e:
print "Could not create proxy to ALSonar"
print "Error was: ", e
# Subscribe to sonars, this will launch sonars (at hardware level)
# and start data acquisition.
sonarProxy.subscribe("myApplication")
# Now you can retrieve sonar data from ALMemory.
# Get sonar left first echo (distance in meters to the first obstacle).
memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
# Same thing for right.
memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
if len(sys.argv) <= 1:
print "Usage python Draw.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
Draw(robotIp,PORT)
if __name__ == "__main__":
# main()
robotIP = "127.0.0.1"
# robotIP = "192.168.0.107"
PORT = 9559
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 3)
try:
motionProxy = ALProxy("ALMotion", robotIP, PORT)
speechProxy = ALProxy("ALTextToSpeech", robotIP, PORT)
except Exception,e:
print "Could not create proxy to ALMotion"
print "Error was: ",e
sys.exit(1)
1.4835298641951802, 0.061086523819801536, 1.4172073526193958,
1.6022122533307945, 0.767944870877505, 0.12
]
GgolfPositionJointAnglesR4 = [
1.03549, 0.314159, 1.66742, 0.971064, -0.980268, 0.12
]
GgolfPositionJointAnglesR5 = [
1.4835298641951802, 0.061086523819801536, 1.4172073526193958,
1.6022122533307945, 0.038397243543875255, 0.6
] # 松杆参数
GgolfPositionJointAnglesR6 = [
1.4835298641951802, 0.061086523819801536, 1.4172073526193958,
1.6022122533307945, 0.038397243543875255, 0.04
] # 抓杆参数
memoryProxy = ALProxy("ALMemory", robotIP, PORT) # memory object
motionPrx = ALProxy("ALMotion", robotIP, PORT) # motion object
tts = ALProxy("ALTextToSpeech", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
#Lifestop = ALProxy("ALAutonomousLife", robotIP, PORT)
#Lifestop.setState("disabled")
redballProxy = ALProxy("ALRedBallDetection", robotIP, PORT)
camProxy = ALProxy("ALVideoDevice", robotIP, PORT)
#landmarkProxy = ALProxy("ALLandMarkDetection", robotIP, PORT)
# ------------------------------------------------------------------------------------------------------------#
StiffnessOn(motionPrx)
postureProxy.goToPosture("StandInit", 1.0)
#Grab()
alpha = -math.pi / 2 #valore iniziale
class MultimodalBehavior(object):
def __init__(self):
self.motion = ALProxy("ALMotion", ip, port)
self.tablet = ALProxy("ALTabletService", ip, port)
self.tablet.showImage(black_img_src)
self.tts = ALProxy("ALTextToSpeech", ip, port)
self.multimodal = RobotConnector(ip, port)
self.process = Thread(target=self.multimodal.run)
self.process.start()
self.input_data = {}
self.read_input_data()
self.start_index = 0
self.running_index = 0
def read_input_data(self):
file_in = open(input_src, "r")
for line in file_in:
self.input_data[line] = 0
file_in.close()
def pick_input(self):
#check start index
frequency = self.input_data[list(
self.input_data.keys())[self.start_index]]
if frequency >= 50:
self.start_index = self.start_index + 1
pick_index = self.start_index + (self.running_index % 30)
input_all = list(self.input_data.keys())[pick_index]
frequency = self.input_data[input_all]
self.input_data[input_all] = frequency + 1
self.running_index += 1
return input_all
def start_behavior(self, uuid):
line = self.pick_input()
utterance = line.split("\t")[1]
gesture = line.split("\t")[3].strip()
voice_tone = line.split("\t")[2]
ids = line.split("\t")[0]
file_out = open(output_src, "a")
file_out.write(uuid + "\t" + ids + "\t" + utterance + "\t" +
voice_tone + "\t" + gesture + "\n")
print(uuid + "\t" + ids + "\t" + utterance + "\t" + voice_tone + "\t" +
gesture + "\n")
file_out.close()
self.play_behavior(utterance, gesture, voice_tone)
def log_ipad_input(self, uuid, data):
file_out = open(output_src, "a")
file_out.write(uuid + "\t" + data + "\n")
file_out.close()
def play_behavior(self, utterance, gesture, voice_tone):
try:
for rule in expand_rules:
if rule[0] in utterance:
utterance = utterance.replace(rule[0], rule[1])
self.multimodal.speaking = True
tosay = "\\rst\\\\style=neutral\\\\rspd=95\\"
if voice_tone == "joy":
tosay = "\\rst\\\\rspd=95\\\\style=joyful\\\\vct=95\\"
elif voice_tone == "anger":
tosay = "\\rst\\\\rspd=95\\\\style=neutral\\\\vct=80\\\\bound=N\\"
tosay += utterance
self.tts.post.say(tosay)
if gesture == "strong_nod":
g.strong_nod(self.motion)
elif gesture == "nod":
g.nod(self.motion)
elif gesture == "light_head_shake":
g.light_head_shake(self.motion)
elif gesture == "strong_head_shake":
g.strong_head_shake(self.motion)
elif gesture == "annoyed":
g.annoyed(self.motion)
elif gesture == "idle":
g.idle(self.motion)
elif gesture == "breath":
g.breath(self.motion)
elif gesture == "slow_breath":
g.slow_breath(self.motion)
running = True
while running:
running = self.multimodal.speaking or not self.motion.areResourcesAvailable(
[
"HeadPitch", "HeadYaw", "HipPitch", "HipRoll",
"KneePitch", "RWristYaw", "RShoulderRoll"
])
time.sleep(0.1)
self.process.join()
except:
pass
pNames = "Body"
pStiffnessLists = x
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(robotIP, robotPort):
global motionProxy
global postureProxy
global tts
try:
motionProxy = ALProxy("ALMotion", robotIP, robotPort)
except Exception, e:
print "Could not create proxy to ALMotion"
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, robotPort)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
motionProxy.setWalkArmsEnabled(True, True)
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
while (1):
val = [0, 0, 0, 0, 0, 0, 0]
val[0] = int(raw_input("Instruccion: "))
if val[0] == 3:
val[1] = 6
val[2] = 0
val[3] = 0
val[4] = 1
pNames = "Body"
pStiffnessLists = 1.0
pTimeLists = 1.0
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
def main(ROBOT_IP):
# Init proxies.
try:
motionProxy = ALProxy("ALMotion", ROBOT_IP, PORT)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", ROBOT_IP, PORT)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
# Set NAO in stiffness On
StiffnessOn(motionProxy)
postureProxy.goToPosture("StandInit", 0.5)
###############################
# First we defined each step
###############################
footStepsList = []
# 1) Step forward with your left foot
footStepsList.append([["LLeg"], [[0.06, 0.1, 0.0]]])
class xboxcontroller():
def __init__(self):
if robotConnected:
self.motionProxy = ALProxy("ALMotion", robotIP, PORT)
self.postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
self.textToSpeachProxy = ALProxy("ALTextToSpeech", robotIP, PORT)
self.ledsProxy = ALProxy("ALLeds", robotIP, PORT)
self.basicAwarenessProxy = ALProxy("ALBasicAwareness", robotIP,
PORT)
# Initialize variables
self.motionProxy.setExternalCollisionProtectionEnabled("All", False)
self.joysticks = []
self.clock = pygame.time.Clock()
self.keepPlaying = True
self.xVel = self.yVel = self.tVel = 0.0
self.maxVel = 0.9
self.minVel = -0.9
self.headPitch = 0.0 # Updated at initial head command
self.headYaw = 0.0 # Updated at initial head command
self.shoulderPitch = 1.5 # Updated at initial arm command
self.shoulderRoll = 0.0
self.elbowRoll = 0.5
self.maxShoulderPitch = 1.6
self.minShoulderPitch = -1.0
self.maxShoulderRoll = 0.05
self.minShoulderRoll = -1.5
self.maxElbowRoll = -0.05
self.minElbowRoll = -1.5
self.block = False
self.rightArm = True
self.startControl = False
# Check for controllers connected
for i in range(0, pygame.joystick.get_count()):
self.joysticks.append(
pygame.joystick.Joystick(i)) # create a Joystick object
self.joysticks[-1].init(
) # initialize them all (-1 means loop forever)
print "Detected joystick '", self.joysticks[-1].get_name(), "'"
def runningCode(self):
print "Controller ready"
while self.keepPlaying:
self.clock.tick(60)
for event in pygame.event.get():
if event.type == pygame.JOYAXISMOTION:
self.readStick(event.axis, event.value)
if event.type == pygame.JOYHATMOTION:
self.readPad(event.value)
if event.type == pygame.JOYBUTTONDOWN:
self.readButtonDown(event.button)
if robotConnected and self.startControl:
self.sendArmPosition(self.shoulderPitch, self.shoulderRoll,
self.elbowRoll)
def initRobot(self):
if robotConnected:
self.startControl = True
# self.stopMoving()
self.basicAwarenessProxy.pauseAwareness()
# self.postureProxy.goToPosture("StandInit", 0.1) #Speed 0.0 and 1.0
# self.motionProxy.setExternalCollisionProtectionEnabled("All",False)
# self.ledsProxy.fadeRGB('ChestLeds',0.0,1.0,1.0, 0.5) # Green
self.motionProxy.setMoveArmsEnabled(True, False)
print "Awareness off"
def pauseRobot(self):
if robotConnected:
self.startControl = False
# self.stopMoving()
self.basicAwarenessProxy.resumeAwareness()
self.basicAwarenessProxy.setEngagementMode("FullyEngaged")
self.basicAwarenessProxy.setTrackingMode("Head")
# self.motionProxy.setExternalCollisionProtectionEnabled("All",True)
# self.ledsProxy.fadeRGB('ChestLeds',1.0,1.0,1.0, 0.5)
print "Awareness on"
def readStick(self, axis, value):
if self.startControl == True:
if axis in [1, 0]:
self.driveBody(axis, value)
if axis == 5 and self.block == False:
self.driveForearms(axis, value)
if axis in [3, 4] and self.block == False:
self.driveArms(axis, value)
def readPad(self, value):
if self.startControl == True:
self.driveHead(value)
def readButtonDown(self, value):
if value == 0: # A
self.pauseRobot()
if value == 1: # B
self.stopMoving()
if value == 2: # X
self.initRobot()
if value == 3: # Y
pass
if value == 4: # LB
self.rightArm = False
self.motionProxy.setStiffnesses('RArm', 0.0)
self.motionProxy.setStiffnesses('LArm', 1.0)
if value == 5: # RB
self.rightArm = True
self.motionProxy.setStiffnesses('RArm', 1.0)
self.motionProxy.setStiffnesses('LArm', 0.0)
if value == 6: # Back
self.quitController()
if value == 7: # Start
pass
if value == 8: # center button
pass
if value == 9: # left joy stick
pass
if value == 10: # right joy stick
if self.block == False:
self.block = True
print "Block true"
self.ledsProxy.fadeRGB('ChestLeds', 1.0, 1.0, 1.0, 0.5)
else:
self.block = False
print "Block false"
self.ledsProxy.fadeRGB('ChestLeds', 0.0, 0.0, 1.0, 0.5)
def driveHead(self, value):
if self.startControl and robotConnected:
if value == (0, 1): #Up:
self.motionProxy.changeAngles("HeadPitch", -0.2, 0.1)
if value == (0, -1): #down:
self.motionProxy.changeAngles("HeadPitch", 0.2, 0.1)
if value == (-1, 0): #left
self.motionProxy.changeAngles("HeadYaw", 0.2, 0.1)
if value == (1, 0): #right
self.motionProxy.changeAngles("HeadYaw", -0.2, 0.1)
def driveArms(self, axis, value):
if axis == 4 and value < 0: # Theta axis
self.shoulderPitch = self.limitRange(self.minShoulderPitch,
self.maxShoulderPitch, -1, 0,
value)
if self.rightArm == True:
if axis == 3 and value > 0: # Theta axis
self.shoulderRoll = -self.limitRange(
self.minShoulderRoll, self.maxShoulderRoll, -1, 0, value)
else:
if axis == 3 and value < 0: # Theta axis
self.shoulderRoll = -self.limitRange(
self.minShoulderRoll, self.maxShoulderRoll, -1, 0, value)
self.sendArmPosition(self.shoulderPitch, self.shoulderRoll,
self.elbowRoll)
def driveForearms(self, axis, value):
if self.rightArm == True:
self.elbowRoll = -self.limitRange(self.minElbowRoll,
self.maxElbowRoll, 1, -1, value)
if self.rightArm == False:
self.elbowRoll = self.limitRange(self.minElbowRoll,
self.maxElbowRoll, 1, -1, value)
self.sendArmPosition(self.shoulderPitch, self.shoulderRoll,
self.elbowRoll)
def driveBody(self, axis, value):
self.basicAwarenessProxy.pauseAwareness()
self.headPitch = 0.0
self.headYaw = 0.0
if axis == 1: # X axis
self.xVel = -self.limitRange(self.minVel, self.maxVel, -1, 1,
value)
if axis == 0: # Theta axis
self.tVel = -self.limitRange(self.minVel, self.maxVel, -1, 1,
value)
self.sendVelocity(self.xVel, self.yVel, self.tVel)
self.sendArmPosition(self.shoulderPitch, self.shoulderRoll,
self.elbowRoll)
def blockArms(self):
pass
def limitRange(self, minOut, maxOut, minIn, maxIn, inputValue):
outputValue = ((maxOut - minOut) *
(inputValue - minIn)) / (maxIn - minIn) + minOut
return round(outputValue, 1)
def stopMoving(self):
self.xVel = self.yVel = self.tVel = 0.0
if robotConnected:
self.sendVelocity(self.xVel, self.yVel, self.tVel)
def sendVelocity(self, xVel, yVel, tVel):
#print "Vel:", self.xVel, self.yVel, self.tVel
if robotConnected:
self.motionProxy.moveToward(xVel, yVel, tVel)
def sendArmPosition(self, shoulderPitch, shoulderRoll, elbowRoll):
#print "Arm:", self.shoulderPitch, self.shoulderRoll, self.elbowRoll
if self.rightArm == True:
names = [
"RShoulderPitch", "RShoulderRoll", "RElbowRoll", "LElbowRoll"
]
angles = [shoulderPitch, shoulderRoll, elbowRoll, -0.2]
else:
names = [
"LShoulderPitch", "LShoulderRoll", "LElbowRoll", "RElbowRoll"
]
angles = [shoulderPitch, shoulderRoll, elbowRoll, 0.2]
if robotConnected:
self.motionProxy.setAngles(names, angles, 0.1)
def sendShakeHand(self):
if robotConnected:
self.motionProxy.closeHand("RHand")
self.motionProxy.openHand("RHand")
def quitController(self):
self.keepPlaying = False
if robotConnected:
self.stopMoving()
self.basicAwarenessProxy.resumeAwareness()
self.motionProxy.setExternalCollisionProtectionEnabled("All", True)
self.ledsProxy.fadeRGB('ChestLeds', 1.0, 1.0, 1.0, 0.5)
def main(robot_IP, robot_PORT=9559):
# ----------> Connect to robot <----------
aup = ALProxy("ALAudioPlayer", robot_IP, robot_PORT)
# ----------> stop all music <----------
aup.stopAll()
def main(robotIP, port):
# Choregraphe bezier export in Python.
from naoqi import ALProxy
names = list()
times = list()
keys = list()
names.append("HeadPitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append(
[[-0.162965, [3, -0.0133333, 0], [3, 0.253333, 0]], [-0.0855211, [3, -0.253333, 0], [3, 0.266667, 0]],
[-0.162965, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.0401426, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.162965, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.0855211, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.162965, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.0401426, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.162965, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.00777732, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-0.166516, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("HeadYaw")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append(
[[0.00414634, [3, -0.0133333, 0], [3, 0.253333, 0]], [-0.307178, [3, -0.253333, 0], [3, 0.266667, 0]],
[0.00414634, [3, -0.266667, -0.102393], [3, 0.266667, 0.102393]],
[0.307178, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.00414634, [3, -0.266667, 0.102393], [3, 0.266667, -0.102393]],
[-0.307178, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.00414634, [3, -0.266667, -0.102393], [3, 0.266667, 0.102393]],
[0.307178, [3, -0.266667, 0], [3, 0.266667, 0]], [0.00414634, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.00684825, [3, -0.0933333, 0], [3, 0.0133333, 0]], [0.00684825, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LAnklePitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append(
[[0.0878904, [3, -0.0133333, 0], [3, 0.253333, 0]], [-0.813323, [3, -0.253333, 0], [3, 0.266667, 0]],
[0.0878904, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.813323, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.0878904, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.813323, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.0878904, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.813323, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.0878904, [3, -0.266667, 0], [3, 0.0933333, 0]], [0.0014263, [3, -0.0933333, 0], [3, 0.0133333, 0]],
[0.0898795, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LAnkleRoll")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-0.120527, [3, -0.0133333, 0], [3, 0.52, 0]], [-0.120527, [3, -0.52, 0], [3, 0.266667, 0]],
[-0.120527, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.120527, [3, -0.266667, 0], [3, 0.533333, 0]],
[-0.120527, [3, -0.533333, 0], [3, 0.266667, 0]], [-0.120527, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.120527, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.000127128, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-0.122208, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LElbowRoll")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-1.53333, [3, -0.0133333, 0], [3, 0.52, 0]], [-1.53333, [3, -0.52, 0], [3, 0.266667, 0]],
[-1.53333, [3, -0.266667, 0], [3, 0.266667, 0]], [-1.53333, [3, -0.266667, 0], [3, 0.533333, 0]],
[-1.53333, [3, -0.533333, 0], [3, 0.266667, 0]], [-1.53333, [3, -0.266667, 0], [3, 0.266667, 0]],
[-1.53333, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.0440015, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-0.417475, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LElbowYaw")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-0.823919, [3, -0.0133333, 0], [3, 0.52, 0]], [-0.823919, [3, -0.52, 0], [3, 0.266667, 0]],
[-0.823919, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.823919, [3, -0.266667, 0], [3, 0.533333, 0]],
[-0.823919, [3, -0.533333, 0], [3, 0.266667, 0]], [-0.823919, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.823919, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.000165939, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-1.19334, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LHand")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.8, [3, -0.0133333, 0], [3, 0.253333, 0]], [0, [3, -0.253333, 0], [3, 0.266667, 0]],
[0.8, [3, -0.266667, 0], [3, 0.266667, 0]], [0, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.8, [3, -0.266667, 0], [3, 0.266667, 0]], [0, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.8, [3, -0.266667, 0], [3, 0.266667, 0]], [0, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.8, [3, -0.266667, 0], [3, 0.0933333, 0]], [0.000843811, [3, -0.0933333, 0], [3, 0.0133333, 0]],
[0.293851, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LHipPitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.122946, [3, -0.0133333, 0], [3, 0.253333, 0]], [-0.58294, [3, -0.253333, 0], [3, 0.266667, 0]],
[0.122946, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.58294, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.122946, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.58294, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.122946, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.58294, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.122946, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.000129679, [3, -0.0933333, 0], [3, 0.0133333, 0]], [0.122208, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LHipRoll")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.0901686, [3, -0.0133333, 0], [3, 0.52, 0]], [0.0901686, [3, -0.52, 0], [3, 0.266667, 0]],
[0.0901686, [3, -0.266667, 0], [3, 0.266667, 0]], [0.0901686, [3, -0.266667, 0], [3, 0.533333, 0]],
[0.0901686, [3, -0.533333, 0], [3, 0.266667, 0]], [0.0901686, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.0901686, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.00146327, [3, -0.0933333, 0], [3, 0.0133333, 0]], [0.0940062, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LHipYawPitch")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-0.163908, [3, -0.0133333, 0], [3, 0.52, 0]], [-0.163908, [3, -0.52, 0], [3, 0.266667, 0]],
[-0.163908, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.163908, [3, -0.266667, 0], [3, 0.533333, 0]],
[-0.163908, [3, -0.533333, 0], [3, 0.266667, 0]], [-0.163908, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.163908, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.00782232, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-0.166516, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LKneePitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-0.0851167, [3, -0.0133333, 0], [3, 0.253333, 0]], [1.4591, [3, -0.253333, 0], [3, 0.266667, 0]],
[-0.0851167, [3, -0.266667, 0], [3, 0.266667, 0]], [1.4591, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.0851167, [3, -0.266667, 0], [3, 0.266667, 0]], [1.4591, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.0851167, [3, -0.266667, 0], [3, 0.266667, 0]], [1.4591, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.0851167, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.00794698, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-0.0846055, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LShoulderPitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-0.444579, [3, -0.0133333, 0], [3, 0.253333, 0]], [-2.08567, [3, -0.253333, 0], [3, 0.266667, 0]],
[-0.444579, [3, -0.266667, -0.332485], [3, 0.266667, 0.332485]],
[-0.0907571, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.444579, [3, -0.266667, 0.332485], [3, 0.266667, -0.332485]],
[-2.08567, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.444579, [3, -0.266667, -0.332485], [3, 0.266667, 0.332485]],
[-0.0907571, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.444579, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.0024113, [3, -0.0933333, -0.442168], [3, 0.0133333, 0.0631668]],
[1.47006, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LShoulderRoll")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.501883, [3, -0.0133333, 0], [3, 0.52, 0]], [0.501883, [3, -0.52, 0], [3, 0.266667, 0]],
[0.501883, [3, -0.266667, 0], [3, 0.266667, 0]], [0.501883, [3, -0.266667, 0], [3, 0.533333, 0]],
[0.501883, [3, -0.533333, 0], [3, 0.266667, 0]], [0.501883, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.501883, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.0114014, [3, -0.0933333, 0], [3, 0.0133333, 0]], [0.183011, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("LWristYaw")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-1.23717, [3, -0.0133333, 0], [3, 0.52, 0]], [-1.23717, [3, -0.52, 0], [3, 0.266667, 0]],
[-1.23717, [3, -0.266667, 0], [3, 0.266667, 0]], [-1.23717, [3, -0.266667, 0], [3, 0.533333, 0]],
[-1.23717, [3, -0.533333, 0], [3, 0.266667, 0]], [-1.23717, [3, -0.266667, 0], [3, 0.266667, 0]],
[-1.23717, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.00294556, [3, -0.0933333, -0.387871], [3, 0.0133333, 0.0554102]],
[0.0926748, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RAnklePitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.09, [3, -0.0133333, 0], [3, 0.253333, 0]], [-0.813323, [3, -0.253333, 0], [3, 0.266667, 0]],
[0.09, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.813323, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.09, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.813323, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.09, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.813323, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.09, [3, -0.266667, 0], [3, 0.0933333, 0]], [0.00146054, [3, -0.0933333, 0], [3, 0.0133333, 0]],
[0.0898795, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RAnkleRoll")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.125933, [3, -0.0133333, 0], [3, 0.52, 0]], [0.125933, [3, -0.52, 0], [3, 0.266667, 0]],
[0.125933, [3, -0.266667, 0], [3, 0.266667, 0]], [0.125933, [3, -0.266667, 0], [3, 0.533333, 0]],
[0.125933, [3, -0.533333, 0], [3, 0.266667, 0]], [0.125933, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.125933, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.00601001, [3, -0.0933333, 0], [3, 0.0133333, 0]], [0.122208, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RElbowRoll")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[1.53333, [3, -0.0133333, 0], [3, 0.253333, 0]], [1.54462, [3, -0.253333, 0], [3, 0.266667, 0]],
[1.53333, [3, -0.266667, 0], [3, 0.266667, 0]], [1.53333, [3, -0.266667, 0], [3, 0.266667, 0]],
[1.53333, [3, -0.266667, 0], [3, 0.266667, 0]], [1.54462, [3, -0.266667, 0], [3, 0.266667, 0]],
[1.53333, [3, -0.266667, 0], [3, 0.266667, 0]], [1.53333, [3, -0.266667, 0], [3, 0.266667, 0]],
[1.53333, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.0440015, [3, -0.0933333, 0], [3, 0.0133333, 0]], [0.417475, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RElbowYaw")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.823919, [3, -0.0133333, 0], [3, 0.52, 0]], [0.823919, [3, -0.52, 0], [3, 0.266667, 0]],
[0.823919, [3, -0.266667, 0], [3, 0.266667, 0]], [0.823919, [3, -0.266667, 0], [3, 0.533333, 0]],
[0.823919, [3, -0.533333, 0], [3, 0.266667, 0]], [0.823919, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.823919, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.00016594, [3, -0.0933333, 0], [3, 0.0133333, 0]], [1.19334, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RHand")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.8, [3, -0.0133333, 0], [3, 0.253333, 0]], [0.12, [3, -0.253333, 0], [3, 0.266667, 0]],
[0.8, [3, -0.266667, 0], [3, 0.266667, 0]], [0, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.8, [3, -0.266667, 0], [3, 0.266667, 0]], [0.12, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.8, [3, -0.266667, 0], [3, 0.266667, 0]], [0, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.8, [3, -0.266667, 0], [3, 0.0933333, 0]], [0.000843811, [3, -0.0933333, 0], [3, 0.0133333, 0]],
[0.293851, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RHipPitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[0.122946, [3, -0.0133333, 0], [3, 0.253333, 0]], [-0.58294, [3, -0.253333, 0], [3, 0.266667, 0]],
[0.122946, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.58294, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.122946, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.58294, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.122946, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.58294, [3, -0.266667, 0], [3, 0.266667, 0]],
[0.122946, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.000129679, [3, -0.0933333, 0], [3, 0.0133333, 0]], [0.122208, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RHipRoll")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-0.0901686, [3, -0.0133333, 0], [3, 0.52, 0]], [-0.0901686, [3, -0.52, 0], [3, 0.266667, 0]],
[-0.0901686, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.0901686, [3, -0.266667, 0], [3, 0.533333, 0]],
[-0.0901686, [3, -0.533333, 0], [3, 0.266667, 0]],
[-0.0901686, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.0901686, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.00146327, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-0.0940062, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RHipYawPitch")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-0.163908, [3, -0.0133333, 0], [3, 0.52, 0]], [-0.163908, [3, -0.52, 0], [3, 0.266667, 0]],
[-0.164061, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.163908, [3, -0.266667, 0], [3, 0.533333, 0]],
[-0.163908, [3, -0.533333, 0], [3, 0.266667, 0]], [-0.164061, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.163908, [3, -0.266667, -0.000153415], [3, 0.0933333, 5.36952e-05]],
[-0.00782232, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-0.166516, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RKneePitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[-0.0820331, [3, -0.0133333, 0], [3, 0.253333, 0]], [1.4591, [3, -0.253333, 0], [3, 0.266667, 0]],
[-0.0820331, [3, -0.266667, 0], [3, 0.266667, 0]], [1.4591, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.0820331, [3, -0.266667, 0], [3, 0.266667, 0]], [1.4591, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.0820331, [3, -0.266667, 0], [3, 0.266667, 0]], [1.4591, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.0820331, [3, -0.266667, 0], [3, 0.0933333, 0]],
[-0.00765907, [3, -0.0933333, 0], [3, 0.0133333, 0]], [-0.0846055, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RShoulderPitch")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append(
[[-0.444579, [3, -0.0133333, 0], [3, 0.253333, 0]], [-0.0610865, [3, -0.253333, 0], [3, 0.266667, 0]],
[-0.444579, [3, -0.266667, 0.33743], [3, 0.266667, -0.33743]],
[-2.08567, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.444579, [3, -0.266667, -0.33743], [3, 0.266667, 0.33743]],
[-0.0610865, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.444579, [3, -0.266667, 0.33743], [3, 0.266667, -0.33743]],
[-2.08567, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.444579, [3, -0.266667, -0.514384], [3, 0.0933333, 0.180035]],
[-0.00241132, [3, -0.0933333, -0.442168], [3, 0.0133333, 0.0631668]],
[1.47006, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RShoulderRoll")
times.append([0.04, 0.8, 1.6, 2.4, 3.2, 4, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append(
[[-0.501883, [3, -0.0133333, 0], [3, 0.253333, 0]], [-0.413643, [3, -0.253333, 0], [3, 0.266667, 0]],
[-0.501883, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.501883, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.501883, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.413643, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.501883, [3, -0.266667, 0], [3, 0.266667, 0]], [-0.501883, [3, -0.266667, 0], [3, 0.266667, 0]],
[-0.501883, [3, -0.266667, 0], [3, 0.0933333, 0]], [-0.0114014, [3, -0.0933333, 0], [3, 0.0133333, 0]],
[-0.183011, [3, -0.0133333, 0], [3, 0, 0]]])
names.append("RWristYaw")
times.append([0.04, 1.6, 2.4, 3.2, 4.8, 5.6, 6.4, 6.68, 6.72])
keys.append([[1.23717, [3, -0.0133333, 0], [3, 0.52, 0]], [1.23717, [3, -0.52, 0], [3, 0.266667, 0]],
[1.23717, [3, -0.266667, 0], [3, 0.266667, 0]], [1.23717, [3, -0.266667, 0], [3, 0.533333, 0]],
[1.23717, [3, -0.533333, 0], [3, 0.266667, 0]], [1.23717, [3, -0.266667, 0], [3, 0.266667, 0]],
[1.23717, [3, -0.266667, 0], [3, 0.0933333, 0]],
[0.00294556, [3, -0.0933333, 0], [3, 0.0133333, 0]], [0.0984608, [3, -0.0133333, 0], [3, 0, 0]]])
try:
# uncomment the following line and modify the IP if you use this script outside Choregraphe.
motion = ALProxy("ALMotion", robotIP, port)
# motion = ALProxy("ALMotion")
motion.angleInterpolationBezier(names, times, keys)
except BaseException, err:
print err
def main():
rospy.init_node('read_lines_location_event')
global robot_state_pub
robot_state_pub = rospy.Publisher("robot_state", String, queue_size=5)
robot_state_time_pub = rospy.Publisher("robot_state_time", Pose, queue_size=5)
global current_word_pub
current_word_pub = rospy.Publisher("current_word", String, queue_size=5)
current_word_time_pub = rospy.Publisher("current_word_time", Pose, queue_size=5)
global detected_tags_pub
detected_tags_pub = rospy.Publisher("detected_tags", String, queue_size=5)
global robot_hand_pose_pub
robot_hand_pose_pub = rospy.Publisher("robot_hand_pose", Pose, queue_size=10)
robot_hand_pose_time_pub = rospy.Publisher("robot_hand_pose_time", Pose, queue_size=5)
#nao_IP = rospy.get_param('~nao_ip')
nao_IP = 'nao.local'
global story
story = ALProxy("ALTextToSpeech", nao_IP, 9559)
global conversration
conversation = ALProxy("ALTextToSpeech", nao_IP, 9559)
global motionProxy
motionProxy = ALProxy("ALMotion", nao_IP, 9559)
global postureProxy
postureProxy = ALProxy("ALRobotPosture", nao_IP, 9559)
global audiomotionProxy
audiomotionProxy = ALProxy("ALAudioPlayer", nao_IP, 9559)
global tracker
tracker = ALProxy("ALTracker", nao_IP, 9559)
global proxy
proxy = ALProxy("ALLeds", nao_IP, 9559)
global audioReactionProxy
audioReactionProxy = ALProxy("ALAudioPlayer", nao_IP, 9559)
# Initialize the node
logFile = logger()
#storyNum = 1
#pub = rospy.Publisher('story_number', String, queue_size=1)
#pub.publish(str(storyNum))
#storySelection()
# Face tracking activated
faceTrackingEnded()
facesize = 0.1
global restingEnabled
restingEnabled = False
faceTrackingStarted(facesize)
global space
space = motion.FRAME_ROBOT
# Select a story and activity level
global selectedStory
#selectedStory = storySelection()
correctFlag = False
activityLevel = rospy.get_param('actlevel', 'medium')
#mistakeNum = numberOfMistakesSelection(activityLevel)
time.sleep(1)
IntroduceNao()
#rospy.Subscriber('tag_id_state', String, IntroduceNao)
faceTrackingEnded()
#time.sleep(1)
pitch_angle = 0.3
yaw_angle = 0.8
LookAtTheBook(pitch_angle)
"""waitAndRecord = RECORDANDTRANSCRIBE()
waitAndRecord.recordTheOutput('new.wav')
response = waitAndRecord.transcribeTheOutput()
#print recordedWord
reWord = response["results"][0]["alternatives"][0]["transcript"]
reConf = response["results"][0]["alternatives"][0]["confidence"]
repeatWord = '\\rspd=80\\' + str(reWord)
story.say(repeatWord)
print reConf
print reWord"""
rospy.Subscriber('tag_id_state', String, tagDetection)
try:
while True:
time.sleep(1)
except restingEnabled == True:
print
print "Interrupted by user, shutting down"
myBroker.shutdown()
sys.exit(0)
classes = j['images'][0]['classifiers'][0]['classes']
classes.sort(key=lambda x: x['score'], reverse=True)
for i in classes:
if i['class'] in predefined_classes:
return i['class']
#return classes[0]['class']
except Exception as e:
print(e)
return None
if ROBOT:
proxy = ALProxy('ALVideoDevice', '192.168.1.19', 9559)
videoClient = proxy.subscribe('python_client', 1, 11, 5)
tts = ALProxy('ALTextToSpeech', '192.168.1.19', 9559)
tts.setLanguage('English')
led = ALProxy('ALLeds', '192.168.1.19', 9559)
def read_item_name(name):
tts.say("One, {}".format(str(classes[c_id])))
def rotate_eyes():
led.rotateEyes(0x0000ff00, 0.3, 1.0)
fgbg = cv2.createBackgroundSubtractorMOG2()
def main():
global story
story = ALProxy("ALTextToSpeech", "nao.local", 9559)
global conversration
conversation = ALProxy("ALTextToSpeech", "nao.local", 9559)
global motionProxy
motionProxy = ALProxy("ALMotion", "nao.local", 9559)
global postureProxy
postureProxy = ALProxy("ALRobotPosture", "nao.local", 9559)
global audiomotionProxy
audiomotionProxy = ALProxy("ALAudioPlayer", "nao.local", 9559)
global tracker
tracker = ALProxy("ALTracker", "nao.local", 9559)
global proxy
proxy = ALProxy("ALLeds", "nao.local", 9559)
# Initialize the node
rospy.init_node('read_lines_event')
#storyNum = 1
#pub = rospy.Publisher('story_number', String, queue_size=1)
#pub.publish(str(storyNum))
#storySelection()
# Face tracking activated
facesize = 0.1
global restingEnabled
restingEnabled = False
faceTrackingStarted(facesize)
global space
space = motion.FRAME_ROBOT
# Select a story and activity level
global selectedStory
#selectedStory = storySelection()
correctFlag = False
activityLevel = rospy.get_param('actlevel', 'medium')
#mistakeNum = numberOfMistakesSelection(activityLevel)
time.sleep(3)
IntroduceNao()
#rospy.Subscriber('tag_id_state', String, IntroduceNao)
faceTrackingEnded()
pitch_angle = 0.8
LookAtTheBook(pitch_angle)
global effector
effector = "LArm"
rospy.Subscriber('tag_id_state', String, tag_detection)
#rospy.Subscriber('card_id_state', String, card_detection)
#readTheTaggedStory(selectedStory, correctFlag)
#readTheTaggedStoryWithLevel(selectedStory, correctFlag, mistakeNum)
#rospy.Subscriber('button_start', String, startTheStoryReading)
#rospy.Subscriber('button_wrong', String, mistakeDetected)
#rospy.Subscriber('keys', String, faceTrackingEnded)
#rospy.Subscriber('keys', String, repeatTheStoryReading)
try:
while True:
time.sleep(1)
except restingEnabled == True:
print
print "Interrupted by user, shutting down"
myBroker.shutdown()
sys.exit(0)
def keyPressEvent(self, e):
global flag
global i
if e.key() == QtCore.Qt.Key_S:
flag = 1
i = 0
print ("Start Capture")
elif e.key() == QtCore.Qt.Key_Q:
flag = 0
print ("End Capture")
s.sendall('Event')
data = s.recv(1024)
ans = int(data.encode('hex'), 16)
print(ans)
tts = ALProxy("ALTextToSpeech", self.nao_ip, self.nao_port)
if ans == 0:
# no gesture
tts.say("No Gesture Detected")
elif ans == 1:
# left
pi_2 = -3.1415/2
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
posture = postureProxy.getPosture()
if(posture == 'Sit'):
tts.say("I need to stand first")
postureProxy.goToPosture("Stand", 1.0)
tts.say("Moving to your Left")
motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)
motion.moveInit()
motion.moveTo(0, 0, pi_2)
motion.moveTo(0.2, 0, 0)
elif ans == 2:
# right
pi_2 = 3.1415/2
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
posture = postureProxy.getPosture()
if(posture == 'Sit'):
tts.say("I need to stand first")
postureProxy.goToPosture("Stand", 1.0)
tts.say("Moving to your Right")
motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)
motion.moveInit()
motion.moveTo(0, 0, pi_2)
motion.moveTo(0.2, 0, 0)
elif ans == 3:
# down
tts.say("Sit Down")
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
postureProxy.goToPosture("Sit", 1.0)
elif ans == 4:
# up
tts.say("Stand Up")
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
postureProxy.goToPosture("Stand", 1.0)
elif ans == 5:
# shaking hand
tts.say("Hi")
elif ans == 6:
# stop
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
posture = postureProxy.getPosture()
if(posture == 'Sit'):
tts.say("I need to stand first")
postureProxy.goToPosture("Stand", 1.0)
tts.say("Moving Back")
motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)
motion.moveInit()
motion.moveTo(-0.2, 0, 0)
elif ans == 7:
# forward
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
posture = postureProxy.getPosture()
if(posture == 'Sit'):
tts.say("I need to stand first")
postureProxy.goToPosture("Stand", 1.0)
tts.say("Moving Forward")
motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)
motion.moveInit()
motion.moveTo(0.2, 0, 0)
files = glob.glob('./temp/test_img/*')
for f in files:
os.remove(f)
self.close()
s.close()
if __name__ == '__main__':
IP = "172.16.21.202" # Replace here with your NaoQi's IP address.
PORT = 9559
CameraID = 0
# Read IP address from first argument if any.
if len(sys.argv) > 1:
IP = sys.argv[1]
# Read CameraID from second argument if any.
if len(sys.argv) > 2:
CameraID = int(sys.argv[2])
aware = ALProxy("ALBasicAwareness",IP,PORT)
aware.setEngagementMode("FullyEngaged")
aware.setStimulusDetectionEnabled("Sound",False)
aware.startAwareness()
app = QApplication(sys.argv)
myWidget = ImageWidget(IP, PORT, CameraID)
myWidget.show()
sys.exit(app.exec_())
def main(robotIP, port, unit_time):
names = list()
times = list()
keys = list()
names.append("HeadPitch")
times.append([0.52, 1, 1.52, 2, 2.44, 3.08, 3.52, 3.92])
keys.append([[0.108872, [3, -0.173333, 0], [3, 0.16, 0]], [0.0981341, [3, -0.16, 0], [3, 0.173333, 0]], [0.4034, [3, -0.173333, -0.0752477], [3, 0.16, 0.0694594]], [0.532256, [3, -0.16, 0], [3, 0.146667, 0]], [-0.182588, [3, -0.146667, 0], [3, 0.213333, 0]], [0.085862, [3, -0.213333, -0.115751], [3, 0.146667, 0.0795785]], [0.4034, [3, -0.146667, 0], [3, 0.133333, 0]], [-0.144238, [3, -0.133333, 0], [3, 0, 0]]])
names.append("HeadYaw")
times.append([0.52, 1, 1.52, 2, 2.44, 3.08, 3.52, 3.92])
keys.append([[-0.015382, [3, -0.173333, 0], [3, 0.16, 0]], [-0.0184499, [3, -0.16, 0.00306792], [3, 0.173333, -0.00332358]], [-0.426494, [3, -0.173333, 0], [3, 0.16, 0]], [-0.20944, [3, -0.16, -0.052823], [3, 0.146667, 0.048421]], [-0.122762, [3, -0.146667, 0], [3, 0.213333, 0]], [-0.128898, [3, -0.213333, 0.00613606], [3, 0.146667, -0.00421854]], [-0.426494, [3, -0.146667, 0], [3, 0.133333, 0]], [-0.032256, [3, -0.133333, 0], [3, 0, 0]]])
names.append("LAnklePitch")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[-0.292008, [3, -0.2, 0], [3, 0.493333, 0]], [-0.0112903, [3, -0.493333, 0], [3, 0.36, 0]], [-0.121738, [3, -0.36, 0], [3, 0.253333, 0]], [0.093532, [3, -0.253333, 0], [3, 0, 0]]])
names.append("LAnkleRoll")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[0.0227781, [3, -0.2, 0], [3, 0.493333, 0]], [-0.193732, [3, -0.493333, 0], [3, 0.36, 0]], [0.0483652, [3, -0.36, 0], [3, 0.253333, 0]], [-0.116542, [3, -0.253333, 0], [3, 0, 0]]])
names.append("LElbowRoll")
times.append([0.56, 1.04, 1.56, 2.04, 2.48, 3.12, 3.56, 3.92])
keys.append([[-0.472429, [3, -0.186667, 0], [3, 0.16, 0]], [-0.624296, [3, -0.16, 0], [3, 0.173333, 0]], [-0.424876, [3, -0.173333, 0], [3, 0.16, 0]], [-0.61049, [3, -0.16, 0], [3, 0.146667, 0]], [-0.429478, [3, -0.146667, 0], [3, 0.213333, 0]], [-0.605888, [3, -0.213333, 0], [3, 0.146667, 0]], [-0.424876, [3, -0.146667, 0], [3, 0.12, 0]], [-0.427944, [3, -0.12, 0], [3, 0, 0]]])
names.append("LElbowYaw")
times.append([0.56, 1.04, 1.56, 2.04, 2.48, 3.12, 3.56, 3.92])
keys.append([[-2.00037, [3, -0.186667, 0], [3, 0.16, 0]], [-1.51563, [3, -0.16, 0], [3, 0.173333, 0]], [-1.68591, [3, -0.173333, 0], [3, 0.16, 0]], [-1.5233, [3, -0.16, 0], [3, 0.146667, 0]], [-1.6767, [3, -0.146667, 0], [3, 0.213333, 0]], [-1.51717, [3, -0.213333, 0], [3, 0.146667, 0]], [-1.68591, [3, -0.146667, 0], [3, 0.12, 0]], [-1.17509, [3, -0.12, 0], [3, 0, 0]]])
names.append("LHand")
times.append([0.48, 1.48, 3.48, 3.92])
keys.append([[0.295298, [3, -0.16, 0], [3, 0.333333, 0]], [0.237117, [3, -0.333333, 0], [3, 0.666667, 0]], [0.237117, [3, -0.666667, 0], [3, 0.146667, 0]], [0.3048, [3, -0.146667, 0], [3, 0, 0]]])
names.append("LHipPitch")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[0.26973, [3, -0.2, 0], [3, 0.493333, 0]], [0.33444, [3, -0.493333, -0.0248861], [3, 0.36, 0.0181601]], [0.398869, [3, -0.36, 0], [3, 0.253333, 0]], [0.138102, [3, -0.253333, 0], [3, 0, 0]]])
names.append("LHipRoll")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[0.0853684, [3, -0.2, 0], [3, 0.493333, 0]], [0.295708, [3, -0.493333, 0], [3, 0.36, 0]], [0.014986, [3, -0.36, 0], [3, 0.253333, 0]], [0.11816, [3, -0.253333, 0], [3, 0, 0]]])
names.append("LHipYawPitch")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[-0.384992, [3, -0.2, 0], [3, 0.493333, 0]], [-0.401866, [3, -0.493333, 0], [3, 0.36, 0]], [-0.384992, [3, -0.36, -0.0168739], [3, 0.253333, 0.0118742]], [-0.171766, [3, -0.253333, 0], [3, 0, 0]]])
names.append("LKneePitch")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[0.388217, [3, -0.2, 0], [3, 0.493333, 0]], [0.0300457, [3, -0.493333, 0], [3, 0.36, 0]], [0.109814, [3, -0.36, 0], [3, 0.253333, 0]], [-0.090548, [3, -0.253333, 0], [3, 0, 0]]])
names.append("LShoulderPitch")
times.append([0.56, 1.04, 1.56, 2.04, 2.48, 3.12, 3.56, 3.92])
keys.append([[1.59072, [3, -0.186667, 0], [3, 0.16, 0]], [1.57998, [3, -0.16, 0], [3, 0.173333, 0]], [1.61066, [3, -0.173333, 0], [3, 0.16, 0]], [1.60299, [3, -0.16, 0], [3, 0.146667, 0]], [1.60452, [3, -0.146667, -0.000416456], [3, 0.213333, 0.000605754]], [1.60606, [3, -0.213333, -0.00121217], [3, 0.146667, 0.000833364]], [1.61066, [3, -0.146667, 0], [3, 0.12, 0]], [1.48027, [3, -0.12, 0], [3, 0, 0]]])
names.append("LShoulderRoll")
times.append([0.56, 1.04, 1.56, 2.04, 2.48, 3.12, 3.56, 3.92])
keys.append([[0.0643861, [3, -0.186667, 0], [3, 0.16, 0]], [0.102736, [3, -0.16, 0], [3, 0.173333, 0]], [0.0904641, [3, -0.173333, 0], [3, 0.16, 0]], [0.0904641, [3, -0.16, 0], [3, 0.146667, 0]], [0.0904641, [3, -0.146667, 0], [3, 0.213333, 0]], [0.093532, [3, -0.213333, 0], [3, 0.146667, 0]], [0.0904641, [3, -0.146667, 0.00112491], [3, 0.12, -0.000920382]], [0.0873961, [3, -0.12, 0], [3, 0, 0]]])
names.append("LWristYaw")
times.append([0.48, 1.48, 3.48, 3.92])
keys.append([[0.136484, [3, -0.16, 0], [3, 0.333333, 0]], [0.567537, [3, -0.333333, 0], [3, 0.666667, 0]], [0.567537, [3, -0.666667, 0], [3, 0.146667, 0]], [0.145688, [3, -0.146667, 0], [3, 0, 0]]])
names.append("RAnklePitch")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[-0.298148, [3, -0.2, 0], [3, 0.493333, 0]], [-0.0358286, [3, -0.493333, -0.00840843], [3, 0.36, 0.00613588]], [-0.0296928, [3, -0.36, -0.00613588], [3, 0.253333, 0.00431784]], [0.107422, [3, -0.253333, 0], [3, 0, 0]]])
names.append("RAnkleRoll")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[-0.0192192, [3, -0.2, 0], [3, 0.493333, 0]], [-0.0780021, [3, -0.493333, 0], [3, 0.36, 0]], [0.0355138, [3, -0.36, -0.0293756], [3, 0.253333, 0.0206717]], [0.07214, [3, -0.253333, 0], [3, 0, 0]]])
names.append("RElbowRoll")
times.append([0.56, 1.04, 1.56, 2.04, 2.48, 3.12, 3.56, 3.92])
keys.append([[1.52944, [3, -0.186667, 0], [3, 0.16, 0]], [0.89283, [3, -0.16, 0], [3, 0.173333, 0]], [1.48649, [3, -0.173333, 0], [3, 0.16, 0]], [0.994073, [3, -0.16, 0], [3, 0.146667, 0]], [1.46961, [3, -0.146667, 0], [3, 0.213333, 0]], [0.966462, [3, -0.213333, 0], [3, 0.146667, 0]], [1.48649, [3, -0.146667, 0], [3, 0.12, 0]], [0.389678, [3, -0.12, 0], [3, 0, 0]]])
names.append("RElbowYaw")
times.append([0.56, 1.04, 1.56, 2.04, 2.48, 3.12, 3.56, 3.92])
keys.append([[0.944902, [3, -0.186667, 0], [3, 0.16, 0]], [2.0816, [3, -0.16, 0], [3, 0.173333, 0]], [0.757754, [3, -0.173333, 0], [3, 0.16, 0]], [2.0944, [3, -0.16, 0], [3, 0.146667, 0]], [0.74088, [3, -0.146667, 0], [3, 0.213333, 0]], [2.07699, [3, -0.213333, 0], [3, 0.146667, 0]], [0.757754, [3, -0.146667, 0], [3, 0.12, 0]], [1.17654, [3, -0.12, 0], [3, 0, 0]]])
names.append("RHand")
times.append([0.48, 1.48, 2, 2.8, 3.48, 3.92])
keys.append([[0.340389, [3, -0.16, 0], [3, 0.333333, 0]], [0.247273, [3, -0.333333, 0], [3, 0.173333, 0]], [0.630909, [3, -0.173333, -0.0658953], [3, 0.266667, 0.101377]], [0.749091, [3, -0.266667, 0], [3, 0.226667, 0]], [0.247273, [3, -0.226667, 0], [3, 0.146667, 0]], [0.3068, [3, -0.146667, 0], [3, 0, 0]]])
names.append("RHipPitch")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[0.25774, [3, -0.2, 0], [3, 0.493333, 0]], [0.397054, [3, -0.493333, -0.0168164], [3, 0.36, 0.0122714]], [0.409325, [3, -0.36, 0], [3, 0.253333, 0]], [0.131882, [3, -0.253333, 0], [3, 0, 0]]])
names.append("RHipRoll")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[-0.0671419, [3, -0.2, 0], [3, 0.493333, 0]], [0.0511575, [3, -0.493333, 0], [3, 0.36, 0]], [-0.148262, [3, -0.36, 0], [3, 0.253333, 0]], [-0.06592, [3, -0.253333, 0], [3, 0, 0]]])
names.append("RHipYawPitch")
times.append([3.92])
keys.append([[-0.171766, [3, -1.30667, 0], [3, 0, 0]]])
names.append("RKneePitch")
times.append([0.6, 2.08, 3.16, 3.92])
keys.append([[0.405876, [3, -0.2, 0], [3, 0.493333, 0]], [0.0307944, [3, -0.493333, 0.00210211], [3, 0.36, -0.00153397]], [0.0292604, [3, -0.36, 0.00153397], [3, 0.253333, -0.00107946]], [-0.091998, [3, -0.253333, 0], [3, 0, 0]]])
names.append("RShoulderPitch")
times.append([0.56, 1.04, 1.56, 2.04, 2.48, 3.12, 3.56, 3.92])
keys.append([[1.34536, [3, -0.186667, 0], [3, 0.16, 0]], [1.88839, [3, -0.16, 0], [3, 0.173333, 0]], [1.3561, [3, -0.173333, 0], [3, 0.16, 0]], [1.45427, [3, -0.16, 0], [3, 0.146667, 0]], [1.41899, [3, -0.146667, 0], [3, 0.213333, 0]], [1.46961, [3, -0.213333, 0], [3, 0.146667, 0]], [1.3561, [3, -0.146667, 0], [3, 0.12, 0]], [1.49569, [3, -0.12, 0], [3, 0, 0]]])
names.append("RShoulderRoll")
times.append([0.56, 1.04, 1.56, 2.04, 2.48, 3.12, 3.56, 3.92])
keys.append([[-0.112024, [3, -0.186667, 0], [3, 0.16, 0]], [-0.104354, [3, -0.16, 0], [3, 0.173333, 0]], [-0.570689, [3, -0.173333, 0], [3, 0.16, 0]], [-0.0706059, [3, -0.16, 0], [3, 0.146667, 0]], [-0.526205, [3, -0.146667, 0], [3, 0.213333, 0]], [-0.067538, [3, -0.213333, 0], [3, 0.146667, 0]], [-0.570689, [3, -0.146667, 0], [3, 0.12, 0]], [-0.104354, [3, -0.12, 0], [3, 0, 0]]])
names.append("RWristYaw")
times.append([0.48, 1.48, 3.48, 3.92])
keys.append([[-0.366667, [3, -0.16, 0], [3, 0.333333, 0]], [1.04921, [3, -0.333333, 0], [3, 0.666667, 0]], [1.04921, [3, -0.666667, 0], [3, 0.146667, 0]], [0.0720561, [3, -0.146667, 0], [3, 0, 0]]])
mul = unit_time/4.1
times_unit = [list(np.array(t, dtype=float)*mul) for t in times]
try:
motion = ALProxy("ALMotion", robotIP, port)
motion.angleInterpolationBezier(names, times_unit, keys)
except BaseException, err:
print err
by the client is saved to disk.
"""
import time
from naoqi import ALProxy
IP = "nao.local" # Enter your NAOqi's IP address here.
PORT = 9559
#________________________________
# Generic Proxy creation
#________________________________
camProxy = ALProxy("ALVideoDevice", IP, PORT)
#________________________________
# Vision module creation
#________________________________
resolution = 1 #0:QVGA 1:QVGA 2:VGA (camera doesn't provide QQVGA that is obtained through CPU processing)
colorSpace = 9 #kYUV422InterlacedColorSpace
fps = 30 #not activated
# We subscrive our video client.
nameId = "pythonGVM"
nameId = camProxy.subscribe(nameId, resolution, colorSpace, fps)
#________________________________
# Ask to grab a maximum of 3500
def _registerImageClient(self, IP, PORT):
"""
Register our video module to the robot.
"""
self._videoProxy = ALProxy("ALVideoDevice", IP, PORT)
resolution = vision_definitions.kQQVGA
colorSpace = vision_definitions.kRGBColorSpace
self._imgClient = self._videoProxy.subscribe("_client", resolution, colorSpace, 5)
# Select camera.
self._videoProxy.setParam(vision_definitions.kCameraSelectID,
self._cameraID)
def _unregisterImageClient(self):
"""
Unregister our naoqi video module.
"""
if self._imgClient != "":
self._videoProxy.unsubscribe(self._imgClient)
def paintEvent(self, event):
"""
Draw the QImage on screen.
"""
# painter = QPainter(self)
# painter.drawImage(painter.viewport(), self._image)
def _updateImage(self):
"""
Retrieve a new image from Nao.
"""
global i
global flag
self._alImage = self._videoProxy.getImageRemote(self._imgClient)
self._image = QImage(self._alImage[6], # Pixel array.
self._alImage[0], # Width.
self._alImage[1], # Height.
QImage.Format_RGB888)
if flag == 1:
if i < 10:
filename = './temp/test_img/0' + str(i) + '.png'
else:
filename = './temp/test_img/' + str(i) + '.png'
self._image.save(filename)
i +=1
def timerEvent(self, event):
"""
Called periodically. Retrieve a nao image, and update the widget.
"""
self._updateImage()
self.update()
def __del__(self):
"""
When the widget is deleted, we unregister our naoqi video module.
"""
self._unregisterImageClient()
def initUI(self):
self.setGeometry(300, 300, 250, 150)
self.setWindowTitle('Event handler')
self.show()
def keyPressEvent(self, e):
global flag
global i
if e.key() == QtCore.Qt.Key_S:
flag = 1
i = 0
print ("Start Capture")
elif e.key() == QtCore.Qt.Key_Q:
flag = 0
print ("End Capture")
s.sendall('Event')
data = s.recv(1024)
ans = int(data.encode('hex'), 16)
print(ans)
tts = ALProxy("ALTextToSpeech", self.nao_ip, self.nao_port)
if ans == 0:
# no gesture
tts.say("No Gesture Detected")
elif ans == 1:
# left
pi_2 = -3.1415/2
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
posture = postureProxy.getPosture()
if(posture == 'Sit'):
tts.say("I need to stand first")
postureProxy.goToPosture("Stand", 1.0)
tts.say("Moving to your Left")
motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)
motion.moveInit()
motion.moveTo(0, 0, pi_2)
motion.moveTo(0.2, 0, 0)
elif ans == 2:
# right
pi_2 = 3.1415/2
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
posture = postureProxy.getPosture()
if(posture == 'Sit'):
tts.say("I need to stand first")
postureProxy.goToPosture("Stand", 1.0)
tts.say("Moving to your Right")
motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)
motion.moveInit()
motion.moveTo(0, 0, pi_2)
motion.moveTo(0.2, 0, 0)
elif ans == 3:
# down
tts.say("Sit Down")
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
postureProxy.goToPosture("Sit", 1.0)
elif ans == 4:
# up
tts.say("Stand Up")
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
postureProxy.goToPosture("Stand", 1.0)
elif ans == 5:
# shaking hand
tts.say("Hi")
elif ans == 6:
# stop
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
posture = postureProxy.getPosture()
if(posture == 'Sit'):
tts.say("I need to stand first")
postureProxy.goToPosture("Stand", 1.0)
tts.say("Moving Back")
motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)
motion.moveInit()
motion.moveTo(-0.2, 0, 0)
elif ans == 7:
# forward
postureProxy = ALProxy("ALRobotPosture", self.nao_ip, self.nao_port)
posture = postureProxy.getPosture()
if(posture == 'Sit'):
tts.say("I need to stand first")
postureProxy.goToPosture("Stand", 1.0)
tts.say("Moving Forward")
motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)
motion.moveInit()
motion.moveTo(0.2, 0, 0)
files = glob.glob('./temp/test_img/*')
for f in files:
os.remove(f)
self.close()
s.close()
if __name__ == '__main__':
IP = "172.16.21.202" # Replace here with your NaoQi's IP address.
PORT = 9559
CameraID = 0
# Read IP address from first argument if any.
if len(sys.argv) > 1:
IP = sys.argv[1]
# Read CameraID from second argument if any.
if len(sys.argv) > 2:
CameraID = int(sys.argv[2])
aware = ALProxy("ALBasicAwareness",IP,PORT)
aware.setEngagementMode("FullyEngaged")
aware.setStimulusDetectionEnabled("Sound",False)
aware.startAwareness()
app = QApplication(sys.argv)
myWidget = ImageWidget(IP, PORT, CameraID)
myWidget.show()
sys.exit(app.exec_())
def stt():
speech_client = speech.Client.from_service_account_json('/home/nao/ourcodes/MyFirstProject-47fa9e048ac2.json')
tts = ALProxy("ALTextToSpeech","localhost",9559)
tts.resetSpeed()
channels = []
channels.append(0)
channels.append(0)
channels.append(1)
channels.append(0)
rec = ALProxy("ALAudioRecorder","localhost",9559)
leds = ALProxy("ALLeds","localhost",9559)
rec.startMicrophonesRecording("/home/nao/ourcodes/test.wav", "wav", 16000, channels)
leds.rotateEyes(0x000000FF,1,5)
rec.stopMicrophonesRecording()
leds.on("FaceLeds")
with open("/home/nao/ourcodes/test.wav", 'rb') as audio_file:
content = audio_file.read()
audio_sample = speech_client.sample(
content=content,
source_uri=None,
encoding='LINEAR16',
sample_rate=16000)
try:
alternatives = speech_client.speech_api.sync_recognize(audio_sample,language_code='en-IN')
return (str(alternatives[0].transcript))
except ValueError:
return ""
def mainModule(IP, PORT):
"""
First get an image from Nao, then show it on the screen with PIL.
:param IP:
:param PORT:
"""
myBroker = ALBroker("myBroker",
"0.0.0.0", # listen to anyone
0, # find a free port and use it
IP, # parent broker IP
PORT) # parent broker port
camProxy = ALProxy("ALVideoDevice", IP, PORT)
cameraIndex=1
resolution = 2 # VGA
colorSpace = 11 # RGB
#camProxy.setActiveCamera("python_client", cameraIndex)
videoClient = camProxy.subscribeCamera("python_client", cameraIndex, resolution, colorSpace, 30)
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = camProxy.getImageRemote(videoClient)
camProxy.unsubscribe(videoClient)
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
#Create a PIL Image Instance from our pixel array.
img0= Image.frombytes("RGB", (imageWidth, imageHeight), array)
#Convert to PIL image
original=shapeDetectionModule.convert2pil(img0)
#convert to numpy array
frame=np.asarray(original[:,:])
#frame = shapeDetectionModule.visualize(img0, shapeDetectionModule.dict)
#shapeDetectionModule.showImage('contour', frame)
dict={}
dict=shapeDetectionModule.getDetectedShapesCenters(img0,shapeDetectionModule.dict)
#array of colors
colors=["red","green","blue","yellow"]
#array of shapes
shapes = ["triangle","pentagon","square","circle"]
#iterate over all colors and show contours of shape of corresponding color
for color in colors:
for shape in shapes:
centers=dict[color][shape]
print (color+" "+shape+" ",centers)
if len(centers)!=0:
for center in centers:
cv2.circle(frame, (int(center[0]),int(center[1])), 3, (255, 0, 0), -1)
text=str(int(center[0]))+" , "+str(int(center[1]))
cv2.putText(frame,text, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.5, (255,255,255),1)
# cv2.circle(frame, center, radius, (0, 255, 0), 1)
shapeDetectionModule.showImage('contour', frame)
cv2.imwrite(path+ "/recognizedImage.jpg", frame)
#TODO
#change the rectangle with min area to just a rectangle:
#
#
# def labelDetectedObject(cnt, frame, text):
# #bounding rectangle
# x,y,w,h = cv2.boundingRect(cnt)
# cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),2)
# center = (int(x+w/2),int(y+h/2))
# centerCoords="( "+str(int(center[0]))+" , "+str(int(center[1]))+" )"
# cv2.putText(frame,text, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.5, (255,255,255),1)
# cv2.putText(frame,centerCoords, (int(center[0]), int(center[1]+10)), cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.4, (0,255,255),1)
# #put a circle in the center
cv2.circle(frame,center,3,(0, 0, 255),-1,8,0)
import math
import os
import cv2
import numpy as np
from lxml import etree
from naoqi import ALProxy
if __name__ == "__main__":
body_names = ['RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw',
'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw']
_motionProxy = None
try:
_motionProxy = ALProxy('ALMotion', '127.0.0.1', 9559)
except Exception, e:
print 'Could not create proxy to ALMotion'
print 'Error was: ', e
# joint_limits = []
# for n in body_names:
# joint_limits.append(_motionProxy.getLimits(n)[0])
joint_limits = [[-119.500006171, 119.500006171], [-75.9999998382, 18.0000005009], [-119.500006171, 119.500006171],
[1.99999998451, 88.4999973401], [-104.500002339, 104.500002339], [-119.500006171, 119.500006171],
[-18.0000005009, 75.9999998382], [-119.500006171, 119.500006171], [-88.4999973401, -1.99999998451],
[-104.500002339, 104.500002339]]
src_img = cv2.imread('P2.jpg')
dst_img = cv2.cvtColor(src_img, cv2.COLOR_RGB2HSV)
def main(robotIP, port):
# Choregraphe simplified export in Python.
names = list()
times = list()
keys = list()
names.append("LElbowRoll")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
-0.454363, -0.452525, -0.45045, -0.443249, -0.443249, -0.443249,
-0.443249, -1.25321
])
names.append("LElbowYaw")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
-0.498749, -0.504517, -0.504517, -0.50399, -0.50399, -0.50399,
-0.50399, -0.513369
])
names.append("LHand")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
0.990117, 0.990117, 0.990117, 0.990117, 0.990117, 0.990117, 0.990117,
0.296697
])
names.append("LShoulderPitch")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
-0.885062, -0.876902, -0.867317, -0.856969, -0.867144, -0.856969,
-0.867144, 0.93291
])
names.append("LShoulderRoll")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
0.8957, -0.205949, 1.2767, 0.0240161, 1.29769, 0.0240161, 1.29769,
0.288381
])
names.append("LWristYaw")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
-1.82287, -1.81429, -1.81429, -1.81429, -1.81429, -1.81429, -1.81429,
0.0280378
])
names.append("RElbowRoll")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
0.454363, 0.457746, 0.454211, 0.443548, 0.45369, 0.443548, 0.45369,
1.20817
])
names.append("RElbowYaw")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
0.434748, 0.436193, 0.436193, 0.43359, 0.43359, 0.43359, 0.43359,
0.447153
])
names.append("RHand")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
0.990117, 0.990117, 0.990117, 0.990117, 0.990117, 0.990117, 0.990117,
0.292421
])
names.append("RShoulderPitch")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
-0.885062, -0.877171, -0.867202, -0.857236, -0.857236, -0.857236,
-0.857236, 0.873036
])
names.append("RShoulderRoll")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
-0.8957, 0.205949, -1.2767, -0.024598, -1.29769, -0.024598, -1.29769,
-0.247769
])
names.append("RWristYaw")
times.append([0.56, 1.04, 1.52, 2, 2.48, 2.96, 3.48, 4])
keys.append([
1.82287, 1.81425, 1.81425, 1.81425, 1.81425, 1.81425, 1.81425,
-0.036702
])
try:
# uncomment the following line and modify the IP if you use this script outside Choregraphe.
# motion = ALProxy("ALMotion", IP, 9559)
motion = ALProxy("ALMotion", robotIP, port)
motion.angleInterpolation(names, keys, times, True)
except BaseException, err:
print err
1.4835298641951802, 0.061086523819801536, 1.4172073526193958,
1.6022122533307945, 0.767944870877505, 0.12
]
GgolfPositionJointAnglesR4 = [
1.03549, 0.314159, 1.66742, 0.971064, -0.980268, 0.12
]
GgolfPositionJointAnglesR5 = [
1.4835298641951802, 0.061086523819801536, 1.4172073526193958,
1.6022122533307945, 0.038397243543875255, 0.6
]
GgolfPositionJointAnglesR6 = [
1.4835298641951802, 0.061086523819801536, 1.4172073526193958,
1.6022122533307945, 0.038397243543875255, 0.04
]
memoryProxy = ALProxy("ALMemory", robotIP, PORT) # memory object
motionPrx = ALProxy("ALMotion", robotIP, PORT) # motion object
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
redballProxy = ALProxy("ALRedBallDetection", robotIP, PORT)
camProxy = ALProxy("ALVideoDevice", robotIP, PORT)
# ------------------------------------------------------------------------------------------------------------#
StiffnessOn(motionPrx)
postureProxy.goToPosture("StandInit", 1.0)
alpha = -math.pi / 2 # valore iniziale
primoColpo()
time.sleep(2)
TurnAfterHitball1()
effectornamelist = ["RWristYaw"]
timelist = [0.7]
targetlist = [65 * math.pi / 180.0]
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to set LArm Position, using a fraction of max speed
chainName = "LArm"
frame = motion.FRAME_TORSO
useSensor = False
# Get the current position of the chainName in the same frame
current = motionProxy.getPosition(chainName, frame, useSensor)
target = [
current[0] + 0.05, current[1] + 0.05, current[2] + 0.05,
current[3] + 0.0, current[4] + 0.0, current[5] + 0.0
]
fractionMaxSpeed = 0.5
axisMask = 7 # just control position
motionProxy.setPositions(chainName, frame, target, fractionMaxSpeed,
axisMask)
time.sleep(1.0)
# Example showing how to set Torso Position, using a fraction of max speed
chainName = "Torso"
frame = motion.FRAME_ROBOT
position = [0.0, 0.0, 0.25, 0.0, 0.0, 0.0] # Absolute Position
fractionMaxSpeed = 0.2
axisMask = 63
motionProxy.setPositions(chainName, frame, position, fractionMaxSpeed,
axisMask)
time.sleep(4.0)
# Go to rest position
motionProxy.rest()
#initial location in cell (1, 1)
#x, y, z = CELL_DIMENSION * MAZE_WIDTH_CELLS * 0.03 * median_perception(landmarkProxy) + CELL_DIMENSION * MAZE_WIDTH_CELLS
x, y, z = median_perception(landmarkProxy)
for i in range(0, len(x), 2):
plotRobotMove(x[i], y[i],0,label='y+')
plotRobotMove(np.median(x), np.median(y), theta=0, label='gx')
return np.median(x), np.median(y), np.median(z)
#sim_plotter()
# Init proxies.
awarenessProxy = ALProxy("ALBasicAwareness", ROBOT_IP, PORT)
motionProxy = ALProxy("ALMotion", ROBOT_IP, PORT)
postureProxy = ALProxy("ALRobotPosture", ROBOT_IP, PORT)
faceProxy = ALProxy("ALFaceDetection", ROBOT_IP, PORT)
speakProxy = ALProxy("ALTextToSpeech", ROBOT_IP, PORT)
memoryProxy = ALProxy("ALMemory", ROBOT_IP, PORT)
landmarkProxy = ALProxy("ALLandMarkDetection", ROBOT_IP, PORT)
sonarProxy = ALProxy("ALSonar", ROBOT_IP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Disable autonomous features
faceProxy.enableTracking(False)
awarenessProxy.setStimulusDetectionEnabled("People", False)
awarenessProxy.setStimulusDetectionEnabled("Movement", False)
keys.append([[0.470523, [3, -0.144444, 0], [3, 1.18889, 0]], [0.0918047, [3, -1.18889, 0], [3, 0, 0]]])
names.append("RShoulderPitch")
times.append([0.166667, 0.6, 0.833333, 1.06667, 1.36667, 1.7, 1.9, 1.96667, 2.3, 2.56667, 2.8, 3.06667, 3.4, 4])
keys.append([[0.889762, [3, -0.0555556, 0], [3, 0.144444, 0]], [0.868286, [3, -0.144444, 0], [3, 0.0777778, 0]], [0.92351, [3, -0.0777778, -0.0112495], [3, 0.0777778, 0.0112495]], [0.935783, [3, -0.0777778, 0], [3, 0.1, 0]], [0.89283, [3, -0.1, 0.00552293], [3, 0.111111, -0.00613659]], [0.886694, [3, -0.111111, 0], [3, 0.0666667, 0]], [0.89283, [3, -0.0666667, 0], [3, 0.0222222, 0]], [0.886694, [3, -0.0222222, 0], [3, 0.111111, 0]], [0.89283, [3, -0.111111, 0], [3, 0.0888889, 0]], [0.886694, [3, -0.0888889, 0], [3, 0.0777778, 0]], [0.89283, [3, -0.0777778, 0], [3, 0.0888889, 0]], [0.886694, [3, -0.0888889, 0], [3, 0.111111, 0]], [0.89283, [3, -0.111111, -0.00613659], [3, 0.2, 0.0110459]], [1.55681, [3, -0.2, 0], [3, 0, 0]]])
names.append("RShoulderRoll")
times.append([0.166667, 0.6, 0.833333, 1.06667, 1.36667, 1.7, 1.9, 1.96667, 2.3, 2.56667, 2.8, 3.06667, 3.4, 4])
keys.append([[-0.181053, [3, -0.0555556, 0], [3, 0.144444, 0]], [-0.101286, [3, -0.144444, -0.0222685], [3, 0.0777778, 0.0119907]], [-0.0782759, [3, -0.0777778, 0], [3, 0.0777778, 0]], [-0.16418, [3, -0.0777778, 0], [3, 0.1, 0]], [-0.075208, [3, -0.1, 0], [3, 0.111111, 0]], [-0.104354, [3, -0.111111, 0], [3, 0.0666667, 0]], [-0.075208, [3, -0.0666667, 0], [3, 0.0222222, 0]], [-0.104354, [3, -0.0222222, 0], [3, 0.111111, 0]], [-0.075208, [3, -0.111111, 0], [3, 0.0888889, 0]], [-0.104354, [3, -0.0888889, 0], [3, 0.0777778, 0]], [-0.075208, [3, -0.0777778, 0], [3, 0.0888889, 0]], [-0.104354, [3, -0.0888889, 0], [3, 0.111111, 0]], [-0.075208, [3, -0.111111, 0], [3, 0.2, 0]], [-0.356235, [3, -0.2, 0], [3, 0, 0]]])
names.append("RWristYaw")
times.append([0.166667, 0.6, 4])
keys.append([[0.96331, [3, -0.0555556, 0], [3, 0.144444, 0]], [0.682588, [3, -0.144444, 0], [3, 1.13333, 0]], [0.964264, [3, -1.13333, 0], [3, 0, 0]]])
try:
motion = ALProxy("ALMotion",robotIP, port)
motion.angleInterpolationBezier(names, times, keys)
posture = ALProxy("ALRobotPosture", robotIP, port)
#posture.goToPosture("StandInit", 1.0)
except BaseException, err:
print err
if __name__ == "__main__":
robotIP = "127.0.0.1"
port = 9559
if len(sys.argv) <= 1:
print "(robotIP default: 127.0.0.1)"
elif len(sys.argv) <= 2:
robotIP = sys.argv[1]
class ALVisualCompass(object):
def __init__(self):
self.proxy = None
def force_connect(self):
self.proxy = ALProxy("ALVisualCompass")
def enableReferenceRefresh(self, refresh):
"""
:param bool refresh: True if the reference is automatically refreshed at extractor startup; false to use the manually set reference image.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.enableReferenceRefresh(refresh)
def getActiveCamera(self):
"""Gets extractor active camera
:returns int: Id of the current active camera of the extractor
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getActiveCamera()
def getCurrentPeriod(self):
"""Gets the current period.
:returns int: Refresh period (in milliseconds).
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getCurrentPeriod()
def getCurrentPrecision(self):
"""Gets the current precision.
:returns float: Precision of the extractor.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getCurrentPrecision()
def getEventList(self):
"""Get the list of events updated in ALMemory.
:returns std::vector<std::string>: Array of events updated by this extractor in ALMemory
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getEventList()
def getFrameRate(self):
"""Gets extractor framerate
:returns int: Current value of the framerate of the extractor
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getFrameRate()
def getMatchingQuality(self):
"""Returns the reliability of the matching and the compass deviation computations. [1]: Number of keypoints matching.
:returns AL::ALValue: [0]: Percentage of the matched keypoints that are used to compute the deviation (significant if over 50%)
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getMatchingQuality()
def getMemoryKeyList(self):
"""Get the list of events updated in ALMemory.
:returns std::vector<std::string>: Array of events updated by this extractor in ALMemory
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getMemoryKeyList()
def getMyPeriod(self, name):
"""Gets the period for a specific subscription.
:param str name: Name of the module which has subscribed.
:returns int: Refresh period (in milliseconds).
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getMyPeriod(name)
def getMyPrecision(self, name):
"""Gets the precision for a specific subscription.
:param str name: name of the module which has subscribed
:returns float: precision of the extractor
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getMyPrecision(name)
def getOutputNames(self):
"""Get the list of values updated in ALMemory.
:returns std::vector<std::string>: Array of values updated by this extractor in ALMemory
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getOutputNames()
def getReferenceImage(self):
"""Returns an ALValue containing the image used as a reference.
:returns AL::ALValue: Reference image (formatted as the ALValue from getImageRemote of ALVideoDevice)
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getReferenceImage()
def getResolution(self):
"""Gets extractor resolution
:returns int: Current value of the resolution of the extractor
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getResolution()
def getSubscribersInfo(self):
"""Gets the parameters given by the module.
:returns AL::ALValue: Array of names and parameters of all subscribers.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.getSubscribersInfo()
def isPaused(self):
"""Gets extractor pause status
:returns bool: True if the extractor is paused, False if not
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.isPaused()
def isProcessing(self):
"""Gets extractor running status
:returns bool: True if the extractor is currently processing images, False if not
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.isProcessing()
def moveStraightTo(self, x):
"""Move along the robot X axis.
:param float x: Algebric distance along the X axis in meters.
:returns bool:
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.moveStraightTo(x)
def moveTo(self, x, y, theta):
"""Go to input pose (in robot referential).
:param float x: Distance along the X axis in meters.
:param float y: Distance along the Y axis in meters.
:param float theta: Rotation around the Z axis in radians [-3.1415 to 3.1415].
:returns bool:
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.moveTo(x, y, theta)
def pause(self, paused):
"""Changes the pause status of the extractor
:param bool paused: New pause satus
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.pause(paused)
def ping(self):
"""Just a ping. Always returns true
:returns bool: returns true
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.ping()
def setActiveCamera(self, cameraId):
"""Sets extractor active camera
:param int cameraId: Id of the camera that will become the active camera
:returns bool: True if the update succeeded, False if not
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.setActiveCamera(cameraId)
def setCurrentImageAsReference(self):
"""Sets the reference image for the compass.
:returns bool: True if the reference image has been successfully set
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.setCurrentImageAsReference()
def setFrameRate(self, subscriberName, framerate):
"""Sets the extractor framerate for a chosen subscriber
:param str subscriberName: Name of the subcriber
:param int framerate: New framerate
:returns bool: True if the update succeeded, False if not
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.setFrameRate(subscriberName, framerate)
def setFrameRate2(self, framerate):
"""Sets the extractor framerate for all the subscribers
:param int framerate: New framerate
:returns bool: True if the update succeeded, False if not
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.setFrameRate(framerate)
def setParameter(self, paramName, value):
"""DEPRECATED: Sets pause and resolution
:param str paramName: Name of the parameter to set
:param AL::ALValue value: New value
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.setParameter(paramName, value)
def setResolution(self, resolution):
"""Sets extractor resolution
:param int resolution: New resolution
:returns bool: True if the update succeeded, False if not
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.setResolution(resolution)
def subscribe(self, name, period, precision):
"""Subscribes to the extractor. This causes the extractor to start writing information to memory using the keys described by getOutputNames(). These can be accessed in memory using ALMemory.getData("keyName"). In many cases you can avoid calling subscribe on the extractor by just calling ALMemory.subscribeToEvent() supplying a callback method. This will automatically subscribe to the extractor for you.
:param str name: Name of the module which subscribes.
:param int period: Refresh period (in milliseconds) if relevant.
:param float precision: Precision of the extractor if relevant.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.subscribe(name, period, precision)
def subscribe2(self, name):
"""Subscribes to the extractor. This causes the extractor to start writing information to memory using the keys described by getOutputNames(). These can be accessed in memory using ALMemory.getData("keyName"). In many cases you can avoid calling subscribe on the extractor by just calling ALMemory.subscribeToEvent() supplying a callback method. This will automatically subscribe to the extractor for you.
:param str name: Name of the module which subscribes.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.subscribe(name)
def unsubscribe(self, name):
"""Unsubscribes from the extractor.
:param str name: Name of the module which had subscribed.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.unsubscribe(name)
def updatePeriod(self, name, period):
"""Updates the period if relevant.
:param str name: Name of the module which has subscribed.
:param int period: Refresh period (in milliseconds).
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.updatePeriod(name, period)
def updatePrecision(self, name, precision):
"""Updates the precision if relevant.
:param str name: Name of the module which has subscribed.
:param float precision: Precision of the extractor.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.updatePrecision(name, precision)
def version(self):
"""Returns the version of the module.
:returns str: A string containing the version of the module.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.version()
def waitUntilTargetReached(self):
"""Block the current thread until the target is reached.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.waitUntilTargetReached()
names.append("LShoulderPitch")
times.append([0.4, 0.6, 1.04, 1.44, 1.6, 1.88, 2.12, 2.36, 2.56, 2.76, 3.6])
keys.append([
1.56926, 1.24252, -0.254641, -1.07379, -1.07379, -0.958186, -1.11527,
-0.853466, -0.825541, -1.08452, 1.48178
])
names.append("LShoulderRoll")
times.append([0.4, 0.6, 1.04, 1.44, 1.6, 1.88, 2.36, 2.56, 2.76, 3.6])
keys.append([
0.15033, 0.18675, 0.213223, 0.222427, 0.222427, 0.18101, 0.18101, 0.18101,
0.18101, 0.205949
])
names.append("LWristYaw")
times.append([0.4, 0.6, 1.04, 1.44, 1.6, 1.88, 2.36, 2.56, 2.76, 3.6])
keys.append([
-0.116542, 0.898966, -0.207048, -1.10751, -1.10751, -0.954106, -0.954107,
-0.954107, -0.954107, -0.125664
])
try:
# uncomment the following line and modify the IP if you use this script outside Choregraphe.
motion = ALProxy("ALMotion", IP, 9559)
motion.setExternalCollisionProtectionEnabled("Arms", False)
# motion = ALProxy("ALMotion")
motion.angleInterpolation(names, keys, times, True)
except BaseException, err:
print err
def waitUntilTargetReached(self):
"""Block the current thread until the target is reached.
"""
if not self.proxy:
self.proxy = ALProxy("ALVisualCompass")
return self.proxy.waitUntilTargetReached()
import time
from naoqi import ALProxy
from os.path import expanduser
import os
import sys
from os.path import expanduser
import os
import time
ROBOT_IP = "169.254.126.202"
CHANNELS = [0, 0, 1, 0]
ar = ALProxy("ALAudioRecorder", ROBOT_IP, 9559)
ap = ALProxy("ALAudioPlayer", ROBOT_IP, 9559)
def record_sound():
AUDIO_FILE = '/home/nao/test.wav'
ar.startMicrophonesRecording(AUDIO_FILE, "wav", 16000, CHANNELS)
print "Audio recorder started recording"
time.sleep(10)
ar.stopMicrophonesRecording()
print "Audio recorder stopped recording"
fileId = ap.post.playFile(AUDIO_FILE)
if __name__ == "__main__":
record_sound()
