Programming Robots using TiViPE

TiViPEVisual ProgrammingProgramming Robots using TiViPE–A step by step approach using Aldebaran’s NAO robotsTechnical Report: Draft version 0.2.0Copyright c○TiViPE 2010-2011. All rights reserved.Tino LourensTiViPEKanaaldijk ZW 115706 LD HelmondThe Netherlandstino@tivipe.comSeptember 5, 2011

CONTENTS 2Contents1 Introduction 42 Setup of NAO robots 43 Running ready robot components 53.1 Running the video module . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53.2 Hello world in Robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.3 A bit of Tai Chi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.4 Textual language and available commands . . . . . . . . . . . . . . . . . . . . . . . . 113.4.1 LEDS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.4.2 Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.5 Enable the sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.6 Available sensor commands . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.6.1 Using states . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214 Programming new modules 234.1 Setting up a C++ library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2 Adding content to the library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.3 Embedding a routine call into TiViPE . . . . . . . . . . . . . . . . . . . . . . . . . . 245 Robot Programming 265.1 Programming a state space routine . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265.2 HeadMenu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275.3 Object detection and tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285.4 Standing up and sitting down . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295.5 Nao demonstration 2011 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 306 Exercises 326.1 Exercise 1: Replace walka by walkto command . . . . . . . . . . . . . . . . . . . . . 326.2 Exercise 2: Calibration of walk, walks, and walka . . . . . . . . . . . . . . . . . . . . 326.3 Exercise 3: Create a more advanced face tracking module . . . . . . . . . . . . . . . . 326.4 Exercise 4: Flexible state activation . . . . . . . . . . . . . . . . . . . . . . . . . . . 326.5 Exercise 5: Getting grip over active states . . . . . . . . . . . . . . . . . . . . . . . . 336.6 Advanced Exercise 6: Auditory processing . . . . . . . . . . . . . . . . . . . . . . . . 336.7 Exercise 7: User scenario construction . . . . . . . . . . . . . . . . . . . . . . . . . . 337 Advanced Robot Programming 337.1 Adding commands to the NAO robot . . . . . . . . . . . . . . . . . . . . . . . . . . . 337.2 Adding sensing elements to the NAO robot . . . . . . . . . . . . . . . . . . . . . . . 337.3 Enabling other features on the NAO robot . . . . . . . . . . . . . . . . . . . . . . . . 337.4 Adding other robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33A TVProbotics source code 34A.1 Project file src.pro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34A.2 Source code of TVProboticsdefs.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36A.3 Source code of TVPnaocommand.h . . . . . . . . . . . . . . . . . . . . . . . . . . . 37A.4 Source code of TVPnaocommand.cpp . . . . . . . . . . . . . . . . . . . . . . . . . . 44

CONTENTS 3AbstractThe aim of TiViPE is to integrate different technologies in a seamless way using graphicalicons [1]. Due to these icons the user does not need to have in depth knowledge of the underlyinghardware architecture.This report elaborates on controlling robots, NAO robots in particular, with the TiViPE environment.The report can be used as a step by step manual, starting to setup and use the software,get acquainted, with the textual robot command language [2], use of ready TiViPE modules tocontrol the robot, and will be provided substantial information on how to adapt, extend, and tocreate modules within TiViPE.

1 Introduction 41 IntroductionThe aim of TiViPE is to integrate different technologies in a seamless way using graphical icons [1].Due to these icons the user does not need to have in depth knowledge of the underlying hardwarearchitecture.This report is outlined as follows: Section 2 provides an introduction of setting up software forNAO robots using TiViPE. Section 3 provides some examples how to create a robot program, and setparameters to gain full control over the robot using ready TiViPE modules. Section 4 elaborates onhow to construct new modules by explaining how to set up a new project for a library, and how towrite new routines, useful for robot interaction. Section 5 provides insight in how to construct roboticsapplications by means of examples. Useful and necessary exercises are provided in Section 6. The lastsection provides some ideas on how to further extend the TVProbotics library.2 Setup of NAO robotsThe NAO software can either be installed from the provided CDROM or by logging in to Aldebaran’swebsite. The version used by TiViPE is 1.10.52, and is assumed to be available on your local computer.The setup of TiViPE starts by downloading the windows installation program from www.tivipe.com.The windows installation program asks for the path where the NAO software has been installed. Inits subdirectories bin, include, lib, and other directories are found. In case of a unix operatingsystem the NAO path is provided in the $TVPHOME/vars file. Likewise on the windows OS its pathcan be found in %TVPHOME%\vars.bat and in case the path needs to be adjusted, it can be eitherby editing the file or using the installer a second time.TiViPE provides a small package called naohome.tar, it contains files for grabbing imagesfrom the camera, and a socket server, to send data to your local machine. First connect the robot tothe internet and switch it on, after the robot has started, press its chest button briefly to obtain thenao-ip-address. Install naohome.tar as follows:%TVPHOME%\vars.batpscp naohome.tar nao@[nao-ip-address]:plink nao@[nao-ip-address]tar xvf naohome.tarexitThe first time of logging on to the nao robot a key is generated, type y to accept this key. Tryplink -l nao -pw nao [nao-ip-address]Your local machine should be granted access to the nao robot and be logged in. Logout by typingexitStart up the TiViPE environment and select File - Edit remote machines, and fill out arow for every NAO robot, as demonstrated by example in Figure 1 for a windows configuration. For adetailed explanation on setting up remote processes press the Help button in this window.From the command line one should test, if one can login on to the remote machine by typing firstthe shell command followed by the host name. Once access is granted type exit to logout.

3 Running ready robot components 5Figure 1: Editing hostnames.Figure 2: Editing hostnames.3 Running ready robot componentsIn this section we introduce step by step the available robot modules, and make the user familiar withboth TiViPE and available robot modules.3.1 Running the video moduleThe installed files from naohome.tar contains a module called FrameGrabber. As a first exercise,please construct the network given in Figure 2.By right clicking the modules toggle from gray, yellow, to red. Red denotes that it is a permanentand streaming process. Between these red colored modules a connection makes use of shared memorywhen the modules run on the local machine. The FrameGrabber module has to be adjusted since itshould run on the robot, hence change the left lower button to the desired IP address of the robot, andchange the output connection to a socket, which is indicated by a green color at the upper part of theoutput connection. By right clicking on the connection the color changes, red denotes file or sharedmemory, blue denotes socket and file output, and green denotes a socket, the installed socket serverswill take care of the communication between two modules.

3.2 Hello world in Robotics 6Try to start the USBgrabber on the local machine, this will start grabbing images from yourlocal machine, and function only if there is a camera installed. There is multiple camera support, andwhen more than one cameras are available on the system, select the upper right square in the module,and change the device from 0 to 1 (or 2, 3, ... depending on the number of camera’s installed) in thewindow that has popped up.Next try to start FrameGrabber, and try to change the settings, by clicking on the right upper(parameter) button in the FrameGrabber module. In case you have an NVIDIA CUDA capablevideo card, and have enabled this during the TiViPE installation, you can connect the GPU display tothe Framegrabber module, to make optimal use out of all the available hardware.The FrameGrabber example provides full access to the video system of the robot and can becombined with one or more of the available image processing modules available within TiViPE.3.2 Hello world in RoboticsAbout two decades ago programming hello world in C was as follows:#includemain (){printf (‘‘Hello world!\n’’);}Saving the text above to hello.c and depending on the used compiler typing gcc hello.c orcc hello.c on the command prompt resulted in a program that in turn could be executed by abinary called a.out. Later different more sophisticated hello world programs have become popular,for instance the one provided by Qt, that demonstrates a window with a waving hello flag.Within TiViPE we would like to do a hello world program as well. It is performed in two steps:1. lie the robot on its back, and let it get up2. wave with hand and say helloThis program is illustrated in Figure 3, and should be self explanatory. CommandNaoStandUpFromBackproduces a string that performs the first step. CommandString that generates a string “say(Hello,50)” which means that the robot will produce the word ’Hello’ at a volume of 50 percent using a textto speech engine. CommandNaoHello1 perform hand waving. CommandParellel constructs astring where latter two commands are executed at the same time, while CommandSerial ensuresthat the robot will first get up before it starts talking and waving.3.3 A bit of Tai ChiLike the handwaving module, Aldebaran robotics has been providing a Tai-Chi module, within TiViPEthis module has been reproduced to yield a very similar behavior. When the robot is standing, theCommandNaoTaiChi is connected to the NaoRobot module, as illustrated in Figure 4. The commandscan be exchanged easily since the commanding robot language is using pure text. In the C orC++ programming language performed by a so called string.Tai chi is a single module (CommandNaoTaiChi), it gives a performance of 50 seconds. This isdone by producing a string that is as follows:

3.3 A bit of Tai Chi 990.0, 1500, 90.0, 1000, 90.0, 1000, 90.0, 1000, 90.0, 1000,11.0, 1000, 20.0, 1000, 90.0, 1000, 24.0, 1000, 24.0, 1000,5.0, 1300, 24.0, 1300, -11.0, 1100, -12.0, 1000, 0.0, 1000,0.0, 900, 0.0, 1300, 0.0, 1300, 0.0, 1300, 0.0, 1100,14.0, 900, 90.0, 2000, 79.8, 2000,RElbowRoll,0.0, 1500, 40.0, 1000, 60.0, 1000, 0.0, 1000, 0.0, 1000,71.0, 1000, 94.0, 1000, 4.0, 1000, 64.0, 1000, 49.0, 1000,72.0, 1300, 49.0, 1300, 49.0, 1100, 51.0, 1000, 58.0, 1000,58.0, 900, 58.0, 1300, 0.0, 1300, 58.0, 1300, 58.0, 1100,92.0, 900, 0.1, 2000, 58.8, 2000,RWristYaw,88.0, 1500, 87.0,25500,RHand,0.0, 1500, 5.5,25500,LHipYawPitch,0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,-5.1, 1000, -5.7, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,0.0, 1300, 0.0, 1300, 0.0, 1100, 0.0, 1000, 0.0, 1000,0.0, 900, 0.0, 1300, 0.0, 1300, 0.0, 1300, -3.9, 1100,0.0, 900, -0.1, 2000, 0.0, 2000,LHipRoll,-3.0, 1500, -7.0, 1000, -10.0, 1000, 6.0, 1000, 6.0, 1000,-10.0, 1000, 10.0, 1000, 24.1, 1000, 30.3, 1000, 35.0, 1000,35.0, 1300, 35.0, 1300, 20.0, 1100, 0.0, 1000, -15.0, 1000,11.2, 900, 8.0, 1300, 40.0, 1300, 8.0, 1300, -15.0, 1100,0.0, 900, -2.8, 2000, 0.0, 2000,LHipPitch,0.0, 1500, 0.0, 1000, -20.0, 1000, 0.0, 1000, 0.0, 1000,-40.0, 1000, -35.0, 1000, -60.0, 1000, -60.0, 1000, -60.0, 1000,-60.0, 1300, -60.0, 1300, -60.0, 1100, -50.0, 1000, -42.5, 1000,-7.0, 900, -50.0, 1300, 0.0, 1300, -50.0, 1300, -37.5, 1100,-60.0, 900, -0.2, 2000, -25.0, 2000,LKneePitch,0.0, 1500, 0.0, 1000, 40.0, 1000, 0.0, 1000, 0.0, 1000,80.0, 1000, 70.0, 1000, 120.0, 1000, 120.0, 1000, 120.0, 1000,120.0, 1300, 120.0, 1300, 120.9, 1100, 100.0, 1000, 85.0, 1000,7.0, 900, 100.0, 1300, 0.0, 1300, 100.0, 1300, 75.0, 1100,120.0, 900, 0.2, 2000, 40.1, 2000,LAnklePitch,0.0, 1500, 0.0, 1000, -20.0, 1000, 0.0, 1000, 0.0, 1000,-40.0, 1000, -35.0, 1000, -60.0, 1000, -60.0, 1000, -60.0, 1000,-60.0, 1300, -60.0, 1300, -60.0, 1100, -50.0, 1000, -42.5, 1000,0.0, 900, 0.0, 1300, 30.0, 1300, 0.0, 1300, -37.5, 1100,-60.0, 900, -0.2, 2000, -20.1, 2000,LAnkleRoll,3.0, 1500, 7.0, 1000, 10.0, 1000, -6.0, 1000, -6.0, 1000,10.0, 1000, -10.0, 1000, 10.0, 1000, 10.0, 1000, 5.0, 1000,5.0, 1300, 5.0, 1300, 10.0, 1100, 0.0, 1000, -15.0, 1000,-44.2, 900, -20.0, 1300, 0.0, 1300, -20.0, 1300, -15.0, 1100,0.0, 900, 2.8, 2000, 0.1, 2000,RHipRoll,3.0, 1500, -7.0, 1000, -25.1, 1000, -6.0, 1000, -6.0, 1000,-20.0, 1000, -45.0, 1000, -31.0, 1000, -8.0, 1000, -8.0, 1000,

3.3 A bit of Tai Chi 10-40.0, 1300, -8.0, 1300, 15.0, 1100, 0.0, 1000, -20.0, 1000,-30.9, 900, -35.0, 1300, -35.0, 1300, -35.0, 1300, -28.4, 1100,0.0, 900, 2.8, 2000, 0.1, 2000,RHipPitch,0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,-40.0, 1000, -10.0, 1000, -6.0, 1000, -7.0, 1000, -50.0, 1000,0.0, 1300, -50.0, 1300, -42.5, 1100, -50.0, 1000, -60.0, 1000,-60.0, 900, -60.0, 1300, -60.0, 1300, -60.0, 1300, -60.0, 1100,-60.0, 900, -0.2, 2000, -25.1, 2000,RKneePitch,0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,80.0, 1000, 20.0, 1000, 7.0, 1000, 7.0, 1000, 100.0, 1000,0.0, 1300, 100.0, 1300, 85.0, 1100, 100.0, 1000, 120.0, 1000,120.0, 900, 120.0, 1300, 120.0, 1300, 120.0, 1300, 120.0, 1100,120.0, 900, 0.2, 2000, 39.9, 2000,RAnklePitch,0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,-40.0, 1000, -10.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,30.0, 1300, 0.0, 1300, -42.5, 1100, -50.0, 1000, -60.0, 1000,-60.0, 900, -60.0, 1300, -60.0, 1300, -60.0, 1300, -60.0, 1100,-60.0, 900, -0.2, 2000, -20.0, 2000,RAnkleRoll,-3.0, 1500, 7.0, 1000, 25.0, 1000, 6.0, 1000, 6.0, 1000,24.5, 1000, 40.0, 1000, 45.0, 1000, 42.6, 1000, 25.0, 1000,0.0, 1300, 20.0, 1300, 15.0, 1100, 0.0, 1000, -10.0, 1000,-10.0, 900, -5.0, 1300, -5.0, 1300, -5.0, 1300, -10.0, 1100,0.0, 900, -2.8, 2000, -0.1, 2000) |play (/opt/naoqi/share/naoqi/mp3/swiftswords_ext.mp3, 75) |[ledoff (AllLeds) & wait (1500) &[[ledfade (LeftFaceLedRed0, 0, 1000) |ledfade (RightFaceLedRed0, 0, 1000) |ledfade (LeftFaceLedRed7, 0, 1000) |ledfade (RightFaceLedRed7, 0, 1000) |ledfade (LeftFaceLedGreen0, 0, 1000) |ledfade (RightFaceLedGreen0, 0, 1000) |ledfade (LeftFaceLedGreen7, 0, 1000) |ledfade (RightFaceLedGreen7, 0, 1000) |ledfade (LeftFaceLedBlue0, 1, 1000) |ledfade (RightFaceLedBlue0, 1, 1000) |ledfade (LeftFaceLedBlue7, 1, 1000) |ledfade (RightFaceLedBlue7, 1, 1000) ] |[ledfade (LeftFaceLedRed1, 0, 1000) |ledfade (RightFaceLedRed5, 0, 1000) |ledfade (LeftFaceLedRed2, 0, 1000) |ledfade (RightFaceLedRed6, 0, 1000) |ledfade (LeftFaceLedGreen1, 1, 1000) |ledfade (RightFaceLedGreen5, 1, 1000) |ledfade (LeftFaceLedGreen2, 1, 1000) |ledfade (RightFaceLedGreen6, 1, 1000) |ledfade (LeftFaceLedBlue1, 0, 1000) |ledfade (RightFaceLedBlue5, 0, 1000) |ledfade (LeftFaceLedBlue2, 0, 1000) |ledfade (RightFaceLedBlue6, 0, 1000) ] |[ledfade (LeftFaceLedRed3, 0, 1000) |

3.4 Textual language and available commands 11ledfade (RightFaceLedRed3, 0, 1000) |ledfade (LeftFaceLedRed4, 0, 1000) |ledfade (RightFaceLedRed4, 0, 1000) |ledfade (LeftFaceLedGreen3, 0, 1000) |ledfade (RightFaceLedGreen3, 0, 1000) |ledfade (LeftFaceLedGreen4, 0, 1000) |ledfade (RightFaceLedGreen4, 0, 1000) |ledfade (LeftFaceLedBlue3, 1, 1000) |ledfade (RightFaceLedBlue3, 1, 1000) |ledfade (LeftFaceLedBlue4, 1, 1000) |ledfade (RightFaceLedBlue4, 1, 1000) ] |[ledfade (LeftFaceLedRed5, 0, 1000) |ledfade (RightFaceLedRed1, 0, 1000) |ledfade (LeftFaceLedRed6, 0, 1000) |ledfade (RightFaceLedRed2, 0, 1000) |ledfade (LeftFaceLedGreen5, 0, 1000) |ledfade (RightFaceLedGreen1, 0, 1000) |ledfade (LeftFaceLedGreen6, 0, 1000) |ledfade (RightFaceLedGreen2, 0, 1000) |ledfade (LeftFaceLedBlue5, 1, 1000) |ledfade (RightFaceLedBlue1, 1, 1000) |ledfade (LeftFaceLedBlue6, 1, 1000) |.... repeat blue and green blinking leds 19 timesledfade (RightFaceLedBlue2, 1, 1000) ] ] &ledfade (RightFaceLedBlue6, 0, 1000) ] &ledfade (FaceLeds, 0, 5000) ]]&speed (100)]The tai chi command consists of a set of move, play, led, and speed commands. In Section 3.4we will elaborate on the available commands and their syntax.3.4 Textual language and available commandsThe aim of constructing a textual robot language is that no in depth knowledge of a robot is required,nor that a specific robot is required. The effectiveness of a generic set of textual commands willhowever depend on the capabilities of such a robot. The goal is that a compact language would berequired where command instructions can be executed in a serial or parallel manner. Our aim is to geta language like adding and multiplying, a concept that is understood from this principle(a + b) ∗ c + d ∗ e (1)knowing that brackets bind stronger than multiplication, and multiplication has higher priority overaddition, we can construct a powerful language that is in essence identical[a | b] & c | d & e (2)where a | b denotes that a and b are executed in parallel, d & e denotes that d and e are executedsubsequently first d then e, the square brackets denote the binding order of execution. Commands a, b,c, d, and e are individual elementary commands that can be executed on a robot.The commands available and functioning on the NAO robot are

3.4 Textual language and available commands 12wait (time)flush ()ledon (ledname)ledoff (ledname)ledset (ledname, val)ledfade (ledname, val, time)say (text[, volume[, language[, voice]]])play (filename[, volume])move (name, angle, duration, [stiffness])movem ([name, [angle, duration]+]+)movemsi ([name, [angle, ty1, a1, t1, ty2, a2, t2, duration]+]+)stiff ([value, duration, idle]+)speed (value)walk (distance, time)walks (distance, time)walkto (x, y, theta)walkd (x, y, theta, freq, duration)These 17 commands are divided into 4 (generic, leds, audio, and motion) distinct groups. Commandwait waits for a given number of milliseconds, and flush flushes commands that have beennot being processed.Leds can be controlled addressing a led by a name that is coupled. The list of these names for theNAO robot is given in Section 3.4.1. These leds have a value between 0 and 1, it implies that ledoff(ledname) is equal to ledset (lename, 0), likewise for ledon with value 1. The ledfadecommands transforms from the current value to the given value, over a time given in milliseconds.The say command is converting text to speech on the robot, in practise the first 2 arguments areused. Text is coupled using underscores, for example:say(My_name_is_Nao)The other audio command play plays mp3 files that are stored on the robot.The attractive part of robots is their mobility, and different commands can be used to provide thismobility. For the NAO robot this starts by enabling the motors by setting its stiffness, 0 for disabledor relaxed, and 1 for enabled or stressed. It is executed by the command stiff, for example stiff(1.0, 1500, 0) denotes that within 1.5 seconds the motors are fully enabled, and stay like thatuntil a new stiff command is given. It is allowed to give multiple triples in a single command.The move and movem are commands for individual control of every motor available, and is givenby a list of names, in Section 3.4.2 these names are specified for the NAO robot. The movem commandis a move multiple command where multiple motors are controlled over time by providing desired angle,and time to move from current angle to desired angle, see also the Tai Chi example given inSection 3.3. Tai chi is played in slow motion using the global speed command. In this example thespeed is set with value 183, and denotes a slow down of 1.83. It implies that every time argument inmovem is multiplied with 1.83. The movemsi command is very similar to movem, but uses splineinterpolation to make movements a bit smoother compared to the movem command. The spline interpolationcontains two triples, every triple starts with an integer value denoting the type of interpolation,and two floating point values a (delta in) angle, and a (delta in) time.

3.4 Textual language and available commands 13Finally there are 4 commands to let the robot walk, and of course this command could have beenperformed by the movem command, but the walk commands are substantially more compact to use.The walkto, command makes a robot move accurately over a distance (x, y) in meters, and rotate therobot torso (in the vertical axis) around -180 to 180 degrees. The walkd command makes a robot walka distance over a given duration in milliseconds, x, y, theta, and frequency are relative values, the first3 are given between -1 and 1 while the frequency is between 0 and 1. In addition to these commandswalk straight walk and walk sideways walks are available, the aim of these two commands is tomove the robot the specified distance over the specified time.3.4.1 LEDSThe LED names available, given in the second column, on the NAO robot areIndex Name Internal description0 AllLeds1 AllLedsBlue2 AllLedsGreen3 AllLedsRed4 ChestLedBlue ChestBoard/Led/Blue/Actuator/Value5 ChestLedGreen ChestBoard/Led/Green/Actuator/Value6 ChestLedRed ChestBoard/Led/Red/Actuator/Value7 ChestLeds8 EarLeds9 FaceLed010 FaceLed111 FaceLed212 FaceLed313 FaceLed414 FaceLed515 FaceLed616 FaceLed717 FaceLedLeft018 FaceLedLeft119 FaceLedLeft220 FaceLedLeft321 FaceLedLeft422 FaceLedLeft523 FaceLedLeft624 FaceLedLeft725 FaceLedRight026 FaceLedRight127 FaceLedRight228 FaceLedRight329 FaceLedRight430 FaceLedRight531 FaceLedRight632 FaceLedRight733 FaceLeds

3.4 Textual language and available commands 1434 FaceLedsBottom35 FaceLedsExternal36 FaceLedsInternal37 FaceLedsLeftBottom38 FaceLedsLeftExternal39 FaceLedsLeftInternal40 FaceLedsLeftTop41 FaceLedsRightBottom42 FaceLedsRightExternal43 FaceLedsRightInternal44 FaceLedsRightTop45 FaceLedsTop46 FeetLeds47 LeftEarLed0 Ears/Led/Left/0Deg/Actuator/Value48 LeftEarLed1 Ears/Led/Left/36Deg/Actuator/Value49 LeftEarLed2 Ears/Led/Left/72Deg/Actuator/Value50 LeftEarLed3 Ears/Led/Left/108Deg/Actuator/Value51 LeftEarLed4 Ears/Led/Left/144Deg/Actuator/Value52 LeftEarLed5 Ears/Led/Left/180Deg/Actuator/Value53 LeftEarLed6 Ears/Led/Left/216Deg/Actuator/Value54 LeftEarLed7 Ears/Led/Left/252Deg/Actuator/Value55 LeftEarLed8 Ears/Led/Left/288Deg/Actuator/Value56 LeftEarLed9 Ears/Led/Left/324Deg/Actuator/Value57 LeftEarLeds58 LeftEarLedsBack59 LeftEarLedsEven60 LeftEarLedsFront61 LeftEarLedsOdd62 LeftFaceLeds63 LeftFaceLedBlue0 Face/Led/Blue/Left/0Deg/Actuator/Value64 LeftFaceLedBlue1 Face/Led/Blue/Left/45Deg/Actuator/Value65 LeftFaceLedBlue2 Face/Led/Blue/Left/90Deg/Actuator/Value66 LeftFaceLedBlue3 Face/Led/Blue/Left/135Deg/Actuator/Value67 LeftFaceLedBlue4 Face/Led/Blue/Left/180Deg/Actuator/Value68 LeftFaceLedBlue5 Face/Led/Blue/Left/225Deg/Actuator/Value69 LeftFaceLedBlue6 Face/Led/Blue/Left/270Deg/Actuator/Value70 LeftFaceLedBlue7 Face/Led/Blue/Left/315Deg/Actuator/Value71 LeftFaceLedGreen0 Face/Led/Green/Left/0Deg/Actuator/Value72 LeftFaceLedGreen1 Face/Led/Green/Left/45Deg/Actuator/Value73 LeftFaceLedGreen2 Face/Led/Green/Left/90Deg/Actuator/Value74 LeftFaceLedGreen3 Face/Led/Green/Left/135Deg/Actuator/Value75 LeftFaceLedGreen4 Face/Led/Green/Left/180Deg/Actuator/Value76 LeftFaceLedGreen5 Face/Led/Green/Left/225Deg/Actuator/Value77 LeftFaceLedGreen6 Face/Led/Green/Left/270Deg/Actuator/Value78 LeftFaceLedGreen7 Face/Led/Green/Left/315Deg/Actuator/Value79 LeftFaceLedRed0 Face/Led/Red/Left/0Deg/Actuator/Value80 LeftFaceLedRed1 Face/Led/Red/Left/45Deg/Actuator/Value81 LeftFaceLedRed2 Face/Led/Red/Left/90Deg/Actuator/Value

3.4 Textual language and available commands 1582 LeftFaceLedRed3 Face/Led/Red/Left/135Deg/Actuator/Value83 LeftFaceLedRed4 Face/Led/Red/Left/180Deg/Actuator/Value84 LeftFaceLedRed5 Face/Led/Red/Left/225Deg/Actuator/Value85 LeftFaceLedRed6 Face/Led/Red/Left/270Deg/Actuator/Value86 LeftFaceLedRed7 Face/Led/Red/Left/315Deg/Actuator/Value87 LeftFaceLedsBlue88 LeftFaceLedsGreen89 LeftFaceLedsRed90 LeftFootLedBlue LFoot/Led/Blue/Actuator/Value91 LeftFootLedGreen LFoot/Led/Green/Actuator/Value92 LeftFootLedRed LFoot/Led/Red/Actuator/Value93 LeftFootLeds94 RightEarLeds95 RightEarLed0 Ears/Led/Right/0Deg/Actuator/Value96 RightEarLed1 Ears/Led/Right/36Deg/Actuator/Value97 RightEarLed2 Ears/Led/Right/72Deg/Actuator/Value98 RightEarLed3 Ears/Led/Right/108Deg/Actuator/Value99 RightEarLed4 Ears/Led/Right/144Deg/Actuator/Value100 RightEarLed5 Ears/Led/Right/180Deg/Actuator/Value101 RightEarLed6 Ears/Led/Right/216Deg/Actuator/Value102 RightEarLed7 Ears/Led/Right/252Deg/Actuator/Value103 RightEarLed8 Ears/Led/Right/288Deg/Actuator/Value104 RightEarLed9 Ears/Led/Right/324Deg/Actuator/Value105 RightEarLedsBack106 RightEarLedsEven107 RightEarLedsFront108 RightEarLedsOdd109 RightFaceLedBlue0 Face/Led/Blue/Right/0Deg/Actuator/Value110 RightFaceLedBlue1 Face/Led/Blue/Right/45Deg/Actuator/Value111 RightFaceLedBlue2 Face/Led/Blue/Right/90Deg/Actuator/Value112 RightFaceLedBlue3 Face/Led/Blue/Right/135Deg/Actuator/Value113 RightFaceLedBlue4 Face/Led/Blue/Right/180Deg/Actuator/Value114 RightFaceLedBlue5 Face/Led/Blue/Right/225Deg/Actuator/Value115 RightFaceLedBlue6 Face/Led/Blue/Right/270Deg/Actuator/Value116 RightFaceLedBlue7 Face/Led/Blue/Right/315Deg/Actuator/Value117 RightFaceLedGreen0 Face/Led/Green/Right/0Deg/Actuator/Value118 RightFaceLedGreen1 Face/Led/Green/Right/45Deg/Actuator/Value119 RightFaceLedGreen2 Face/Led/Green/Right/90Deg/Actuator/Value120 RightFaceLedGreen3 Face/Led/Green/Right/135Deg/Actuator/Value121 RightFaceLedGreen4 Face/Led/Green/Right/180Deg/Actuator/Value122 RightFaceLedGreen5 Face/Led/Green/Right/225Deg/Actuator/Value123 RightFaceLedGreen6 Face/Led/Green/Right/270Deg/Actuator/Value124 RightFaceLedGreen7 Face/Led/Green/Right/315Deg/Actuator/Value125 RightFaceLedRed0 Face/Led/Red/Right/0Deg/Actuator/Value126 RightFaceLedRed1 Face/Led/Red/Right/45Deg/Actuator/Value127 RightFaceLedRed2 Face/Led/Red/Right/90Deg/Actuator/Value128 RightFaceLedRed3 Face/Led/Red/Right/135Deg/Actuator/Value129 RightFaceLedRed4 Face/Led/Red/Right/180Deg/Actuator/Value

3.5 Enable the sensors 16130 RightFaceLedRed5 Face/Led/Red/Right/225Deg/Actuator/Value131 RightFaceLedRed6 Face/Led/Red/Right/270Deg/Actuator/Value132 RightFaceLedRed7 Face/Led/Red/Right/315Deg/Actuator/Value133 RightFaceLeds134 RightFaceLedsBlue135 RightFaceLedsGreen136 RightFaceLedsRed137 RightFootLedBlue RFoot/Led/Blue/Actuator/Value138 RightFootLedGreen RFoot/Led/Green/Actuator/Value139 RightFootLedRed RFoot/Led/Red/Actuator/Value140 RightFootLeds3.4.2 MotorsThe motor names available on the NAO robot areMotor name Minimum Angle Maximum Angle Maximum Change Per Cycle (deg)HeadYaw -120 120 9.47HeadPitch -45 45 8.24LShoulderPitch -120 120 9.47LShoulderRoll 0 95 8.24LElbowYaw -120 120 9.47LElbowRoll -95 0 8.24LWristYaw -105 150 8.24LHand 0 57.2958 8.24LHipYawPitch -55 40 4.76LHipRoll -25 45 4.76LHipPitch -90 30 7.33LKneePitch 0 130 7.33LAnklePitch -70 45 7.33LAnkleRoll -45 45 4.76RHipYawPitch -55 40 4.76RHipRoll -45 25 4.76RHipPitch -90 30 7.33RKneePitch 0 130 7.33RAnklePitch -70 45 7.33RAnkleRoll -45 45 4.76RShoulderPitch -120 120 9.47RShoulderRoll -95 0 8.24RElbowYaw -120 120 9.47RElbowRoll 0 95 8.24RWristYaw -105 150 8.24RHand 0 57.2958 8.243.5 Enable the sensorsBesides the camera available on the robot a number of sensing values can be obtained from the robot.This is obtained by the NaoSense module (Figure 5), it reads out all single valued sensory data.

3.5 Enable the sensors 17Figure 5: Sensing internal data.The added CommandNaoBySensing module servers as an example, how to create a command thatdepends on sensory data.The code for enabling sensory data is given below, for the Left foot bumper sensor, it the sameway all other sensors can be added, for a full list of the available sensors, see Section 3.6.In a c++ class define/*** List of sensory elements*/TVPnaoSensors *ns;/*** index for left foot bummper at left side*/int LFootBumperLeft;In the constructor use/********************************************************* Setup list of sensors********************************************************/ns = new TVPnaoSensors ();/********************************************************* Get indices********************************************************/LFootBumperLeft = ns->getNameId ("LFootBumperLeft");In the destructor use/********************************************************* Free list of sensors********************************************************/delete ns;In the routine that reads out the sensory data d of type Data5D use/********************************************************* evaluate input data********************************************************/if ((d.datatype == DATA5D_FLOAT) &&

3.6 Available sensor commands 18{}(d.w == ns->size ()) && (d.h == 1) &&(d.d == 1) && (d.t == 1) &&(d.e == 1))/***************************************************** data is sensory data****************************************************/float *data = (float *) d.data;/***************************************************** 0 = LFootBumperLeft value is 0 or 1 (pushed)****************************************************/if (data[LFootBumperLeft] > 0.5f){// You pushed the left foot bumper}3.6 Available sensor commandsThe sensor names and values available on the NAO robot areGroup Number Name DescriptionBumpers 0 LFootBumperLeft Value1 LFootBumperRight Value2 RFootBumperLeft Value3 RFootBumperRight ValueHeadPitch 4 HeadPitch Sensor value5 HeadPitchA Actuator value6 HeadPitchEC Electric current sensor value7 HeadPitchH Hardness actuator value8 HeadPitchT Temperature valueHeadYaw 9 HeadYaw10 HeadYawEC11 HeadYawH12 HeadYawTLAnklePitch 13 LAnklePitch14 LAnklePitchA15 LAnklePitchEC16 LAnklePitchH17 LAnklePitchTLAnkleRoll 18 LAnkleRoll19 LAnkleRollA20 LAnkleRollEC21 LAnkleRollH22 LAnkleRollTLElbowRoll 23 LElbowRoll24 LElbowRollA25 LElbowRollEC26 LElbowRollH

3.6 Available sensor commands 1927 LElbowRollTLElbowYaw 28 LElbowYaw29 LElbowYawA30 LElbowYawEC31 LElbowYawH32 LElbowYawTLHand 33 LHand34 LHandA35 LHandEC36 LHandH37 LHandTLHipPitch 38 LHipPitch39 LHipPitchA40 LHipPitchE41 LHipPitchH42 LHipPitchTLHipRoll 43 LHipRoll44 LHipRollA45 LHipRollEC46 LHipRollH47 LHipRollTLHipYawPitch 48 LHipYawPitch49 LHipYawPitchA50 LHipYawPitchEC51 LHipYawPitchH52 LHipYawPitchTLKneePitch 53 LKneePitch54 LKneePitchA55 LKneePitchEC56 LKneePitchH57 LKneePitchTLShoulderPitch 58 LShoulderPitch59 LShoulderPitchA60 LShoulderPitchEC61 LShoulderPitchH62 LShoulderPitchTLWristYaw 63 LWristYaw64 LWristYawA65 LWristYawEC66 LWristYawH67 LWristYawTRAnklePitch 68 RAnklePitch69 RAnklePitchA70 RAnklePitchEC71 RAnklePitchH72 RAnklePitchTRAnkleRoll 73 RAnkleRoll74 RAnkleRollA

3.6 Available sensor commands 2075 RAnkleRollEC76 RAnkleRollH77 RAnkleRollTRElbowRoll 78 RElbowRoll79 RElbowRollA80 RElbowRollEC81 RElbowRollH82 RElbowRollTRElbowYaw 83 RElbowYaw84 RElbowYawA85 RElbowYawEC86 RElbowYawH87 RElbowYawTRHand 88 RHand89 RHandA90 RHandEC91 RHandH92 RHandTRHipPitch 93 RHipPitch94 RHipPitchA95 RHipPitchEC96 RHipPitchH97 RHipPitchTRHipRoll 98 RHipRoll99 RHipRollA100 RHipRollEC101 RHipRollH102 RHipRollTRHipYawPitch 103 RHipYawPitch104 RHipYawPitchEC105 RHipYawPitchHRKneePitch 106 RKneePitch107 RKneePitchA108 RKneePitchEC109 RKneePitchH110 RKneePitchTRShoulderPitch 111 RShoulderPitch112 RShoulderPitchA113 RShoulderPitchEC114 RShoulderPitchH115 RShoulderPitchTRShoulderRoll 116 RShoulderRoll117 RShoulderRollA118 RShoulderRollEC119 RShoulderRollH120 RShoulderRollTRWristYaw 121 RWristYaw122 RWristYawA

3.6 Available sensor commands 21123 RWristYawEC124 RWristYawH125 RWristYawTAccX 126 AccX X is forward in m/s 2AccY 127 AccY Y is leftAccZ 128 AccZ Z is upGyrAngleX 129 GyrAngleX in radiansGyrAngleY 130 GyrAngleYGyrX 131 GyrXGyrY 132 GyrYGyrRef 133 GyrRefFSRLcenterX 134 FSRLcenterX force sensors in (kilo)gramsFSRLcenterY 135 FSRLcenterYFSRLfrontLeft 136 FSRLfrontLeftFSRLfrontRight 137 FSRLfrontRightFSRLrearLeft 138 FSRLrearLeftFSRLrearRight 139 FSRLrearRightFSRLweight 140 FSRLweightFSRRcenterX 141 FSRRcenterXFSRRcenterY 142 FSRRcenterYFSRRfrontLeft 143 FSRRfrontLeftFSRRfrontRight 144 FSRRfrontRightFSRRrearLeft 145 FSRRrearLeftFSRRrearRight 146 FSRRrearRightFSRRweight 147 FSRRweightsonarLeft 148 sonarLeft 500ms speed updatessonarRight 149 sonarRight useful range 0.25-1.25recognizedWords 150 recognizedWords give confidence levelspeechDetected 151 speechDetected 1 is yes; 0 is noHeadTouchFront 152 HeadTouchFront 0 or 1 being touchedHeadTouchMiddle 153 HeadTouchMiddleHeadTouchRear 154 HeadTouchRear3.6.1 Using statesFigure 5 illustrates a natural cycle of sensing, planning, and generation. In real time processes it iscommon to use state spaces, its a simple way subdivide a large problem into partial components. Theexample constructed moves the arms forward, and after that starts moving the arms in opposite direction.Five states are constructed, the first (state 1) is to position both arms properly, when completedstates 2 and 3 are activated, one moves one arm upward, the other state the other arm downward, whencompleted state 2 activates 4, which in turn moves the arm in th other direction, and when completedit activates state 2 again. Like states 2 and 4 are alternating, The same is done with states 3 and 5 forthe other arm.An example of using these 5 states is given in Figure 6.

3.6 Available sensor commands 22StateCommandId: 1Ready:2, 3Move left and rightshoulder pitch to 60 degCommand 1StateCommandId: 2Ready:4Move left elbowroll to -50 degCommand 2StateCommandId: 3Ready:5Move right elbowroll to 0 degCommand 3StateCommandId: 4Ready:2Move left elbowroll to 0 degCommand 4StateCommandId: 5Ready:3Move right elbow roll to 50 degCommand 5Figure 6: An example using 5 different states. Top left design. Top right implementation. The threewindows provide the parameter settings of the three implemented modules.

4 Programming new modules 234 Programming new modulesThe concept of TiViPE is to embed a routine call from a programmed library into the environmentwithout additional programming. Unfortunately, sometimes the desired routine is not available andneeds to be programmed from scratch, by construction of a software library. The aim of this section isto provide a step by step introduction to project construction, C++ programming of some routine calls,and embedding these routines into TiViPE.4.1 Setting up a C++ libraryConstruction of a project is done by setting up a text file that contains all necessary files to construct alibrary. This text file in turn is converted to a Makefile. A makefile is used by another program calledmake or nmake, both programs in turn call the compiler and linker to compile the source code and linkit together to a library.A project file for the TVProbotics library is a text file called src.pro, and can be found inAppendix A.1.For the construction of a project file the TEMPLATE variable should be set to lib. Next a flag(DEFINES += ’flag’ is required to ensure exportation of routine calls into a library, when usinga Microsoft compiler. Note that ’+=’ stands for concatenation. All header files used (HEADERS+= ’filename’ together with the paths to these files (INCLUDEPATH += ’path’ need to beprovided, likewise for all other libraries where this library depends on are specified using LIBS +=-L’path’ -l’library’. Most important are the files with the source code, these are providedby SOURCES += ’file’. Finally the library needs a name which is specified by TARGET =’name’.The syntax used in the project file is provided by Qt, hence Nokia’s qmake is required to converta project file into a Makefile by runningqmake src.prowhere src.pro is the project file. Note that TiViPE depends on Qt, and when running the abovecommand fails, try running %TVPHOME%\vars.bat first.The library will be compiled usingnmakeand on unix make.By default the gnu compiler exports all routine calls, for the microsoft compiler this is not the case,hence a ’defs’ file is created to properly export routines. For the robotics library this is done by theTVProboticsdefs.h file that can be found at Appendix A.2.4.2 Adding content to the libraryTwo files TVPnaocommand.h and TVPnaocommand.cpp, both files are provided in Appendix A.3and A.4, have been included in the project. In these two files a some routines have been implementedand are discussed below.The first routine created isString TVPrespond (Data5D d,String rs)

4.3 Embedding a routine call into TiViPE 24This test routine reads all sensory data obtained from the robot, stored in variable d, and retrieves arecognized word or set of words rs. The aim is to respond to pushing the feet bumper sensors and tosome of the words recognized.The second test command isString TVPwalkArc (Data5D d,float angle,float radius,float duration)this should let the robot walk in a circle, but in case it has fallen it will first get up before walking. Theroutine has not been completed, see excercise at Section 6.1.The third command isQString TVPheadmenu (Data5D d)touching the first or last of the captive sensors on the head of the NAO robot allows a user to stepthrough a menu, and when the desired item in the menu is found an action can be provided touchingthe central sensor. The TVPheadmenu routine provides a single step only, and must be used in a loopconstruction. Latter is performed by the commandStateStringHeadmenu routine. The TiViPEthe headmenu example is explained in more detail in Section 5.2.Two other commands implemented in TVPnaocommand.cpp are standing TVPstandup andsitting down TVPsitdown. Both commands make use of the routine TVPgetBodyPose latterreads out the sensory data and provides the desired body posture by means of an integer number. Bothstandup and sitdown generate a string command that is dependent on the body posture, and are partialsolutions towards a goal position, either standing or sitting. Hence both routines are useful if they areused in repetition until the solution is found.Repetition is performed by using routines in a state-space concept. In TVPnaocommand.cppthese routines all start with commandStateString. In the current implementation Headmenu,StandUp, Sitdown,LookForObject, MoveToObject, and Pose are available.In Section 5 the use of states will be explained in more detail and applications are given how statesare used within TiViPE.4.3 Embedding a routine call into TiViPEIn Section 4.2 two routines have been (partly) implemented TVPwalkArc and TVPrespond.Sartup TiViPE and select File->Module->New and use an appropriate name for the module asdescribed in Help->Modifying Modules.For the TVPrespond module the following is filled out in the 10 tabloids:# ---Key-Brief-Description-----------------------------------------------------CommandNaoBySensing yields a string that can be used as command based uponsensed data.# ---Key-Full-Description------------------------------------------------------CommandNaoBySensing yields a string that can be used as command based uponsensed data.# ---Key-Input-Description-----------------------------------------------------Input sensory data.Input recognized sentence sensory data.# ---Key-Output-Description----------------------------------------------------Output output command string.# ---Key-Parameter-Description-------------------------------------------------# ---Key-Examples--------------------------------------------------------------

4.3 Embedding a routine call into TiViPE 25None.# ---Key-Libraries--------------------------------------------------------------L$(TVPHOME)/lib -lTVProbotics# ---Key-Include---------------------------------------------------------------$(TVPHOME)/Libraries/TVProbotics/src:TVPnaocommand.h# ---Key-Gui-------------------------------------------------------------------Input("Input sensory data", "-i", DATA5D, DATA5D_FLOAT) [1];Input("Input recognized sentence", "-irs", STRING) [2];Output("Output", "-o", STRING) [F];Integer("Poll", "-poll", 100, 0, INT_MAX);# ---Key-Code------------------------------------------------------------------TVPnaocommand::TVPrespondFor the TVPwalkArc module the following is filled out in the 10 tabloids:# ---Key-Brief-Description-----------------------------------------------------CommandMoveArc moves an arc given a radius, and anangle with the aim to complete it in an a-priori given time.# ---Key-Full-Description------------------------------------------------------CommandMoveArc moves an arc given a radius, and anangle with the aim to complete it in an a-priori given time.# ---Key-Input-Description-----------------------------------------------------Input sensor data provides sensor data.# ---Key-Output-Description----------------------------------------------------Output command output command yieldseither output for arc movement or preparation for arc movement.# ---Key-Parameter-Description-------------------------------------------------Angle in degrees.Radius in meters.Duration in milliseconds.Robot type of robot that is being used.# ---Key-Examples--------------------------------------------------------------None.# ---Key-Libraries--------------------------------------------------------------L$(TVPHOME)/lib -lTVProbotics# ---Key-Include---------------------------------------------------------------$(TVPHOME)/Libraries/TVProbotics/src:TVPmotion.h# ---Key-Gui-------------------------------------------------------------------Input("Sensor data", "-i", DATA5D, DATA5D_FLOAT) [1];Output("Output", "-o", STRING) [F];BeginGroup(2);Integer("Poll", "-poll", 33, 0, INT_MAX);Float("Angle in degrees", "-angle", 90, -360, 360, 10) [2];Float("Radius in meters", "-radius", 0.25, 0, DBL_MAX, 10) [3];Float("Duration in milliseconds", "-time", 5000, 0, DBL_MAX, 10) [4];EndGroup();Toggle("Robot", EXCLUSIVE) [5];ToggleElement("Admoveo", "-admoveo");ToggleElement("Nao", "-nao", CHECKED);EndToggle();# ---Key-Code------------------------------------------------------------------TVPmotion::TVPmoveArcPressing the ok button will generate the module and is ready to be used within the TiViPE envi-

5 Robot Programming 26ronment.5 Robot ProgrammingThis section is created to provide the user insight in how to program a robot using the concept ofTiViPE by showing an application.5.1 Programming a state space routineThe definition of a state element within TiViPE is that a state1. contains a unique and fixed id; implemented by an integer number2. generates a command; implemented as a string3. activates other states when command is completed; implemented as a stringor more formal a state is a triple (id, command, states).States enter a loop that is timer and event driven. For instance when one state is being used (10,“say(Hello)”, “10”), it implies that Hello is said over and over again.TiViPE makes no restrictions to command and states A command can be an empty string, and canbe different over time. For instance (10, C[“say(Hello)”][“Say(Bye)”], C[“10”][“20”]), (20, ””, “20”),where C is a condition that can be implemented by the user, if the condition holds the first command,and first set of states is taken, i.e., “Say(Hello)”, and “10”, or otherwise second command and set ofstates is taken.A state module is connected to another state module by passing states using istates and ostates andby passing commands using icommands and ocommands. A minimalistic state space module Name ofclass ClassName is implemented as follows:void ClassName::commandStateStringName (Data5D istates,String icommands,Data5D &ostates,String &ocommands){/***************************************************************************** Three variables need to be set:* -int id has a fixed and unique value and is a-priori known* -QString states are the states that are activated once* the command is executed* -QString comm is the command to be executed on the robot* here the TiViPE robot text language is used****************************************************************************/if (initStream){id = 10;comm = "";states = QString ("%1").arg (id); // activate itselfinitStream = false;}/***************************************************************************** Generate command to be executed and set of states to be activated****************************************************************************/states = "20";comm = "say (Hello)";/****************************************************************************

5.2 HeadMenu 27}* Create state string****************************************************************************/tcc->commandStateString (istates, icommands, id, comm, states, ostates,ocommands);The magic happens in the commandStateString routine call. This routine passes on thestates that will be activated and the commands that will be executed. It is done by appending statesand comm if and only if id is currently active. The order of processing the state modules in TiViPEtherefore does not matter, the only requirement is that the modules need to be connected in a sequentialorder.The last module passes the final set of commands to the SendCommand2 module that in turngives data to the robot module. This module is also connected to writeSharedData that in turntriggers the UpdateStates module to begin a new cycle.As mentioned before, TiViPE allows the user to change the states states and command commvariables, most likely on the basis of sensory information, to make a single state powerful in itselfby changing commands, but also extremely powerful by deciding which states to activate when thiscommand is completed.The best way to design robot behaviors is to graphically design a state first, and couple a list ofcommands that can be generated within this state. Next select all states necessary to obtain the desiredbehavior, anf finally draw the connections between the different states, and use differenct color codingsto denote different activation types upon completion.Some examples, where the command generated depends on the sensory information obtained, aregiven in Appendixapp:naocommand.cpp for standing up at lines 988 to 1042 and sitting down at lines1062-1116.The state command commandStateStringLookForObject implemented at lines 1119 to1411 of Appendixapp:naocommand.cpp uses a timer to decide what states to active. If within a givenmaximum time an object is found, different states are activated compared to when no object would befound. An a-priori known set of options can be passed on as states to be activated in this module. Forinstance, the variable astates can be set to [20,30][40,50] to denote that the module activatesstates 20 and 30 when for instance a criterion x is met, while it activates the second set when it fails tomeet x. In the code it implies that a substring is copied, from the variable astates, e.g., illustratedat lines 1570-1571, 1617-1618, and 1650-1651.5.2 HeadMenuThe module CommandStateNaoHeadMenu is a specific module where individual commands canbe executed stepping through a menu. The internal construction is made so that every menu elementcontains 4 states, one for no action, a front button, a middle button, and a rear button touchedaction. Detailed comments can be found in the TVPnaocommand.cpp source code as well, seeAppendixapp:naocommand.cpp lines 760 to 788.The module uses an offset, and makes the starting state is equal to the offset + 1. The menu of thismodule has a cyclic property, so it cannot be used with other state modules without modification.The whole headmenu.net program in TiViPE, found in directory $TVPHOME/misc/Networks,and illustrated in Figure 7 contains solely 2 modules NaoSense and CommandStateNaoHeadMenuMerged.Latter is a merged module illustrated by 5 green modules at the right side of this module. Merging allmodules UpdateState, all state modules, writeSharedData5D, SendCommand2, and NaoRobot isrequired. Creating a single module involves a single process (with multiple threads) that guarranteesthe order of processing, hence compounding these modules is required. These five modules contain a

5.3 Object detection and tracking 28Figure 7: TiViPE implementation of headmenu control.key (in some cases a second key is used it has an “R” appended) for the robot, and one for the states.For every robot these keys need to be unique.Note that the CommandString and NaoRobot are used in the beginning and the end to enableand disable the stiffness of the motors mannually to avoid overheating of the motors.5.3 Object detection and trackingSensing the environment and acting upon what is happening is one of the key elements in robotics. Asmalle example has been created to show how face tracking is performed using two states, see also thetop part of Figure 8. Both states are active in an alternating manner.The first state tracks a face, here there are 3 different commands possible, depending on the sensoryinformation:1. no face has been found: do nothing2. a face has been found and it is in the middle of the visual field: show that a face is in the middleby enabling the leds of the eyes3. a face has been found but it is not in the middle of the visual field: move face in the middle ofthe visual field.In all cases the following state is activated. The code of tracking faces is found in TVPnaocommand1.cpp.The second state says how many faces have been found.

5.4 Standing up and sitting down 29Figure 8: TiViPE implementation (twostates.net of face tracking at the top and alternationbetween standing up and sitting at the bottom part. On the right side the merged components areillustrated.States are updates either by time or by event, therefore all modules are evaluated as if they arein a loop. The activation of states determines what commands are activated. A single state moduleshould activate itself to get updated continously, if it activates another (non-existing) module, it willbe executed one time before activating an (illegal) unknown state. By creating 2 states a ping-pongeffect between 2 states can be created as is done in this example.5.4 Standing up and sitting downA similar example is given when sensing the embodiment rather than the environment by letting arobot stand up and sit down in an alternating manner. The implemented network is given at the bottomof Figure 8.Both modules generate a command that is depending on the current body posture, but that does notnecessarily bring the robot in one step to the desired position, see A.4 lines 522-584 and lines 586-646what commands are generated for standing up and sitting down respectively.The implementation of the state modules for standing up and sitting down are given in A.4 lines970-1042 and 1044-1116. Both modules stay in the current state until the desired pose is obtained, seealso lines 1033-1036 and 1107-1110.Two states have been created and connected to get a ping-pong effect. In this example we showthat multiple pings (or pongs) can occur, by deciding when the robot has obtained a desired position.Hence one can consider that there is an if-the-else (or case) construction created within a module. Inthis case to activate itself or to activate the other module. Therefore a state, when activating itself, canbe subdivided into small steps and can be called multiple times before reaching its goal.

5.5 Nao demonstration 2011 30Start10 Enable stiffnessstiff (1, 500, 0)20 Stand upStep by step approach to stand;Stays in this state until robot stands229 Say-move to object30 Find objectFind object (Face areas as input)60 second criterion on success40 Pose: wave hand 50 Anybody there?230 Find ObjectFind object (Disc areas as input)60 second criterion on success350 Say: No Object100 Say-exercise51 Sit down240 Move to objectStep by step approach to sit down;Stays in this state until robot sits230-350 Goto object110 Pose: tai-chi260 Give up52 Lie down229 Say-move to object250 Found move back51 Sit downa)230-350 Goto object250, 260, 350b)Step by step approach to sit down;Stays in this state until robot sitsFigure 9: Scheme of Nao demonstration 2011. (a) Overall scheme. (b) 230-350 Goto Object explanation.Black arrow denotes goto this state when ready, green arrow denotes goto this state whencondition is met, and a red arrow denotes goto this state upon failure.5.5 Nao demonstration 2011The aim of this demonstration was to give an indication that within short period of time an interactivescenario can be designed and implemented. The robot shows different actions on an area of approximatelyone square meter.In the demonstration face and object recognition are performed, the robot interacts on the basis ofthis sensory information. In addition, some activities are done to entertain people, in this case by atai-chi exercise, and relax, i.e., sit and lie down to avoid overheating of the motors.A design was made that is illustrated in Figure 9.In total 15 different states have been used. The starting states are provided by parameter Activestates in module UpdateeStates. In the demonstration the parameter is set to “10”. State 10is implemented as (10, “stiff (1, 500, 0)”, “20”) for the NAO robot it is required to enable and disablestiffness from the motors. In the current generation of robots motors need to be regularly set to disabledstiffness “stiff (0, 500, 0)” to avoid overheated motors. Its implementation is perfromed by moduleCommandState, where the three parameters Id, Command, and States to activate are filled out asgiven as explained before. The module calls routine commandStateString directly.State 20 reads out the body position from the sensed values and determines what move should beperformed to get to a standing position, once the robot is standing state 3 is activated. The implementationof this module is called commandStateStringStandup and is found in Appendix A.4 atlines 970-1043. Note that all commandStateString commands generate a triple (id, command,states to activate) before calling commandStateString.State 30 obtains all areas of recognized faces. In case a face area is found the robot moves its headtowared the position of the face, i.e. it tries to position the face in the middle of the object. Once this

5.5 Nao demonstration 2011 31Figure 10: TiViPE implementation of nao robot demonstration. Five modules (in green) form thedemonstration. The CommandStateRobotDemo2011Mereged module is a compound module,the content of this module is given at the left. The content of the CVfacediscDetectMergedmodule is given at the bottom-right.is done state 40 will be activated. If no faces are found over a certain period of time, the module timesout and activates state 50. Module CommandStateNaoFindObject is used to execute routinecommandStateStringLookForObject and can be found in Appendix A.4 at lines 1118-1411.State 50 is implemented by the triple (50, “say(Hello there is anybody home?, 100) & wait(10000)”,“51”). State 51 lets the robot sit down. The implementation of this module is very similar to standingup, as has been explained before. When ready state 52 is activated. State 52 let the robot wait in asitting position and then lie down its triple is as follows (52, “stiff (0, 500, 0) & wait(60000) & stiff (1,500, 0) & [[[movem (HeadYaw, -39.0, ... LAnkleRoll, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0,1000, 0.0, 2000) —stiff (1.0, 1000, 3000, 0.1, 1000, 0)]] & wait(120000)] & say(That was relaxing,100) & stiff (1, 500, 0)”, “10”). State 52 activates state 10 and thus creates repetitive behavior sincethe first state is being activated again.The CommandStatePose performs a preprogrammed pose. Index 9 denotes a waving posture,parallel to this pose the robot says “Hello”, as performed by state 40. After execution state 100 is activated.State 100 is a standard triple (100, “say(Lets start a bit of excercising together)”, “110”) State110 performs a preprogrammed pose. Index 10 denotes tai-chi. After completion 229 is activated. Thisstate is again a standard triple (229, “wait(2000) & say(That was fun I feel relaxed) & wait(2000) &say(Time to go to work) & wait(1000) & say(I first have to find an object and then to go towards this object),“230”).The next module is a merged object called CommandStatesToObjectMerged and contains5 modules. The first one searches disc type of objects that have a known diameter. Hence walkingtowards the object is possible by calibrating the number of camera pixels with the known object.The calibrated camera parameters are used in module CommandStateNaoMoveToObject. Itsimplementation is given in Appendix A.4 at lines 1413-1699.The implementation of the design is given in Figure 10. The green modules contain the at the right

6 Exercises 32upper part represent the demonstration. Two robot sensory blocks have been used called NaoSenseand FrameGrabber the first one provides all relevant sensory data from the robot, while the secondprovides video images from the robot. The ReadData5D module reads a set of disc objects storedon the local machine from file, and uses this database of objects for detecting objects that is performedby the RecognizCircularObjects module that is part of the CVfacediscDetectMergedmodule. The module shows recognized face, eye, and disc areas in different colors on the display togive the an impression how visual data is observed from the perspective of the robot.6 ExercisesThe aim of this section is to create attractive exercises that contribute to the robotics library. Thissection is therefore likely to change version by version, and solved exercises might shift to a newsection called ’Solved Exercises’.6.1 Exercise 1: Replace walka by walkto commandMoving an arc was in 1.3.17 a standard call but it has been superseded by the move to a positioncommand. Recalculate the the angle and radius into the an x, y position and substitute the code in theTVPwalkArc routine of TVPnaocommand.cpp.6.2 Exercise 2: Calibration of walk, walks, and walkaThe walk, walks, and walka should move to a position in a specified time, unlike the walkto commandwhich moves to the exact position. Consider movement over 1, 3, 5, 10, 15, 20, and ≥30 seconds, andcalibrate the three routines.6.3 Exercise 3: Create a more advanced face tracking moduleReimplement the face tracking module provided in TVPnaocommand1.cpp, and create a searchingmechanism when no object has been found. In case no object is found, keep the current state active.Use a timer to let the module time out once the robot has searched around but did not find anything.6.4 Exercise 4: Flexible state activationImplement the throwing dice example:(10, "say(Throwing_dice)", "[20][30][40][50][60][70]")(20, "say(One)", "10")(30, "say(Two)", "10")(40, "say(Three)", "10")(50, "say(Four)", "10")(60, "say(Five)", "10")(70, "say(Six)", "10")Note that the first module commandStateDice needs to be implemented, while for the other sixmodules the ready module commandStateString can be used.

6.5 Exercise 5: Getting grip over active states 336.5 Exercise 5: Getting grip over active statesCreate of a state that activates two other states. When two states have been activated, reactive thesestates such that a cascade of states is activated. Think of a solution that when two states have beenactivated that after completion, one state is active.One could think of the fibonacci numbers (Leonardo di Pisa, 1202) 1, 1, 2, 3, 5, 8, 13, 21, 34, 55,89, 144, 233, 377, 610, 987, 1597, 2584, ... . In formula f 0 = 0, f 1 = 1, and f n = f n−1 + f n−2 , forn > 1, where a positive n is given and the robot is supposed to say f n times “One”. Note that it iswise to keep n small.6.6 Advanced Exercise 6: Auditory processingCreate a module that processes auditory information on your local machine using for instance a usbcameraor microphone. Take relevant features from the processed auditory data, for instance, pitches,or recognized words.Create a state module that uses processed auditory data.6.7 Exercise 7: User scenario constructionA free exercise, where it is recommended to combine blocks, using multiple states.7 Advanced Robot Programming7.1 Adding commands to the NAO robotFor instance a command like movemsi has been added. An in depth explanation might be given forthis textual robot command.One can imagine that textual commands like listen, or understand could be added as textualrobot commands.7.2 Adding sensing elements to the NAO robotFor instance infra red, a NAO robot is equipped with IR, and hence with a remote control the robot canbe instructed! It might be an option to give additional control, to a robot. Note that the robot can bothsend and receive IR.The reason that IR has not been implemented yet is that a remote computer is a virtual remotecontrol and hence there has been no true need to implement IR.7.3 Enabling other features on the NAO robotFor instance sonar, in case it is not too much energy consuming. If this is the case commands toenable and disable sonar by adding text commands might be desired, e.g., enable(’name’) anddisable (name).7.4 Adding other robotsSimpler robots like an Admoveo or Khepera can be added to this setting as well by adding modifiedversions of TVPnao files in the TVProbotics library.

A TVProbotics source code 34ATVProbotics source codeThis section contains the source code some of the files in the TVProbotics library.A.1 Project file src.pro1 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−2 # C o p y r i g h t (C) 2010−2011 Tino Lourens . A l l r i g h t s r e s e r v e d .3 #4 # P e r m i s s i o n t o use , copy , modify , and d i s t r i b u t e t h i s5 # s o f t w a r e and i t s d o c u m e n t a t i o n under t h e t e r m s of t h e GNU6 # G e n e r a l P u b l i c L i c e n s e i s h e r e b y g r a n t e d . No r e p r e s e n t a t i o n s7 # a r e made a b o u t t h e s u i t a b i l i t y of t h i s s o f t w a r e f o r any8 # p u r p o s e . I t i s p r o v i d e d ” as i s ” w i t h o u t e x p r e s s e d or i m p l i e d9 # w a r r a n t y . See t h e GNU G e n e r a l P u b l i c L i c e n s e f o r more10 # d e t a i l s .11 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−12 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−13 # \ f i l e s r c . pro14 # \ b r i e f Qt M a k e f i l e f o r TVP r o b o t i c s l i b r a r y .15 # \ a u t h o r Tino Lourens16 # \ d a t e 10 Apr 201017 # \ v e r s i o n 16 Apr 201118 #19 # Qt M a k e f i l e f o r TVP r o b o t i c s l i b r a r y .20 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−21 # Setup d e t a i l e d i n f o r m a t i o n a b o u t p r o d u c t and ownership22 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−23 QMAKE TARGET COMPANY = TiViPE24 QMAKE TARGET PRODUCT = TVProbotics25 QMAKE TARGET DESCRIPTION = TVP r o b o t i c s l i b r a r y26 QMAKE TARGET COPYRIGHT = C o p y r i g h t (C) 2010−2011 TiViPE27 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−28 # C o n s t r u c t a l i b r a r y29 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−30 TEMPLATE = l i b31 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−32 VERSION = 2 . 0 . 933 QT += g u i network o p e n g l34 CONFIG += q t t h r e a d35 CONFIG += r e l e a s e36 CONFIG += c o n s o l e37 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−38 # Under M i c r o s o f t windows when u s i n g msvc / i n t e l we need t o e x p o r t t h e r o u t i n e s39 # of a c l a s s by p r o v i d i n g our own l i b r a r y f l a g TVPROBOTICS LIB .40 #41 # Under GNU by d e f a u l t e v e r y t h i n g i s e x p o r t e d ; f o r e f f i c i e n c y r e a s o n s i t i s42 # f a s t e r t o e x p o r t what i s n e c e s s a r y only . In a d d i t i o n we t r y t o keep t h e43 # l i b r a r y c l e a n / compact by s t r i p p i n g a l l symbol i n f o r m a t i o n .44 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−45 win32−msvc ∗ | win32−i c c {46 DEFINES += TVPROBOTICS LIB47 }48 unix {49 DEFINES += TVPROBOTICS LIB50 QMAKE CFLAGS += − f v i s i b i l i t y = hidden51 QMAKE CXXFLAGS += − f v i s i b i l i t y = hidden52 QMAKE CXXFLAGS += − f v i s i b i l i t y −i n l i n e s −hidden

A.1 Project file src.pro 3553 QMAKE LFLAGS += −s54 }55 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−56 # P a t h s57 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−58 # Local59 INCLUDEPATH = .60 # TVP61 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / TVP / s r c62 INCLUDEPATH += $ (TVPHOME) / S t r u c t u r e s / s r c63 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / TVP / TVPextension / s r c64 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / TVPip / s r c65 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / Data5D / s r c66 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / TVPcuda / s r c67 INCLUDEPATH += $ (TVPHOME) / misc / Images /Xpm68 # Aldebaran NAO i n c l u d e s69 # V 1 . 1 0 . 2 570 INCLUDEPATH += $ ( AL DIR ) / i n c l u d e71 INCLUDEPATH += $ ( AL DIR ) / i n c l u d e / alcommon72 INCLUDEPATH += $ ( AL DIR ) / i n c l u d e / r t t o o l s73 INCLUDEPATH += $ ( AL DIR ) / i n c l u d e / a l a u d i o74 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−75 # L ibs76 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−77 # TVP78 LIBS = −L$ (TVPHOME) / l i b −lTVP −lData5D79 # Aldebaran l i b s cmakes a r e found i n l i b \cmake\ modules80 # V 1 . 1 0 . 2 581 LIBS += −L$ ( AL DIR ) / l i b82 LIBS += −L$ ( AL DIR ) / l i b / n a o q i83 LIBS += −lalcommon84 LIBS += −l a l v a l u e85 LIBS += −l a l c o r e86 LIBS += −l p t h r e a d87 LIBS += − l a l t o o l s88 LIBS += −l a l s o a p89 LIBS += −l t i n y x m l90 LIBS += − l r t t o o l s91 LIBS += −l a l t h r e a d92 LIBS += − l a l f a c t o r y93 LIBS += − l a l f i l e94 LIBS += −l a l l o g95 # o p t i o n a l Aldebaran l i b s96 #LIBS += − l l i b b o o s t s e r i a l i z a t i o n −vc90−mt−1 3897 #LIBS += −l l i b b o o s t d a t e t i m e −vc90−mt−1 3898 #LIBS += −l l i b b o o s t t h r e a d −vc90−mt−1 3899 #LIBS += −l l i b b o o s t s i g n a l s −vc90−mt−1 38100 #LIBS += −l l i b b o o s t f i l e s y s t e m −vc90−mt−1 38101 #LIBS += −l l i b b o o s t p r o g r a m o p t i o n s −vc90−mt−1 38102 #LIBS += −l i p p c103 #LIBS += −l f i n d i p p c104 #LIBS += −l p y t h o n105 #LIBS += −l a l i p p c106 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−107 # F i l e s108 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−109 HEADERS = T V P r o b o t i c s d e f s . h110 HEADERS += TVPcommandconstruction . h111 SOURCES = TVPcommandconstruction . cpp

A.2 Source code of TVProboticsdefs.h 36112 HEADERS += TVPmotion . h113 SOURCES += TVPmotion . cpp114 HEADERS += TVPstringgen . h115 SOURCES += TVPstringgen . cpp116 HEADERS += TVProbotcommand . h117 SOURCES += TVProbotcommand . cpp118 #119 # NAO120 #121 HEADERS += TVPnao . h122 SOURCES += TVPnao . cpp123 HEADERS += TVPnaoaldebaran . h124 HEADERS += TVPnaocommand . h125 SOURCES += TVPnaocommand . cpp126 HEADERS += TVPnaocommand1 . h127 SOURCES += TVPnaocommand1 . cpp128 HEADERS += TVPnaoleds . h129 SOURCES += TVPnaoleds . cpp130 HEADERS += TVPnaomotion . h131 SOURCES += TVPnaomotion . cpp132 HEADERS += TVPnaomotion1 . h133 SOURCES += TVPnaomotion1 . cpp134 HEADERS += TVPnaorobot . h135 HEADERS += TVPnaopython . h136 SOURCES += TVPnaopython . cpp137 HEADERS += TVPnaosensors . h138 SOURCES += TVPnaosensors . cpp139140 #HEADERS += TVPnaosound . h141 #SOURCES += TVPnaosound . cpp142 #HEADERS += mysoundprocessingmodule . h143 #SOURCES += mysoundprocessingmodule . cpp144 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−145 # O b j e c t d i r e c t o r y146 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−147 OBJECTS DIR = . . / o b j148 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−149 # T a r g e t150 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−151 TARGET = TVProbotics152 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−153 # D e s t i n a t i o n154 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−155 #DESTDIR = $ (TVPHOME) / l i b156 DESTDIR = . . / . . / . . / l i b157 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−A.2 Source code of TVProboticsdefs.h1 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗2 ∗ C o p y r i g h t (C) 2010 Tino Lourens . A l l r i g h t s r e s e r v e d .3 ∗4 ∗ P e r m i s s i o n t o use , copy , modify , and d i s t r i b u t e t h i s s o f t w a r e and i t s5 ∗ d o c u m e n t a t i o n under t h e t e r m s of t h e GNU G e n e r a l P u b l i c L i c e n s e i s h e reby6 ∗ g r a n t e d . No r e p r e s e n t a t i o n s a r e made a b o u t t h e s u i t a b i l i t y of t h i s s o f t w a r e7 ∗ f o r any p u r p o s e . I t i s p r o v i d e d ” as i s ” w i t h o u t e x p r e s s e d or i m p l i e d8 ∗ w a r r a n t y . See t h e GNU G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .

A.3 Source code of TVPnaocommand.h 379 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /10 / ∗ ∗11 ∗ \ f i l e T V P r o b o t i c s d e f s . h12 ∗ \ b r i e f This f i l e c o n t a i n s d e f i n i t i o n s13 ∗ \ a u t h o r Tino Lourens14 ∗ \ d a t e 10 Apr 201015 ∗ \ v e r s i o n 10 Apr 201016 ∗17 ∗ \ h t m l o n l y \ e n d h t m l o n l y18 ∗ This f i l e c o n t a i n s s t r u c t u r e s and p r o t o t y p e s f o r l i b r a r y e x p o r t .19 ∗ \ h t m l o n l y \ e n d h t m l o n l y20 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /21 # i f n d e f TVPROBOTICSDEFS H22 # d e f i n e TVPROBOTICSDEFS H2324 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗25 ∗ Windows : M i c r o s o f t v i s u a l s t u d i o and i n t e l c o m p i l e r26 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /27 # i f d e f i n e d ( MSC VER ) | | d e f i n e d ( INTEL COMPILER )28 / ∗ ∗29 ∗ msvc r e q u i r e s l i b r a r y e x p o r t s o t h e r w i s e t h e l i b r e m a i n s empty30 ∗ use of ” d e f i n e TVPROBOTICS LIB” i n t h e h e a d e r t o g e t h e r with31 ∗ TVPROBOTICS EXPORT a t t h e c l a s s .32 ∗ /33 # i f d e f TVPROBOTICS LIB34 / ∗ ∗35 ∗ L i b r a r y e x p o r t f l a g36 ∗ /37 # d e f i n e TVPROBOTICS EXPORT d e c l s p e c ( d l l e x p o r t )38 # e l s e39 / ∗ ∗40 ∗ L i b r a r y e x p o r t f l a g41 ∗ /42 # d e f i n e TVPROBOTICS EXPORT d e c l s p e c ( d l l i m p o r t )43 # e n d i f44 # e l i f GNUC >= 4 / / gnu c o m p i l e r v e r s i o n 4 and up45 / ∗ ∗46 ∗ gnu c o m p i l e r by d e f a u l t e x p o r t s a l l r o u t i n e so add47 ∗ QMAKE CXXFLAGS += − f v i s i b i l i t y = hidden48 ∗ QMAKE CXXFLAGS += − f v i s i b i l i t y −i n l i n e s −hidden49 ∗ t o your p r o j e c t f i l e50 ∗ TVPROBOTICS EXPORT a t t h e c l a s s .51 ∗ /52 # d e f i n e TVPROBOTICS EXPORT a t t r i b u t e ( ( v i s i b i l i t y ( ” d e f a u l t ” ) ) )53 # e l s e / / o t h e r c o m p i l e r54 / ∗ ∗55 ∗ L i b r a r y e x p o r t f l a g of any o t h e r c o m p i l e r i s empty56 ∗ /57 # d e f i n e TVPROBOTICS EXPORT58 # e n d i f5960 # e n d i f / ∗ TVPROBOTICSDEFS H ∗ /A.3 Source code of TVPnaocommand.h1 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗2 ∗ C o p y r i g h t (C) 2010−2011 Tino Lourens . A l l r i g h t s r e s e r v e d .

A.3 Source code of TVPnaocommand.h 383 ∗4 ∗ P e r m i s s i o n t o use , copy , modify , and d i s t r i b u t e t h i s s o f t w a r e and i t s5 ∗ d o c u m e n t a t i o n under t h e t e r m s of t h e GNU G e n e r a l P u b l i c L i c e n s e i s h e reby6 ∗ g r a n t e d . No r e p r e s e n t a t i o n s a r e made a b o u t t h e s u i t a b i l i t y of t h i s s o f t w a r e7 ∗ f o r any p u r p o s e . I t i s p r o v i d e d ” as i s ” w i t h o u t e x p r e s s e d or i m p l i e d8 ∗ w a r r a n t y . See t h e GNU G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .9 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /10 / ∗ ∗11 ∗ \ f i l e TVPnaocommand . h12 ∗ \ b r i e f This f i l e c o n t a i n s s t r u c t u r e s and p r o t o t y p e s .13 ∗ \ a u t h o r Tino Lourens14 ∗ \ d a t e 24 Apr 201015 ∗ \ v e r s i o n 11 Jun 201116 ∗17 ∗ \ h t m l o n l y \ e n d h t m l o n l y18 ∗ This f i l e c o n t a i n s s t r u c t u r e s and p r o t o t y p e s f o r s t r i n g g e n e r a t i o n19 ∗ \ h t m l o n l y \ e n d h t m l o n l y20 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /21 # i f n d e f TVPNAOCOMMAND H22 # d e f i n e TVPNAOCOMMAND H2324 / / TVP25 # i n c l u d e 26 # i n c l u d e 27 / / T VProbotics l i b28 # i n c l u d e 29 # i n c l u d e 30 # i n c l u d e 31 # i n c l u d e 32 # i n c l u d e 33 # i n c l u d e 34 # i n c l u d e 3536 / ∗ ∗37 ∗ \ b r i e f C l a s s T V P s t a t e s p a r s e s s t a t e s e t t i n g s38 ∗ \ a u t h o r Tino Lourens39 ∗ \ d a t e 11 Jun 201140 ∗ \ v e r s i o n 11 Jun 201141 ∗42 ∗ \ h t m l o n l y \ e n d h t m l o n l y43 ∗ C l a s s TVPnaocommand1 c l a s s t o g e n e r a t e a commands f o r nao r o b o t .44 ∗ \ h t m l o n l y \ e n d h t m l o n l y45 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /46 c l a s s TVPROBOTICS EXPORT T V P s t a t e s47 {48 p u b l i c :49 Q S t r i n g g e t S t a t e s ( Q S t r i n g s ,50 i n t i n d e x ) ;51 } ;5253 / ∗ ∗54 ∗ \ b r i e f C l a s s TVPnaocommand c l a s s t o g e n e r a t e a commands f o r nao r o b o t .55 ∗ \ a u t h o r Tino Lourens56 ∗ \ d a t e 10 Apr 201057 ∗ \ v e r s i o n 23 Apr 201158 ∗59 ∗ \ h t m l o n l y \ e n d h t m l o n l y60 ∗ C l a s s TVPnaocommand c l a s s t o g e n e r a t e a commands f o r nao r o b o t .61 ∗ \ h t m l o n l y \ e n d h t m l o n l y

A.3 Source code of TVPnaocommand.h 3962 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /63 c l a s s TVPROBOTICS EXPORT TVPnaocommand : p u b l i c TVPstringgen64 {65 p u b l i c :66 TVPnaocommand ( ) ;67 ˜ TVPnaocommand ( ) ;68 S t r i n g TVPrespond ( Data5D d ,69 S t r i n g r s ) ;70 Q S t r i n g TVPwalkArc ( Data5D d ,71 f l o a t angle ,72 f l o a t r a d i u s ,73 f l o a t d u r a t i o n ) ;74 i n t TVPgetBodyPose ( Data5D d ) ;75 Q S t r i n g TVPstandup ( Data5D d ) ;76 Q S t r i n g TVPsitdown ( Data5D d ) ;77 Q S t r i n g TVPheadmenu ( Data5D d ) ;78 void commandStateStringHeadmenu ( Data5D i s t a t e s ,79 S t r i n g icommands ,80 i n t i d o f f s e t ,81 Data5D &o s t a t e s ,82 S t r i n g &ocommands ,83 Data5D d ) ;84 void c o m m a n d S t a t e S t r i n g S t a n d u p ( Data5D i s t a t e s ,85 S t r i n g icommands ,86 i n t i d s t a r t ,87 Q S t r i n g i d r e a d y ,88 Data5D &o s t a t e s ,89 S t r i n g &ocommands ,90 Data5D d ) ;91 void c o m m a n d S t a t e S t r i n g S i t d o w n ( Data5D i s t a t e s ,92 S t r i n g icommands ,93 i n t i d s t a r t ,94 Q S t r i n g i d r e a d y ,95 Data5D &o s t a t e s ,96 S t r i n g &ocommands ,97 Data5D d ) ;98 void c o m m a n d S t a t e S t r i n g L o o k F o r O b j e c t ( Data5D i s t a t e s ,99 S t r i n g icommands ,100 i n t i d s t a r t ,101 Q S t r i n g a s t a t e s ,102 Data5D &o s t a t e s ,103 S t r i n g &ocommands ,104 Data5D d ,105 Data5D s ,106 i n t maxTime ) ;107 void commandStateStringMoveToObject ( Data5D i s t a t e s ,108 S t r i n g icommands ,109 i n t i d s t a r t ,110 Q S t r i n g a s t a t e s ,111 Data5D &o s t a t e s ,112 S t r i n g &ocommands ,113 Data5D d ,114 Data5D s ,115 i n t maxTime ,116 d o u b l e d i s t a n c e ,117 d o u b l e d i a m e t e r ) ;118 void c o m m a n d S t a t e S t r i n g P o s e ( Data5D i s t a t e s ,119 S t r i n g icommands ,120 i n t i d s t a r t ,

A.3 Source code of TVPnaocommand.h 40121 Q S t r i n g i d r e a d y ,122 Data5D &o s t a t e s ,123 S t r i n g &ocommands ,124 i n t index ,125 Q S t r i n g command = ” ” ) ;126 / ∗ ∗127 ∗ walk an a r c128 ∗ /129 S t r i n g TVPwalkArcS ( Data5D d ,130 f l o a t angle ,131 f l o a t r a d i u s ,132 f l o a t d u r a t i o n )133 { r e t u r n TVPstring ( TVPwalkArc ( d , angle , r a d i u s , d u r a t i o n ) ) ; } ;134 / ∗ ∗135 ∗ perform s t a n d up136 ∗ /137 S t r i n g TVPstandupS ( Data5D d )138 { r e t u r n TVPstring ( TVPstandup ( d ) ) ; } ;139 / ∗ ∗140 ∗ perform s i t down141 ∗ /142 S t r i n g TVPsitdownS ( Data5D d )143 { r e t u r n TVPstring ( TVPsitdown ( d ) ) ; } ;144 / ∗ ∗145 ∗ perform head menu t e s t146 ∗ /147 S t r i n g TVPheadmenuS ( Data5D d )148 { r e t u r n TVPstring ( TVPheadmenu ( d ) ) ; } ;149 p r i v a t e :150 / ∗ ∗151 ∗ f r o n t b u t t o n pushed152 ∗ /153 bool fbp ;154 / ∗ ∗155 ∗ middle b u t t o n pushed156 ∗ /157 bool mbp ;158 / ∗ ∗159 ∗ r e a r b u t t o n pushed160 ∗ /161 bool rbp ;162 / ∗ ∗163 ∗ command used i n s t a t e164 ∗ /165 Q S t r i n g comm ;166 / ∗ ∗167 ∗ command s t a t e s t o be a c t i v a t e d168 ∗ /169 Q S t r i n g s t a t e s ;170 / ∗ ∗171 ∗ s t a t e i d172 ∗ /173 i n t i d ;174 / ∗ ∗175 ∗ command c o n s t r u c t i o n u s i n g s t a t e s176 ∗ /177 TVPcommandConstruction ∗ t c c ;178 / ∗ ∗179 ∗ f l a g used t o s t a r t

A.3 Source code of TVPnaocommand.h 41180 ∗ /181 bool i n i t S t r e a m ;182 / ∗ ∗183 ∗ Index used f o r u p d a t e s184 ∗ /185 i n t i n d e x ;186 / ∗ ∗187 ∗ indexMenu used menu f o r u p d a t e s188 ∗ /189 i n t indexMenu ;190 / ∗ ∗191 ∗ L i s t of s e n s o r y e l e m e n t s192 ∗ /193 TVPnaoSensors ∗ ns ;194 / ∗ ∗195 ∗ L i s t of s e n s o r y e l e m e n t s196 ∗ /197 TVPnaoMotion ∗nm ;198 / ∗ ∗199 ∗ l a s t r e c o g n i z e d s e n t e n c e or word ( s )200 ∗ /201 Q S t r i n g l a s t S e n t e n c e ;202 / ∗ ∗203 ∗ l a s t c o n f i d e n c e l e v e l on r e c o g n i z e d s e n t e n c e or word ( s )204 ∗ /205 f l o a t l a s t C o n f i d e n c e L e v e l ;206 / ∗ ∗207 ∗ i n d e x f o r l e f t f o o t bummper a t l e f t s i d e208 ∗ /209 i n t LFootBumperLeft ;210 / ∗ ∗211 ∗ i n d e x f o r l e f t f o o t bummper a t r i g h t s i d e212 ∗ /213 i n t RFootBumperLeft ;214 / ∗ ∗215 ∗ i n d e x f o r r i g h t f o o t bummper a t l e f t s i d e216 ∗ /217 i n t LFootBumperRight ;218 / ∗ ∗219 ∗ i n d e x f o r r i g h t f o o t bummper a t r i g h t s i d e220 ∗ /221 i n t RFootBumperRight ;222 / ∗ ∗223 ∗ i n d e x f o r r e c o g n i z e d words224 ∗ /225 i n t recognizedWords ;226 / ∗ ∗227 ∗ i n d e x f o r s p e e c h d e t e c t e d228 ∗ /229 i n t s p e e c h D e t e c t e d ;230 / ∗ ∗231 ∗ i n d e x f o r a n g l e from gyro i n X−d i r e c t i o n232 ∗ /233 i n t GyrAngleX ;234 / ∗ ∗235 ∗ i n d e x f o r a n g l e from gyro i n Y−d i r e c t i o n236 ∗ /237 i n t GyrAngleY ;238 / ∗ ∗

A.3 Source code of TVPnaocommand.h 42239 ∗ i n d e x f o r head t o u c h f r o n t b u t t o n240 ∗ /241 i n t HeadTouchFront ;242 / ∗ ∗243 ∗ i n d e x f o r head t o u c h r e a r b u t t o n244 ∗ /245 i n t HeadTouchRear ;246 / ∗ ∗247 ∗ i n d e x f o r head t o u c h middle b u t t o n248 ∗ /249 i n t HeadTouchMiddle ;250 / ∗ ∗251 ∗ i n d e x f o r head yaw252 ∗ /253 i n t HeadYaw ;254 / ∗ ∗255 ∗ i n d e x f o r head p i t c h256 ∗ /257 i n t HeadPitch ;258 / ∗ ∗259 ∗ i n d e x f o r l e f t s h o u l d e r p i t c h260 ∗ /261 i n t L S h o u l d e r P i t c h ;262 / ∗ ∗263 ∗ i n d e x f o r l e f t s h o u l d e r r o l l264 ∗ /265 i n t L S h o u l d e r R o l l ;266 / ∗ ∗267 ∗ i n d e x f o r l e f t elbow yaw268 ∗ /269 i n t LElbowYaw ;270 / ∗ ∗271 ∗ i n d e x f o r l e f t elbow r o l l272 ∗ /273 i n t LElbowRoll ;274 / ∗ ∗275 ∗ i n d e x f o r l e f t w r i s t yaw276 ∗ /277 i n t LWristYaw ;278 / ∗ ∗279 ∗ i n d e x f o r r i g h t s h o u l d e r p i t c h280 ∗ /281 i n t R S h o u l d e r P i t c h ;282 / ∗ ∗283 ∗ i n d e x f o r r i g h t s h o u l d e r r o l l284 ∗ /285 i n t R S h o u l d e r R o l l ;286 / ∗ ∗287 ∗ i n d e x f o r r i g h t elbow yaw288 ∗ /289 i n t RElbowYaw ;290 / ∗ ∗291 ∗ i n d e x f o r r i g h t elbow r o l l292 ∗ /293 i n t RElbowRoll ;294 / ∗ ∗295 ∗ i n d e x f o r r i g h t w r i s t yaw296 ∗ /297 i n t RWristYaw ;

A.3 Source code of TVPnaocommand.h 43298 / ∗ ∗299 ∗ i n d e x f o r l e f t h i p yaw p i t c h300 ∗ /301 i n t LHipYawPitch ;302 / ∗ ∗303 ∗ i n d e x f o r l e f t h i p r o l l304 ∗ /305 i n t LHipRoll ;306 / ∗ ∗307 ∗ i n d e x f o r l e f t h i p p i t c h308 ∗ /309 i n t LHipPitch ;310 / ∗ ∗311 ∗ i n d e x f o r l e f t knee p i t c h312 ∗ /313 i n t LKneePitch ;314 / ∗ ∗315 ∗ i n d e x f o r l e f t a n k l e p i t c h316 ∗ /317 i n t LAnklePitch ;318 / ∗ ∗319 ∗ i n d e x f o r l e f t a n k l e r o l l320 ∗ /321 i n t LAnkleRoll ;322 / ∗ ∗323 ∗ i n d e x f o r r i g h t yaw p i t c h324 ∗ /325 i n t RHipYawPitch ;326 / ∗ ∗327 ∗ i n d e x f o r r i g h t h i p r o l l328 ∗ /329 i n t RHipRoll ;330 / ∗ ∗331 ∗ i n d e x f o r r i g h t h i p p i t c h332 ∗ /333 i n t RHipPitch ;334 / ∗ ∗335 ∗ i n d e x f o r r i g h t knee p i t c h336 ∗ /337 i n t RKneePitch ;338 / ∗ ∗339 ∗ i n d e x f o r r i g h t a n k l e p i t c h340 ∗ /341 i n t RAnklePitch ;342 / ∗ ∗343 ∗ i n d e x f o r r i g h t a n k l e r o l l344 ∗ /345 i n t RAnkleRoll ;346 / ∗ ∗347 ∗ s t a r t t ime used i n f i n d i n g a f a c e348 ∗ /349 Time s t a r t T i m e ;350 / ∗ ∗351 ∗ f l a g t o s e t when o b j e c t i s found352 ∗ /353 bool found ;354 / ∗ ∗355 ∗ f l a g t o s e t when r e a d y356 ∗ /

A.4 Source code of TVPnaocommand.cpp 44357 bool f i n i s h e d ;358 } ;359 # e n d i f / ∗ TVPNAOCOMMAND H ∗ /A.4 Source code of TVPnaocommand.cpp1 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗2 ∗ C o p y r i g h t (C) 2010−2011 Tino Lourens . A l l r i g h t s r e s e r v e d .3 ∗4 ∗ P e r m i s s i o n t o use , copy , modify , and d i s t r i b u t e t h i s s o f t w a r e and i t s5 ∗ d o c u m e n t a t i o n under t h e t e r m s of t h e GNU G e n e r a l P u b l i c L i c e n s e i s h e reby6 ∗ g r a n t e d . No r e p r e s e n t a t i o n s a r e made a b o u t t h e s u i t a b i l i t y of t h i s s o f t w a r e7 ∗ f o r any p u r p o s e . I t i s p r o v i d e d ” as i s ” w i t h o u t e x p r e s s e d or i m p l i e d8 ∗ w a r r a n t y . See t h e GNU G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .9 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /10 / ∗ ∗11 ∗ \ f i l e TVPnaocommand . cpp12 ∗ \ b r i e f This f i l e c o n t a i n s r o u t i n e s f o r s t r i n g g e n e r a t i o n .13 ∗ \ a u t h o r Tino Lourens14 ∗ \ d a t e 24 Apr 201015 ∗ \ v e r s i o n 23 Apr 201116 ∗17 ∗ \ h t m l o n l y \ e n d h t m l o n l y18 ∗ This f i l e c o n t a i n s r o u t i n e s f o r s t r i n g g e n e r a t i o n .19 ∗ \ h t m l o n l y \ e n d h t m l o n l y20 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /21 # i n c l u d e 22 # i n c l u d e 2324 / / # d e f i n e TVPNAOCOMMAND DEBUG25 / / # d e f i n e TVPNAOCOMMAND DEBUG12627 / ∗ ∗28 ∗ \ b r i e f TVPnaocommand c o n s t r u c t o r i n i t i a l i z e s .29 ∗ \ a u t h o r Tino Lourens30 ∗ \ d a t e 10 Apr 201031 ∗ \ v e r s i o n 23 Apr 201132 ∗ \ r e t u r n None33 ∗34 ∗ \ h t m l o n l y \ e n d h t m l o n l y35 ∗ TVPnaocommand c o n s t r u c t o r i n i t i a l i z e s .36 ∗ \ h t m l o n l y \ e n d h t m l o n l y37 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /38 TVPnaocommand : : TVPnaocommand ( )39 {40 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)41 qDebug ( )

A.4 Source code of TVPnaocommand.cpp 4552 rbp = f a l s e ;53 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗54 ∗ Setup l i s t of s e n s o r s55 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /56 ns = new TVPnaoSensors ( ) ;57 nm = new TVPnaoMotion ( ) ;58 l a s t C o n f i d e n c e L e v e l = −1.0 f ;59 l a s t S e n t e n c e = ” n o t h i n g ” ;60 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗61 ∗ Get i n d i c e s62 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /63 LFootBumperLeft = ns−>getNameId ( ” LFootBumperLeft ” ) ;64 RFootBumperLeft = ns−>getNameId ( ” RFootBumperLeft ” ) ;65 LFootBumperRight = ns−>getNameId ( ” LFootBumperRight ” ) ;66 RFootBumperRight = ns−>getNameId ( ” RFootBumperRight ” ) ;67 HeadTouchFront = ns−>getNameId ( ” HeadTouchFront ” ) ;68 HeadTouchRear = ns−>getNameId ( ” HeadTouchRear ” ) ;69 HeadTouchMiddle = ns−>getNameId ( ” HeadTouchMiddle ” ) ;70 s p e e c h D e t e c t e d = ns−>getNameId ( ” s p e e c h D e t e c t e d ” ) ;71 recognizedWords = ns−>getNameId ( ” recognizedWords ” ) ;72 GyrAngleX = ns−>getNameId ( ” GyrAngleX ” ) ;73 GyrAngleY = ns−>getNameId ( ” GyrAngleY ” ) ;74 HeadYaw = ns−>getNameId ( ”HeadYaw” ) ;75 HeadPitch = ns−>getNameId ( ” HeadPitch ” ) ;7677 L S h o u l d e r P i t c h = ns−>getNameId ( ” L S h o u l d e r P i t c h ” ) ;78 L S h o u l d e r R o l l = ns−>getNameId ( ” L S h o u l d e r R o l l ” ) ;79 LElbowYaw = ns−>getNameId ( ”LElbowYaw” ) ;80 LElbowRoll = ns−>getNameId ( ” LElbowRoll ” ) ;81 LWristYaw = ns−>getNameId ( ” LWristYaw ” ) ;8283 R S h o u l d e r P i t c h = ns−>getNameId ( ” R S h o u l d e r P i t c h ” ) ;84 R S h o u l d e r R o l l = ns−>getNameId ( ” R S h o u l d e r R o l l ” ) ;85 RElbowYaw = ns−>getNameId ( ”RElbowYaw” ) ;86 RElbowRoll = ns−>getNameId ( ” RElbowRoll ” ) ;87 RWristYaw = ns−>getNameId ( ” RWristYaw ” ) ;8889 LHipYawPitch = ns−>getNameId ( ” LHipYawPitch ” ) ;90 LHipRoll = ns−>getNameId ( ” LHipRoll ” ) ;91 LHipPitch = ns−>getNameId ( ” LHipPitch ” ) ;92 LKneePitch = ns−>getNameId ( ” LKneePitch ” ) ;93 LAnklePitch = ns−>getNameId ( ” LAnklePitch ” ) ;94 LAnkleRoll = ns−>getNameId ( ” LAnkleRoll ” ) ;9596 RHipYawPitch = ns−>getNameId ( ” RHipYawPitch ” ) ;97 RHipRoll = ns−>getNameId ( ” RHipRoll ” ) ;98 RHipPitch = ns−>getNameId ( ” RHipPitch ” ) ;99 RKneePitch = ns−>getNameId ( ” RKneePitch ” ) ;100 RAnklePitch = ns−>getNameId ( ” RAnklePitch ” ) ;101 RAnkleRoll = ns−>getNameId ( ” RAnkleRoll ” ) ;102 }103104 / ∗ ∗105 ∗ \ b r i e f ˜ TVPnaocommand d e s t r u c t o r f r e e d a t a .106 ∗ \ a u t h o r Tino Lourens107 ∗ \ d a t e 10 Apr 2010108 ∗ \ v e r s i o n 08 May 2010109 ∗ \ r e t u r n None110 ∗

A.4 Source code of TVPnaocommand.cpp 46111 ∗ \ h t m l o n l y \ e n d h t m l o n l y112 ∗ ˜ TVPnaocommand d e s t r u c t o r f r e e d a t a .113 ∗ \ h t m l o n l y \ e n d h t m l o n l y114 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /115 TVPnaocommand : : ˜ TVPnaocommand ( )116 {117 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)118 qDebug ( )

A.4 Source code of TVPnaocommand.cpp 47170 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗171 ∗ e v a l u a t e i n p u t d a t a172 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /173 i f ( ( d . d a t a t y p e == DATA5D FLOAT) &&174 ( ( d .w == ns−>s i z e ( ) ) | | ( d .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l u e s175 ( d . h == 1) && ( d . d == 1) && ( d . t == 1) && ( d . e == 1) )176 {177 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗178 ∗ d a t a i s s e n s o r y d a t a179 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /180 f l o a t ∗ d a t a = ( f l o a t ∗) d . d a t a ;181 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗182 ∗ e v a l u a t e r e c o g n i z e d s e n t e n c e183 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /184 Q S t r i n g r s s = ” ” ;185 f o r ( i n t i = 0 ; i < r s . l e n ; i ++)186 r s s += r s . s [ i ] ;187 i f ( d a t a [ s p e e c h D e t e c t e d ] > 0 . 5 ) / / no s peech = 0 ; s peech = 1188 i f ( d a t a [ recognizedWords ] > 0 . 2 ) / / c o n f i d e n c e l e v e l 0−1189 {190 / ∗191 c o u t 0 . 5 f )224 {225 i f ( s . l e n g t h ( ) > 0)226 s += ”&” ;227 s += ” say ( L e f t f o o t l e f t ) ” ;228 }

A.4 Source code of TVPnaocommand.cpp 48229 e l s e230 i f ( d a t a [ 1 ] > 0 . 5 f )231 {232 i f ( s . l e n g t h ( ) > 0)233 s += ”&” ;234 s += ” say ( L e f t f o o t r i g h t ) ” ;235 }236 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗237 ∗ 2 = RFootBumperLeft v a l u e i s 0 or 1 ( pushed )238 ∗ 3 = RFootBumperRight v a l u e i s 0 or 1 ( pushed )239 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /240 i f ( ( d a t a [ RFootBumperLeft ] > 0 . 5 f ) &&241 ( d a t a [ RFootBumperRight ] > 0 . 5 f ) )242 {243 i f ( s . l e n g t h ( ) > 0)244 s += ”&” ;245 s += ” [ say ( R i g h t f o o t ) ” ;246 s += ” | ” ;247 s += ”move ( HeadYaw , −35, 500) ] ” ;248 }249 e l s e250 i f ( d a t a [ 2 ] > 0 . 5 f )251 {252 i f ( s . l e n g t h ( ) > 0)253 s += ”&” ;254 s += ” say ( R i g h t f o o t l e f t ) ” ;255 }256 e l s e257 i f ( d a t a [ 3 ] > 0 . 5 f )258 {259 i f ( s . l e n g t h ( ) > 0)260 s += ”&” ;261 s += ” say ( R i g h t f o o t r i g h t ) ” ;262 }263 }264 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗265 ∗ o u t p u t r e s u l t i n g s t r i n g266 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /267 S t r i n g s t r ;268 s t r . l e n = s . l e n g t h ( ) ;269 i f ( s t r . l e n > 0)270 {271 s t r . s = new c h a r [ s t r . l e n ] ;272 s t r i n g s s = s . t o S t d S t r i n g ( ) ;273 f o r ( i n t i = 0 ; i < s t r . l e n ; i ++)274 s t r . s [ i ] = s s [ i ] ;275 }276 e l s e277 s t r . s = NULL;278 r e t u r n s t r ;279 }280281 / ∗ ∗282 ∗ \ b r i e f TVPwalkArc walks an a r c or s e t s up b e h a v i o r t o walk .283 ∗ \ a u t h o r Tino Lourens284 ∗ \ d a t e 05 Jun 2010285 ∗ \ v e r s i o n 05 Jun 2010286 ∗ \ param d i n p u t s e n s o r y d a t a287 ∗ \ param a n g l e a n g l e i n d e g r e e s

A.4 Source code of TVPnaocommand.cpp 49288 ∗ \ param r a d i u s r a d i u s i n m e t e r s289 ∗ \ param d u r a t i o n time t o perform a r c g i v e n i t s a n g l e and r a d i u s290 ∗ \ r e t u r n S t r i n g t h a t p e r f o r m s an a c t i o n291 ∗292 ∗ \ h t m l o n l y \ e n d h t m l o n l y293 ∗ TVPwalkArc walks an a r c or s e t s up b e h a v i o r t o walk .294 ∗ \ h t m l o n l y \ e n d h t m l o n l y295 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /296 Q S t r i n g TVPnaocommand : : TVPwalkArc ( Data5D d ,297 f l o a t angle ,298 f l o a t r a d i u s ,299 f l o a t d u r a t i o n )300 {301 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)302 qDebug ( )

A.4 Source code of TVPnaocommand.cpp 50347 # e n d i f348 / / v1 . 6 . 1 3 body pose349 / ∗350 # g e t v a l u e Torso Angle Y & X from Memory t o check i f t h e r o b o t i s on t h eb e l l y or on t h e back351352 rAngleX , rAngleY = ALMemory . g e t L i s t D a t a ( [ ” Device / S u b D e v i c e L i s t /I n e r t i a l S e n s o r / AngleX / Sensor / Value ” , ” Device / S u b D e v i c e L i s t /I n e r t i a l S e n s o r / AngleY / Sensor / Value ” ] ) ;353354 s e l f . d e b u g P r i n t ( ” D e t e c t P o s i t i o n : rAngleX : %5.3 f ; rAngleY : %5.3 f ” % (rAngleX , rAngleY ) ) ;355356 r J o i n t V a l H i p P i t c h = ( ALMotion . g e t A n g l e s ( ” LHipPitch ” , True ) [ 0 ] +ALMotion . g e t A n g l e s ( ” RHipPitch ” , True ) [ 0 ] ) / 2 . ;357 r J o i n t V a l K n e e P i t c h = ( ALMotion . g e t A n g l e s ( ” LKneePitch ” , True ) [ 0 ] +ALMotion . g e t A n g l e s ( ” RKneePitch ” , True ) [ 0 ] ) / 2 . ;358 r J o i n t V a l A n k l e P i t c h = ( ALMotion . g e t A n g l e s ( ” LAnklePitch ” , True ) [ 0 ] +ALMotion . g e t A n g l e s ( ” RAnklePitch ” , True ) [ 0 ] ) / 2 . ;359360 i f ( r J o i n t V a l H i p P i t c h > −0.9 and r J o i n t V a l K n e e P i t c h > 1 . 2 andr J o i n t V a l A n k l e P i t c h > 0 . 3 and rAngleY < 1 . 2 ) :361 s e l f . d e b u g P r i n t ( ” o u t p u t k n e e l i n g ” ) ;362 s e l f . o u t p u t k n e e l i n g ( ) ;363 r e t u r n ;364365 i f rAngleY > −0.82 and rAngleY < 0 . 9 and abs ( rAngleX ) < 0 . 5 :366 s e l f . d e b u g P r i n t ( ” t h e t o r s o i s v e r t i c a l ” ) ;367368 s t r S p e c i f J o i n t N a m e = ” LHipRoll ” ;369 r J o i n t V a l H i p R o l l = ALMotion . g e t A n g l e s ( s t r S p e c i f J o i n t N a m e , True ) [ 0 ] ;370 s t r S p e c i f J o i n t N a m e = ” LHipYawPitch ” ;371 r J o i n t V a l L H i p Y a w P i t c h = ALMotion . g e t A n g l e s ( s t r S p e c i f J o i n t N a m e , True )[ 0 ] ;372373 s e l f . d e b u g P r i n t ( ” D e t e c t P o s i t i o n : H i p P i t c h : %5.3 f ; KneePitch : %5.3 f ;A n k l e P i t c h : %5.3 f ; LHipRoll : %5.3 f ; LHipYawPitch : %5.3 f ” % (r J o i n t V a l H i p P i t c h , r J o i n t V a l K n e e P i t c h , r J o i n t V a l A n k l e P i t c h ,r J o i n t V a l H i p R o l l , r J o i n t V a l L H i p Y a w P i t c h ) ) ;374 i f ( r J o i n t V a l H i p P i t c h < −0.45 and r J o i n t V a l K n e e P i t c h > 1 . 8 andr J o i n t V a l A n k l e P i t c h < −0.7 ) :375 s e l f . d e b u g P r i n t ( ” o u t p u t c r o u c h ” ) ;376 s e l f . o u t p u t c r o u c h ( ) ;377 e l i f ( r J o i n t V a l H i p P i t c h < −1.2 or ( r J o i n t V a l H i p P i t c h < −0.8 andr J o i n t V a l K n e e P i t c h < 0 . 5 ) ) :378 s e l f . d e b u g P r i n t ( ” o u t p u t s i t ” ) ;379 s e l f . o u t p u t s i t ( ) ;380 e l i f ( r J o i n t V a l H i p P i t c h < 0 . 5 and r J o i n t V a l K n e e P i t c h < 1 . 1 andr J o i n t V a l A n k l e P i t c h < 0 . 1 and r J o i n t V a l H i p R o l l < 0 . 5 ) : # Alma :a v a n t l e h i p p i t c h e t a t a 0 . 3 mais p o u r q u o i pas 0 . 5 ? idem pourr J o i n t V a l H i p R o l l ( pour p o s i t i o n avec une jambe s u r l e c o t )381 s e l f . d e b u g P r i n t ( ” o u t p u t s t a n d i n g ” ) ;382 s e l f . o u t p u t s t a n d i n g ( ) ;383 e l i f ( r J o i n t V a l H i p P i t c h < −1.2 and r J o i n t V a l K n e e P i t c h > 1 . 1 ) :384 # p r e d e f i n e d p o s i t i o n : ” i n t e r m e d i a r y ” => s t a n d , s i t or l y i n g (a l r e a d y done i n t h e h o r i z o n t a l c a s e )385 i f ( rAngleY > −0.7 and rAngleY < 0 . 1 ) :386 s e l f . d e b u g P r i n t ( ” o u t p u t s i t 2 ” ) ;387 s e l f . o u t p u t s i t ( ) ;

A.4 Source code of TVPnaocommand.cpp 51388 e l i f ( rAngleY > 0 . 4 ) :389 s e l f . d e b u g P r i n t ( ” o u t p u t s t a n d i n g 2 ” ) ;390 s e l f . o u t p u t s t a n d i n g ( ) ;391 e l s e :392 s e l f . d e b u g P r i n t ( ” output unknown1 ” ) ;393 s e l f . output unknown ( ) ;394 e l s e :395 s e l f . d e b u g P r i n t ( ” output unknown2 ” ) ;396 s e l f . output unknown ( ) ;397 e l s e :398 s e l f . d e b u g P r i n t ( ” t h e t o r s o i s h o r i z o n t a l ” ) ;399 s e l f . d e b u g P r i n t ( ” D e t e c t P o s i t i o n : H i p P i t c h : %5.3 f ; KneePitch : %5.3 f ” %( r J o i n t V a l H i p P i t c h , r J o i n t V a l K n e e P i t c h ) ) ;400 rAngleX Side = 1 . 3 ;401 rErrorMax = 0 . 3 ;402 i f ( rAngleY > 1 . 0 ) :403 s e l f . d e b u g P r i n t ( ” o u t p u t b e l l y ” ) ;404 s e l f . o u t p u t b e l l y ( ) ;405 e l i f ( rAngleY < −0.85 ) :406 s e l f . d e b u g P r i n t ( ” o u t p u t b a c k ” ) ;407 s e l f . o u t p u t b a c k ( ) ;408 e l i f ( rAngleY > −0.6 and rAngleY < 0 . 6 and409 (410 ( rAngleX > rAngleX Side−rErrorMax and rAngleX −(rAngleX Side + rErrorMax ) )412 )413 ) :414 # on t h e r i g h t s i d e = ( 0 . 0 , 1 . 3 5 ) , on t h e l e f t s i d e : ( 0 . 0 , −1.30)415 i f ( rAngleX > 0 ) :416 s e l f . d e b u g P r i n t ( ” o u t p u t s i d e r i g h t ” ) ;417 s e l f . o u t p u t s i d e r i g h t ( ) ;418 e l s e :419 s e l f . d e b u g P r i n t ( ” o u t p u t s i d e l e f t ” ) ;420 s e l f . o u t p u t s i d e l e f t ( ) ;421 e l s e :422 s e l f . d e b u g P r i n t ( ” output unknown3 ” ) ;423 s e l f . output unknown ( ) ;424 ∗ /425 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗426 ∗ e v a l u a t e i n p u t d a t a427 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /428 i f ( ( d . d a t a t y p e == DATA5D FLOAT) &&429 ( ( d .w == ns−>s i z e ( ) ) | | ( d .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l u e s430 ( d . h == 1) && ( d . d == 1) && ( d . t == 1) && ( d . e == 1) )431 {432 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗433 ∗ d a t a i s s e n s o r y d a t a434 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /435 f l o a t ∗ d a t a = ( f l o a t ∗) d . d a t a ;436 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗437 ∗ Get r o b o t i n f o438 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /439 f l o a t angleX = d a t a [ GyrAngleX ] ;440 f l o a t angleY = d a t a [ GyrAngleY ] ;441 f l o a t l h i p p i t c h = d a t a [ LHipPitch ] ;442 f l o a t l k n e e p i t c h = d a t a [ LKneePitch ] ;443 f l o a t l a n k l e p i t c h = d a t a [ LAnklePitch ] ;

A.4 Source code of TVPnaocommand.cpp 52444 f l o a t r h i p p i t c h = d a t a [ RHipPitch ] ;445 f l o a t r k n e e p i t c h = d a t a [ RKneePitch ] ;446 f l o a t r a n k l e p i t c h = d a t a [ RAnklePitch ] ;447 f l o a t h i p p i t c h = ( r h i p p i t c h + l h i p p i t c h ) / 2 . 0 f ;448 f l o a t k n e e p i t c h = ( r k n e e p i t c h + l k n e e p i t c h ) / 2 . 0 f ;449 f l o a t a n k l e p i t c h = ( r a n k l e p i t c h + l a n k l e p i t c h ) / 2 . 0 f ;450 f l o a t h i p r o l l = d a t a [ LHipRoll ] ;451 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)452 c o u t

A.4 Source code of TVPnaocommand.cpp 53503 i f ( ( f a b s ( angleY ) < 0 . 6 ) &&504 ( ( ( angleX > 1 . 0 ) && ( angleX > 1 . 6 ) ) | |505 ( ( angleX < −1.0) && ( angleX > −1.6) ) ) )506 {507 i f ( angleX > 0)508 r e t u r n 7 ; / / R i g h t s i d e509 r e t u r n 6 ; / / L e f t s i d e510 }511 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)512 c o u t

A.4 Source code of TVPnaocommand.cpp 54562 c a s e 5 : / / B e l l y563 c a s e 8 : / / Frog564 c = nm−>b e l l y T o S t a n d ( ) ;565 b r e a k ;566 c a s e 4 : / / S i t567 c = nm−>s i t T o C r o u c h ( ) ;568 b r e a k ;569 c a s e 6 : / / L e f t s i d e570 c a s e 7 : / / R i g h t s i d e571 c = nm−>s i d e T o B e l l y ( ) ;572 b r e a k ;573 d e f a u l t : / / −1 Unknown574 r e t u r n ” ” ;575 }576 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗577 ∗ Wait a second a f t e r t h e a c t i o n t o e n s u r e a p r o p e r r o b o t pose578 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /579 c += ”& w a i t ( 1 0 0 0 ) ” ;580 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗581 ∗ R e t u r n command582 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /583 r e t u r n c ;584 }585586 / ∗ ∗587 ∗ \ b r i e f TVPsitdown l e t s t h e r o b o t s i t down588 ∗ \ a u t h o r Tino Lourens589 ∗ \ d a t e 05 Jun 2010590 ∗ \ v e r s i o n 23 Apr 2011591 ∗ \ param d i n p u t s e n s o r y d a t a592 ∗ \ r e t u r n S t r i n g t h a t p e r f o r m s a c t i o n t o s t a n d up593 ∗594 ∗ \ h t m l o n l y \ e n d h t m l o n l y595 ∗ TVPsitdown l e t s t h e r o b o t s i t down596 ∗ \ h t m l o n l y \ e n d h t m l o n l y597 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /598 Q S t r i n g TVPnaocommand : : TVPsitdown ( Data5D d )599 {600 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)601 qDebug ( )

A.4 Source code of TVPnaocommand.cpp 55621 c = nm−>c r o u c h T o S i t ( ) ;622 b r e a k ;623 c a s e 4 : / / S i t624 r e t u r n ” ” ;625 b r e a k ;626 c a s e 3 : / / Kneel627 c a s e 5 : / / B e l l y628 c a s e 8 : / / Frog629 c = nm−>b ellyToCrouch ( ) ;630 b r e a k ;631 c a s e 6 : / / L e f t s i d e632 c a s e 7 : / / R i g h t s i d e633 c = nm−>s i d e T o B e l l y ( ) ;634 b r e a k ;635 d e f a u l t : / / −1 Unknown636 r e t u r n ” ” ;637 }638 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗639 ∗ Wait a second a f t e r t h e a c t i o n t o e n s u r e a p r o p e r r o b o t pose640 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /641 c += ”& w a i t ( 1 0 0 0 ) ” ;642 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗643 ∗ R e t u r n command644 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /645 r e t u r n c ;646 }647648 / ∗ ∗649 ∗ \ b r i e f TVPheadmenu p e r f o r m s head menu demo .650 ∗ \ a u t h o r S t i j n Boere and Tino Lourens651 ∗ \ d a t e 10 Apr 2010652 ∗ \ v e r s i o n 09 Apr 2011653 ∗ \ param d i n p u t nao s e n s o r y d a t a .654 ∗ \ r e t u r n command s t r i n g655 ∗656 ∗ \ h t m l o n l y \ e n d h t m l o n l y657 ∗ TVPheadmenu p e r f o r m s head menu demo .658 ∗ \ h t m l o n l y \ e n d h t m l o n l y659 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /660 Q S t r i n g TVPnaocommand : : TVPheadmenu ( Data5D d )661 {662 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)663 qDebug ( ) s i z e ( ) − 2) ) && / / 2 s o n a r v a l u e s679 ( d . h == 1) && ( d . d == 1) && ( d . t == 1) && ( d . e == 1) )

A.4 Source code of TVPnaocommand.cpp 56680 {681 f l o a t ∗ d a t a = ( f l o a t ∗) d . d a t a ;682 f l o a t PressHeadTouchFront = d a t a [ HeadTouchFront ] ;683 f l o a t PressHeadTouchRear = d a t a [ HeadTouchRear ] ;684 f l o a t PressHeadTouchMiddle = d a t a [ HeadTouchMiddle ] ;685 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗686 ∗ F r o n t b u t t o n pushed687 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /688 i f ( ( PressHeadTouchFront > 0 . 5 f ) &&689 ( PressHeadTouchRear < 0 . 5 f ) &&690 ( PressHeadTouchMiddle < 0 . 5 f ) )691 {692 indexMenu ++;693 i f ( indexMenu == nm−>TVPposeElements ( ) )694 indexMenu = 0 ;695 s = ” say ( ” + nm−>TVPposeText ( indexMenu ) + ” ) ” ;696 }697 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗698 ∗ Rear b u t t o n pushed699 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /700 i f ( ( PressHeadTouchRear > 0 . 5 f ) &&701 ( PressHeadTouchFront < 0 . 5 f ) &&702 ( PressHeadTouchMiddle < 0 . 5 f ) )703 {704 i f ( indexMenu < 0)705 indexMenu = 0 ;706 e l s e707 indexMenu −−;708 i f ( indexMenu < 0)709 indexMenu = nm−>TVPposeElements ( ) − 1 ;710 s = ” say ( ” + nm−>TVPposeText ( indexMenu ) + ” ) ” ;711 }712 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗713 ∗ Middle b u t t o n pushed714 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /715 i f ( ( PressHeadTouchMiddle > 0 . 5 f ) &&716 ( PressHeadTouchRear < 0 . 5 f ) &&717 ( PressHeadTouchFront < 0 . 5 f ) )718 {719 i f ( indexMenu < 0)720 indexMenu = 0 ;721 TVPstringgen t s g ;722 Q S t r i n g s t i f f = nm−>T V P s t i f f n e s s ( 1 . 0 , 500) ;723 s = ” say (OK) &” ;724 s += t s g . T V P s e r i a l ( s t i f f , nm−>TVPpose ( indexMenu ) ) ;725 }726 }727 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗728 ∗ o u t p u t r e s u l t i n g s t r i n g729 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /730 r e t u r n s ;731 }732733 / ∗ ∗734 ∗ \ b r i e f commandStateStringHeadmenu headmenu demo .735 ∗ \ a u t h o r S t i j n Boere and Tino Lourens736 ∗ \ d a t e 23 Apr 2011737 ∗ \ v e r s i o n 23 Apr 2011738 ∗ \ param i s t a t e s i n p u t s t a t e s

A.4 Source code of TVPnaocommand.cpp 57739 ∗ \ param icommands i n p u t commands740 ∗ \ param i d o f f s e t i d o f f s e t of s t a t e741 ∗ \ param o s t a t e s o u t p u t s t a t e s742 ∗ \ param ocommands o u t p u t commands743 ∗ \ param d i n p u t nao s e n s o r y d a t a744 ∗ \ r e t u r n none745 ∗746 ∗ \ h t m l o n l y \ e n d h t m l o n l y747 ∗ commandStateStringHeadmenu headmenu demo .748 ∗ \ h t m l o n l y \ e n d h t m l o n l y749 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /750 void TVPnaocommand : : commandStateStringHeadmenu ( Data5D i s t a t e s ,751 S t r i n g icommands ,752 i n t i d o f f s e t ,753 Data5D &o s t a t e s ,754 S t r i n g &ocommands ,755 Data5D d )756 {757 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)758 qDebug ( ) s i z e ( ) − 2) ) && / / 2 s o n a r v a l .792 ( d . h == 1) && ( d . d == 1) && ( d . t == 1) && ( d . e == 1) )793 {794 f l o a t ∗ d a t a = ( f l o a t ∗) d . d a t a ;795 f l o a t PressHeadTouchFront = d a t a [ HeadTouchFront ] ;796 f l o a t PressHeadTouchRear = d a t a [ HeadTouchRear ] ;797 f l o a t PressHeadTouchMiddle = d a t a [ HeadTouchMiddle ] ;

A.4 Source code of TVPnaocommand.cpp 58798 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗799 ∗ F r o n t b u t t o n pushed800 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /801 i f ( ( PressHeadTouchFront > 0 . 5 f ) &&802 ( PressHeadTouchRear < 0 . 5 f ) &&803 ( PressHeadTouchMiddle < 0 . 5 f ) )804 fbp = t r u e ;805 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗806 ∗ Rear b u t t o n pushed807 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /808 i f ( ( PressHeadTouchRear > 0 . 5 f ) &&809 ( PressHeadTouchFront < 0 . 5 f ) &&810 ( PressHeadTouchMiddle < 0 . 5 f ) )811 rbp = t r u e ;812 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗813 ∗ Middle b u t t o n pushed814 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /815 i f ( ( PressHeadTouchMiddle > 0 . 5 f ) &&816 ( PressHeadTouchRear < 0 . 5 f ) &&817 ( PressHeadTouchFront < 0 . 5 f ) )818 mbp = t r u e ;819 }820 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗821 ∗ C r e a t e an i n i t phase822 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /823 i f ( i n i t S t r e a m )824 {825 t c c = new TVPcommandConstruction ( ) ;826 i d = i d o f f s e t + 1 ; / / s e t u p s t a r t i n g s t a t e f o r head menu827 comm = ” p l a y ( / home / nao / customsound / d r u k t o e t s e m o t i e . mp3 , 100) ” ;828 i n i t S t r e a m = f a l s e ;829 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d + 1) ; / / a c t i v a t e n e x t s t a t e830 }831 e l s e832 {833 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗834 ∗ we have a dynamic i d ; so g e t f i r s t a c t i v e s t a t e i n t h e r a n g e835 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /836 i n t l b = i d o f f s e t + 1 ;837 i n t ub = i d o f f s e t + 2 + 4 ∗ nm−>TVPposeElements ( ) ;838 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗839 ∗ g e t l i s t of s t a t e s i n e x e c u t i o n ( = 0 )840 ∗ −t a k e t h e s t a t e s used i n t h i s r o u t i n e r a n g i n g from l b t o ub−1841 ∗ −i n c a s e t h e r e a r e a c t i v e s t a t e s ( by i d ) t a k e t h e f i r s t one .842 ∗ − i f no a c t i v e s t a t e use t h e f i r s t i d = l b843 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /844 QList ∗ a s t = t c c −>g e t S t a t e s ( i s t a t e s , lb , ub , 0) ;845 i f ( a s t −>s i z e ( ) > 0)846 i d = a s t −>a t ( 0 ) ;847 e l s e848 i d = l b ;849 d e l e t e a s t ;850 }851 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗852 ∗ l e t s e v a l u a t e what t o do853 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /854 i n t i n d e x = i d − i d o f f s e t − 2 ;855 i f ( i n d e x >= 0) / / a l l e x c e p t s t a r t856 {

A.4 Source code of TVPnaocommand.cpp 59857 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗858 ∗ i n d e x%4 == 1 , 2 , or 3 a r e c aused by p r e s s i n g t h e head b u t t o n s .859 ∗ Once i n such a s t a t e one s h o u l d b r i n g i t back t o a s t a t e where860 ∗ a b u t t o n a c t i o n can be performed a g a i n .861 ∗ −b u t t o n s t o d e f a u l t s t a t e ( f a l s e )862 ∗ −command = ””863 ∗ −b r i n g s t a t e depending on b u t t o n t o864 ∗ ’ l i d ’ middle865 ∗ ’ l i d −1’ r e a r866 ∗ ’ l i d +1 ’ f r o n t b u t t o n .867 ∗ Note i d −> i n d e x =id−i d o f f s e t −2 −> l i d = i n d e x / 4 ( and i n d e x %4==0)868 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /869 i f ( i n d e x % 4 == 1)870 {871 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗872 ∗ F r o n t B u t t o n p r e s s e d a c t i o n has been e x e c u t e d873 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /874 fbp = f a l s e ;875 rbp = f a l s e ;876 mbp = f a l s e ;877 comm = ” ” ;878 i f ( ( i n d e x / 4) == (nm−>TVPposeElements ( ) − 1) )879 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d o f f s e t +2) ; / / menu e l e m e n t 0880 e l s e881 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d +3) ; / / goto menu e l e m e n t i +1882 }883 e l s e884 i f ( i n d e x % 4 == 2)885 {886 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗887 ∗ Middle B u t t o n p r e s s e d a c t i o n has been e x e c u t e d888 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /889 fbp = f a l s e ;890 rbp = f a l s e ;891 mbp = f a l s e ;892 comm = ” ” ;893 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( id −2) ; / / s t a y a t menu e l e m e n t i894 }895 e l s e896 i f ( i n d e x % 4 == 3)897 {898 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗899 ∗ Rear B u t t o n p r e s s e d a c t i o n has been e x e c u t e d900 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /901 fbp = f a l s e ;902 rbp = f a l s e ;903 mbp = f a l s e ;904 comm = ” ” ;905 i f ( i n d e x / 4 == 0)906 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d o f f s e t +2+ / / menu e l e m e n t n−1907 4 ∗ (nm−>TVPposeElements ( ) − 1) ) ;908 e l s e909 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( id −7) ; / / goto menu e l e m e n t i −1910 }911 e l s e / / ( i n d e x % 4 == 0)912 {913 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗914 ∗ Ready f o r a c t i o n s t a t e ; f o r a g i v e n menu i n d e x c a l l e d ’ l i d ’915 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /

A.4 Source code of TVPnaocommand.cpp 60916 i n t l i d = i n d e x / 4 ;917 i f ( mbp )918 {919 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗920 ∗ a c t i o n r e q u i r e d middle b u t t o n was p r e s s e d921 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /922 TVPstringgen t s g ;923 Q S t r i n g say = ” say (OK) ” ;924 comm = t s g . T V P p a r a l l e l ( say , nm−>TVPpose ( l i d ) ) ;925 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d +2) ; / / MB +=2926 }927 e l s e928 i f ( fbp )929 {930 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗931 ∗ a c t i o n r e q u i r e d f r o n t b u t t o n was p r e s s e d932 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /933 i f ( l i d == nm−>TVPposeElements ( ) − 1)934 l i d = 0 ;935 e l s e936 l i d ++;937 comm = ” say ( ” + nm−>TVPposeText ( l i d ) + ” ) ” ;938 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d +1) ; / / FB +=1939 }940 e l s e941 i f ( rbp )942 {943 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗944 ∗ a c t i o n r e q u i r e d r e a r b u t t o n was p r e s s e d945 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /946 i f ( l i d == 0)947 l i d = nm−>TVPposeElements ( ) − 1 ;948 e l s e949 l i d −−;950 comm = ” say ( ” + nm−>TVPposeText ( l i d ) + ” ) ” ;951 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d +3) ; / / RB +=3952 }953 e l s e954 {955 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗956 ∗ n o t h i n g going on keep t h i s module a c t i v e by r e a c t i v a t i n g i d957 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /958 comm = ” ” ;959 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d ) ; / / keep same s t a t e a c t i v e960 }961 }962 }963 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗964 ∗ C r e a t e s t a t e s t r i n g965 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /966 t c c −>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , id , comm , s t a t e s , o s t a t e s ,967 ocommands ) ;968 }969970 / ∗ ∗971 ∗ \ b r i e f c o m m a n d S t a t e S t r i n g S t a n d u p module t h a t makes t h e r o b o t s t a n d u p972 ∗ \ a u t h o r Tino Lourens973 ∗ \ d a t e 23 Apr 2011974 ∗ \ v e r s i o n 23 Apr 2011

A.4 Source code of TVPnaocommand.cpp 61975 ∗ \ param i s t a t e s i n p u t s t a t e s976 ∗ \ param icommands i n p u t commands977 ∗ \ param i d s t a r t i d of s t a r t i n g s t a t e978 ∗ \ param i d r e a d y s t a t e ( s ) t o a c t i v a t e when r e a d y979 ∗ \ param o s t a t e s o u t p u t s t a t e s980 ∗ \ param ocommands o u t p u t commands981 ∗ \ param d i n p u t nao s e n s o r y d a t a982 ∗ \ r e t u r n none983 ∗984 ∗ \ h t m l o n l y \ e n d h t m l o n l y985 ∗ c o m m a n d S t a t e S t r i n g S t a n d u p module t h a t makes t h e r o b o t s t a n d u p986 ∗ \ h t m l o n l y \ e n d h t m l o n l y987 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /988 void TVPnaocommand : : c o m m a n d S t a t e S t r i n g S t a n d u p ( Data5D i s t a t e s ,989 S t r i n g icommands ,990 i n t i d s t a r t ,991 Q S t r i n g i d r e a d y ,992 Data5D &o s t a t e s ,993 S t r i n g &ocommands ,994 Data5D d )995 {996 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)997 qDebug ( )

A.4 Source code of TVPnaocommand.cpp 621034 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d r e a d y ) ; / / we a r e done1035 e l s e1036 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d s t a r t ) ; / / a c t i v a t e t h e same1037 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1038 ∗ C r e a t e s t a t e s t r i n g1039 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1040 t c c −>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , id , comm , s t a t e s , o s t a t e s ,1041 ocommands ) ;1042 }10431044 / ∗ ∗1045 ∗ \ b r i e f c o m m a n d S t a t e S t r i n g S i t d o w n module t h a t makes t h e r o b o t s i t d o w n1046 ∗ \ a u t h o r Tino Lourens1047 ∗ \ d a t e 23 Apr 20111048 ∗ \ v e r s i o n 23 Apr 20111049 ∗ \ param i s t a t e s i n p u t s t a t e s1050 ∗ \ param icommands i n p u t commands1051 ∗ \ param i d s t a r t i d of s t a r t i n g s t a t e1052 ∗ \ param i d r e a d y s t a t e ( s ) t o a c t i v a t e when r e a d y1053 ∗ \ param o s t a t e s o u t p u t s t a t e s1054 ∗ \ param ocommands o u t p u t commands1055 ∗ \ param d i n p u t nao s e n s o r y d a t a1056 ∗ \ r e t u r n none1057 ∗1058 ∗ \ h t m l o n l y \ e n d h t m l o n l y1059 ∗ c o m m a n d S t a t e S t r i n g S i t d o w n module t h a t makes t h e r o b o t s i t d o w n1060 ∗ \ h t m l o n l y \ e n d h t m l o n l y1061 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1062 void TVPnaocommand : : c o m m a n d S t a t e S t r i n g S i t d o w n ( Data5D i s t a t e s ,1063 S t r i n g icommands ,1064 i n t i d s t a r t ,1065 Q S t r i n g i d r e a d y ,1066 Data5D &o s t a t e s ,1067 S t r i n g &ocommands ,1068 Data5D d )1069 {1070 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)1071 qDebug ( )

A.4 Source code of TVPnaocommand.cpp 631093 t c c = new TVPcommandConstruction ( ) ;1094 i d = i d s t a r t ;1095 comm = ” ” ;1096 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d ) ; / / a c t i v a t e i t s e l f1097 i n i t S t r e a m = f a l s e ;1098 }1099 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1100 ∗ L e t s e v a l u a t e what t o do1101 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1102 comm = TVPsitdown ( d ) ;1103 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1104 ∗ The outcome i s empty when an unknown s e t t i n g i s e n c o u n t e r e d or1105 ∗ we have r e a c h e d our g o a l1106 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1107 i f (comm . l e n g t h ( ) == 0)1108 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d r e a d y ) ; / / we a r e done1109 e l s e1110 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d s t a r t ) ; / / a c t i v a t e t h e same1111 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1112 ∗ C r e a t e s t a t e s t r i n g1113 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1114 t c c −>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , id , comm , s t a t e s , o s t a t e s ,1115 ocommands ) ;1116 }11171118 / ∗ ∗1119 ∗ \ b r i e f c o m m a n d S t a t e S t r i n g L o o k F o r O b j e c t g e n e r a t e s a command and u p d a t e s1120 ∗ i t s s t a t e u s i n g o b j e c t s .1121 ∗ \ a u t h o r Tino Lourens1122 ∗ \ d a t e 11 Jun 20111123 ∗ \ v e r s i o n 11 Jun 20111124 ∗ \ param i s t a t e s i n p u t s t a t e s1125 ∗ \ param icommands i n p u t commands1126 ∗ \ param i d s t a r t s t a r t i n g i d1127 ∗ \ param a s t a t e s s t a t e s t o be a c t i v a t e d when command i s e x e c u t e d1128 ∗ \ param o s t a t e s o u t p u t s t a t e s1129 ∗ \ param ocommands o u t p u t commands1130 ∗ \ param d i n p u t o b j e c t a r e a s x1 , x2 , y1 , y2 , w, h1131 ∗ \ param s i n p u t nao s e n s o r y d a t a1132 ∗ \ param maxTime maximum time t o f i n d a o b j e c t i n m i l l i s e c o n d s1133 ∗ \ r e t u r n none1134 ∗1135 ∗ \ h t m l o n l y \ e n d h t m l o n l y1136 ∗ c o m m a n d S t a t e S t r i n g L o o k F o r O b j e c t g e n e r a t e s a command and u p d a t e s1137 ∗ i t s s t a t e u s i n g o b j e c t s .1138 ∗1139 ∗ Module moves head i n 9 d i f f e r e n t p o s i t i o n s t o s e a r c h f o r an o b j e c t .1140 ∗ I t e i t h e r f i n d s t h e o b j e c t or when t h e s e a r c h time has e x p i r e d i t f a i l s .1141 ∗ \ h t m l o n l y \ e n d h t m l o n l y1142 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1143 void TVPnaocommand : : c o m m a n d S t a t e S t r i n g L o o k F o r O b j e c t ( Data5D i s t a t e s ,1144 S t r i n g icommands ,1145 i n t i d s t a r t ,1146 Q S t r i n g a s t a t e s ,1147 Data5D &o s t a t e s ,1148 S t r i n g &ocommands ,1149 Data5D d ,1150 Data5D s ,1151 i n t maxTime )

A.4 Source code of TVPnaocommand.cpp 641152 {1153 # i f d e f i n e d (TVPNAOCOMMAND1 DEBUG) | | d e f i n e d (ALL DEBUG)1154 qDebug ( ) i s S c h e d u l e d ( i s t a t e s , i d s t a r t ) ) )1172 {1173 s t a r t T i m e = Gen . getTime ( ) ;1174 }1175 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1176 ∗ Put d e f a u l t v a l u e s1177 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1178 i n t x = −1;1179 i n t y = −1;1180 i n t w = 3 20;1181 i n t h = 3 20;1182 comm = ” ” ;1183 Q S t r i n g a c t i v e S t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d s t a r t ) ;1184 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1185 ∗ Try t o g e t f i r s t o b j e c t1186 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1187 i f ( ( d . d a t a t y p e == DATA5D INT ) &&1188 ( ( d .w == 6) | | ( d .w == 7) ) && ( d . h >= 1) && ( d . d == 1) && ( d . t == 1) &&1189 ( d . e == 1) )1190 {1191 i n t ∗ d a t a = ( i n t ∗) d . d a t a ;1192 x = ( d a t a [ 0 ] + d a t a [ 1 ] ) / 2 ;1193 y = ( d a t a [ 2 ] + d a t a [ 3 ] ) / 2 ;1194 w = d a t a [ 4 ] ;1195 h = d a t a [ 5 ] ;1196 }1197 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1198 ∗ E v a l u a t e i f t h e r e i s a f i r s t o b j e c t a v a i l a b l e1199 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1200 i f ( ( x >= 0) && ( x < w) && ( y >= 0) && ( y < h ) )1201 {1202 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1203 ∗ An o b j e c t was found1204 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1205 s t a r t T i m e = Gen . getTime ( ) ;1206 found = t r u e ;1207 Q S t r i n g lcomm = ” ” ;1208 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1209 ∗ e v a l u a t e i n p u t d a t a1210 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /

A.4 Source code of TVPnaocommand.cpp 651211 i f ( ( s . d a t a t y p e == DATA5D FLOAT) &&1212 ( ( s .w == ns−>s i z e ( ) ) | | ( s .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l .1213 ( s . h == 1) && ( s . d == 1) && ( s . t == 1) && ( s . e == 1) )1214 {1215 f l o a t ∗ d a t a = ( f l o a t ∗) s . d a t a ;1216 f l o a t currentYaw = d a t a [ HeadYaw ] ∗ ( 1 8 0 / PI ) ;1217 f l o a t c u r r e n t P i t c h = d a t a [ HeadPitch ] ∗ ( 1 8 0 / PI ) ;1218 f l o a t targetYaw = currentYaw + (w/ 2 − x ) / 8 ; / / h a l f t i m e s w1219 f l o a t t a r g e t P i t c h = c u r r e n t P i t c h − ( h / 2 − y ) / 8 ; / / h a l f t i m e s h1220 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( targetYaw ) ;1221 Q S t r i n g b = Q S t r i n g ( ”%1” ) . a r g ( t a r g e t P i t c h ) ;1222 f l o a t eps = 2 ;1223 i f ( ( currentYaw < targetYaw + eps ) &&1224 ( currentYaw > targetYaw − eps ) &&1225 ( c u r r e n t P i t c h < t a r g e t P i t c h + eps ) &&1226 ( c u r r e n t P i t c h > t a r g e t P i t c h − eps ) )1227 {1228 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1229 ∗ We have g o t a o b j e c t i n t h e c e n t e r ; A c t i v a t e f i r s t group i n1230 ∗ a s t a t e s : [ x , y ] [ a , b ] −> x , y1231 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1232 T V P s t a t e s s t ;1233 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 0) ; / / f i r s t group1234 }1235 e l s e1236 {1237 lcomm =1238 ” [ move ( HeadYaw , ” + a + ” , 3 0 0 , 1 ) | ”1239 ” move ( HeadPitch , ” + b + ” , 2 0 0 , 1 ) ]& w a i t ( 1 0 0 0 ) ” ;1240 }1241 }1242 comm = lcomm ;1243 }1244 e l s e1245 {1246 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1247 ∗ No o b j e c t found s t a r t l o o k i n g around1248 ∗ −Determine time t h a t we have n o t seen an o b j e c t ; i f l a r g e r t h a n N s e c1249 ∗ we a r e done1250 ∗ −S t a r t l o o k i n g around1251 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1252 Time t = Gen . getTime ( ) ;1253 i n t t i m e E l a p s e d = Gen . g e t T i m e E l a p s e d ( t , s t a r t T i m e ) ;1254 i f ( t i m e E l a p s e d > maxTime )1255 {1256 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1257 ∗ No o b j e c t found i n t h e maximum time a v a i l a b l e1258 ∗ A c t i v a t e second group of s t a t e s1259 ∗ a s t a t e s : [ x , y ] [ a , b ] −> a , b1260 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1261 T V P s t a t e s s t ;1262 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 1) ; / / second group1263 }1264 e l s e1265 i f ( ( t i m e E l a p s e d < 10000) && ( found ) ) / / o b j e c t found < 10 s e c ago .1266 {1267 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1268 ∗ Do n o t h i n g l i k e l y t h e o b j e c t i s seen soon1269 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /

A.4 Source code of TVPnaocommand.cpp 661270 }1271 e l s e1272 {1273 found = f a l s e ; / / n o t h i n g found l a t e l y1274 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1275 ∗ Look around1276 ∗ use 9 d i f f e r e n t p o s i t i o n s ( i n an e l l i p s e s t a r t i n g i n t h e c e n t e r )1277 ∗ From naomotion . cpp :1278 ∗ HeadYaw −120 120 9 . 4 71279 ∗ HeadPitch − 39 30 8 . 2 41280 ∗ H, V1281 ∗ pos 0 : ( 0 , 0)1282 ∗ pos 1 : ( 0 , 30)1283 ∗ pos 2 : ( 85 , 21)1284 ∗ pos 3 : ( 120 , 0)1285 ∗ pos 4 : ( 85 , −21)1286 ∗ pos 5 : ( 0 , −30)1287 ∗ pos 6 : ( −85, −21)1288 ∗ pos 7 : ( −120 , 0)1289 ∗ pos 8 : ( −85, 21)1290 ∗ pos −1: a l l o t h e r s1291 ∗1292 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1293 f l o a t currentYaw = −10.0 f ;1294 f l o a t c u r r e n t P i t c h = 1 0 . 0 f ;1295 i f ( ( s . d a t a t y p e == DATA5D FLOAT) &&1296 ( ( s .w == ns−>s i z e ( ) ) | | ( s .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r1297 ( s . h == 1) && ( s . d == 1) && ( s . t == 1) && ( s . e == 1) )1298 {1299 f l o a t ∗ d a t a = ( f l o a t ∗) s . d a t a ;1300 currentYaw = d a t a [ HeadYaw ] ∗ ( 1 8 0 . 0 / PI ) ;1301 c u r r e n t P i t c h = d a t a [ HeadPitch ] ∗ ( 1 8 0 . 0 / PI ) ;1302 }1303 i n t pos = −1;1304 c o n s t f l o a t D = 2 . 0 f ;1305 c o n s t f l o a t H0 = −70.0 f ;1306 c o n s t f l o a t H1 = −35.0 f ;1307 c o n s t f l o a t H2 = 0 . 0 f ;1308 c o n s t f l o a t H3 = 3 7 . 0 f ;1309 c o n s t f l o a t H4 = 7 0 . 0 f ;1310 c o n s t f l o a t V0 = −10.0 f ;1311 c o n s t f l o a t V1 = −5.0 f ;1312 c o n s t f l o a t V2 = 0 . 0 f ;1313 c o n s t f l o a t V3 = 5 . 0 f ;1314 c o n s t f l o a t V4 = 1 0 . 0 f ;1315 i f ( ( currentYaw >= H2 − D) && ( currentYaw = V2 − D) && ( c u r r e n t P i t c h = V4 − D) && ( c u r r e n t P i t c h = V0 − D) && ( c u r r e n t P i t c h = H3 − D) && ( currentYaw

A.4 Source code of TVPnaocommand.cpp 671329 i f ( ( c u r r e n t P i t c h >= V3 − D) && ( c u r r e n t P i t c h = V1 − D) && ( c u r r e n t P i t c h = H4 − D) && ( currentYaw = V2 − D) && ( c u r r e n t P i t c h = H1 − D) && ( currentYaw = V3 − D) && ( c u r r e n t P i t c h = V1 − D) && ( c u r r e n t P i t c h = H0 − D) && ( currentYaw = V2 − D) && ( c u r r e n t P i t c h

A.4 Source code of TVPnaocommand.cpp 681388 t a r g e t P i t c h = V1 ;1389 targetYaw = H1 ;1390 b r e a k ;1391 c a s e 7 :1392 targetYaw = H0 ;1393 b r e a k ;1394 c a s e 8 :1395 t a r g e t P i t c h = V3 ;1396 targetYaw = H1 ;1397 b r e a k ;1398 }1399 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( targetYaw ) ;1400 Q S t r i n g b = Q S t r i n g ( ”%1” ) . a r g ( t a r g e t P i t c h ) ;1401 comm =1402 ” [ move ( HeadYaw , ” + a + ” , 1000 , 1) | ”1403 ” move ( HeadPitch , ” + b + ” , 800 , 1) ] & w a i t ( 3 0 0 0 ) ” ;1404 }1405 }1406 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1407 ∗ C r e a t e s t a t e s t r i n g1408 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1409 t c c −>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , i d s t a r t , comm , a c t i v e S t a t e s ,1410 o s t a t e s , ocommands ) ;1411 }14121413 / ∗ ∗1414 ∗ \ b r i e f commandStateStringMoveToObject g e n e r a t e s a command and u p d a t e s1415 ∗ i t s s t a t e u s i n g o b j e c t s .1416 ∗ \ a u t h o r Tino Lourens1417 ∗ \ d a t e 11 Jun 20111418 ∗ \ v e r s i o n 11 Jun 20111419 ∗ \ param i s t a t e s i n p u t s t a t e s1420 ∗ \ param icommands i n p u t commands1421 ∗ \ param i d s t a r t s t a r t i n g i d1422 ∗ \ param a s t a t e s s t a t e s t o be a c t i v a t e d when command i s e x e c u t e d1423 ∗ \ param o s t a t e s o u t p u t s t a t e s1424 ∗ \ param ocommands o u t p u t commands1425 ∗ \ param d i n p u t o b j e c t a r e a s x1 , x2 , y1 , y2 , w, h1426 ∗ \ param s i n p u t nao s e n s o r y d a t a1427 ∗ \ param maxTime maximum time t o f i n d a o b j e c t i n m i l l i s e c o n d s1428 ∗ \ param d i s t a n c e c a l i b r a t e d d i s t a n c e f o r an o b j e c t of 0 . 1 5m @ 50 p i x e l s1429 ∗ \ param d i a m e t e r o b j e c t d i a m e t e r i n m e t e r s1430 ∗ \ r e t u r n none1431 ∗1432 ∗ \ h t m l o n l y \ e n d h t m l o n l y1433 ∗ commandStateStringMoveToObject g e n e r a t e s a command and u p d a t e s1434 ∗ i t s s t a t e u s i n g o b j e c t s .1435 ∗1436 ∗ An o b j e c t has been found1437 ∗ 1 . Mova a r c t o w a r d s o b j e c t ( such t h a t head i s s t r a i g h t )1438 ∗ 2 . Move t o w a r d s o b j e c t ( use c a l i b r a t i o n )1439 ∗ \ h t m l o n l y \ e n d h t m l o n l y1440 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1441 void TVPnaocommand : : commandStateStringMoveToObject ( Data5D i s t a t e s ,1442 S t r i n g icommands ,1443 i n t i d s t a r t ,1444 Q S t r i n g a s t a t e s ,1445 Data5D &o s t a t e s ,1446 S t r i n g &ocommands ,

A.4 Source code of TVPnaocommand.cpp 691447 Data5D d ,1448 Data5D s ,1449 i n t maxTime ,1450 d o u b l e d i s t a n c e ,1451 d o u b l e d i a m e t e r )1452 {1453 # i f d e f i n e d (TVPNAOCOMMAND1 DEBUG) | | d e f i n e d (ALL DEBUG)1454 qDebug ( ) i s S c h e d u l e d ( i s t a t e s , i d s t a r t ) ) )1486 {1487 s t a r t T i m e = Gen . getTime ( ) ;1488 f i n i s h e d = f a l s e ;1489 }1490 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1491 ∗ Try t o g e t f i r s t o b j e c t1492 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1493 i f ( ( d . d a t a t y p e == DATA5D INT ) &&1494 ( ( d .w == 6) | | ( d .w == 7) ) && ( d . h >= 1) && ( d . d == 1) && ( d . t == 1) &&1495 ( d . e == 1) )1496 {1497 i n t ∗ d a t a = ( i n t ∗) d . d a t a ;1498 x l = d a t a [ 0 ] ;1499 xr = d a t a [ 1 ] ;1500 yu = d a t a [ 2 ] ;1501 yd = d a t a [ 3 ] ;1502 x = ( x l + xr ) / 2 ;1503 y = ( yu + yd ) / 2 ;1504 w = d a t a [ 4 ] ;1505 h = d a t a [ 5 ] ;

A.4 Source code of TVPnaocommand.cpp 701506 }1507 i f ( TVPgetBodyPose ( s ) != 1) / / S t a n d i n g1508 comm = TVPstandup ( s ) ;1509 e l s e1510 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1511 ∗ E v a l u a t e i f t h e r e i s a f i r s t o b j e c t a v a i l a b l e1512 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1513 i f ( ( x >= 0) && ( x < w) && ( y >= 0) && ( y < h ) )1514 {1515 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1516 ∗ An o b j e c t was found1517 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1518 s t a r t T i m e = Gen . getTime ( ) ;1519 found = t r u e ;1520 Q S t r i n g lcomm = ” ” ;1521 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1522 ∗ e v a l u a t e i n p u t d a t a1523 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1524 i f ( ( s . d a t a t y p e == DATA5D FLOAT) &&1525 ( ( s .w == ns−>s i z e ( ) ) | | ( s .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l .1526 ( s . h == 1) && ( s . d == 1) && ( s . t == 1) && ( s . e == 1) )1527 {1528 f l o a t ∗ d a t a = ( f l o a t ∗) s . d a t a ;1529 f l o a t currentYaw = d a t a [ HeadYaw ] ∗ ( 1 8 0 / PI ) ;1530 f l o a t c u r r e n t P i t c h = d a t a [ HeadPitch ] ∗ ( 1 8 0 / PI ) ;1531 f l o a t targetYaw = currentYaw + (w/ 2 − x ) / 8 ; / / h a l f t i m e s w1532 f l o a t t a r g e t P i t c h = c u r r e n t P i t c h − ( h / 2 − y ) / 8 ; / / h a l f t i m e s h1533 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( targetYaw ) ;1534 Q S t r i n g b = Q S t r i n g ( ”%1” ) . a r g ( t a r g e t P i t c h ) ;1535 f l o a t eps = 2 ;1536 i f ( ( currentYaw < targetYaw + eps ) &&1537 ( currentYaw > targetYaw − eps ) &&1538 ( c u r r e n t P i t c h < t a r g e t P i t c h + eps ) &&1539 ( c u r r e n t P i t c h > t a r g e t P i t c h − eps ) )1540 {1541 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1542 ∗ We have g o t a o b j e c t i n t h e c e n t e r1543 ∗ L e t s e v a l u a t e how f a r we a r e away from t h e o b j e c t1544 ∗ between 0.25 −2.0 m e t e r s .1545 ∗ ppcm i s p i x e l s p e r c e n t i m e t e r1546 ∗ d i s t m i s d i s t a n c e i n c e n t i m e t e r s1547 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1548 i n t ox = abs ( xr − x l ) ;1549 d o u b l e ppcm = ox / ( 1 0 0 . 0 ∗ d i a m e t e r ) ;1550 d o u b l e i s i z e = 3 2 0 . 0 / w;1551 ppcm ∗= i s i z e ; / / map as i f image was 320 x2401552 / / c o u t

A.4 Source code of TVPnaocommand.cpp 711565 ∗ We a r e j u s t i n f r o n t of t h e o b j e c t ˜30 cm ppcm=151566 ∗ ˜25cm ppcm=161567 ∗ Thus we a r e done .1568 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1569 / / c o u t

A.4 Source code of TVPnaocommand.cpp 721624 e l s e1625 {1626 lcomm =1627 ” [ move ( HeadYaw , ” + a + ” , 3 0 0 , 1 ) | ”1628 ” move ( HeadPitch , ” + b + ” , 2 0 0 , 1 ) ]& w a i t ( 3 0 0 0 ) ” ;1629 }1630 }1631 comm = lcomm ;1632 }1633 e l s e1634 {1635 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1636 ∗ No o b j e c t found anymore1637 ∗ −Determine time t h a t we have n o t seen a o b j e c t ; i f l a r g e r t h a n N s e c1638 ∗ we a r e done1639 ∗ −S t a r t l o o k i n g around a b i t !1640 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1641 Time t = Gen . getTime ( ) ;1642 i n t t i m e E l a p s e d = Gen . g e t T i m e E l a p s e d ( t , s t a r t T i m e ) ;1643 i f ( t i m e E l a p s e d > maxTime )1644 {1645 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1646 ∗ No o b j e c t found i n t h e maximum time a v a i l a b l e1647 ∗ A c t i v a t e second group of s t a t e s1648 ∗ a s t a t e s : [ x , y ] [ a , b ] −> a , b1649 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1650 T V P s t a t e s s t ;1651 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 1) ; / / second group1652 / / c o u t

A.4 Source code of TVPnaocommand.cpp 731683 {1684 f l o a t ∗ d a t a = ( f l o a t ∗) s . d a t a ;1685 f l o a t currentYaw = d a t a [ HeadYaw ] ∗ ( 1 8 0 / PI ) ;1686 s r a n d ( t ime (NULL) ) ;1687 f l o a t v a l = ( ( r and ( ) % 101) −50) / 1 0 . 0 ; / / − 50...50 / 101688 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( currentYaw + v a l ) ;1689 comm =1690 ” [ move ( HeadYaw , ” + a + ” , 1000) & w a i t ( 3 0 0 0 ) ] ” ;1691 }1692 }1693 }1694 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1695 ∗ C r e a t e s t a t e s t r i n g1696 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1697 t c c −>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , i d s t a r t , comm , a c t i v e S t a t e s ,1698 o s t a t e s , ocommands ) ;1699 }17001701 / ∗ ∗1702 ∗ \ b r i e f c o m m a n d S t a t e S t r i n g P o s e p r o v i d e a pose by i n d e x1703 ∗ \ a u t h o r Tino Lourens1704 ∗ \ d a t e 11 Jun 20111705 ∗ \ v e r s i o n 11 Jun 20111706 ∗ \ param i s t a t e s i n p u t s t a t e s1707 ∗ \ param icommands i n p u t commands1708 ∗ \ param i d s t a r t i d of s t a r t i n g s t a t e1709 ∗ \ param i d r e a d y s t a t e ( s ) t o a c t i v a t e when r e a d y1710 ∗ \ param o s t a t e s o u t p u t s t a t e s1711 ∗ \ param ocommands o u t p u t commands1712 ∗ \ param i n d e x i n d e x s t a r t i n g a t 0 = Stand up from f r o n t ; 10 = t a i c h i1713 ∗ \ param command d e f a u l t ” ” ; add t o pose a command1714 ∗ \ r e t u r n none1715 ∗1716 ∗ \ h t m l o n l y \ e n d h t m l o n l y1717 ∗ c o m m a n d S t a t e S t r i n g P o s e p r o v i d e a pose by i n d e x1718 ∗ \ h t m l o n l y \ e n d h t m l o n l y1719 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1720 void TVPnaocommand : : c o m m a n d S t a t e S t r i n g P o s e ( Data5D i s t a t e s ,1721 S t r i n g icommands ,1722 i n t i d s t a r t ,1723 Q S t r i n g i d r e a d y ,1724 Data5D &o s t a t e s ,1725 S t r i n g &ocommands ,1726 i n t index ,1727 Q S t r i n g command )1728 {1729 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)1730 qDebug ( )

REFERENCES 741742 {1743 TVPcommandConstruction t c ;1744 i f ( ! t c . t o S t a t e L i s t ( i d r e a d y ) )1745 {1746 G e n e r a l Gen ;1747 Gen . e r r o r ( ”TVPnaocommand” , ” commandStringPose ” ,1748 QObject : : t r ( ” S t a t e s when r e a d y s h o u l d be i n t e g e r v a l u e s ”1749 ” s e p a r a t e d by comma ’ s . The c u r r e n t l i s t i s ”1750 ” %1. ” ) . a r g ( i d r e a d y ) ) ;1751 }1752 t c c = new TVPcommandConstruction ( ) ;1753 i n i t S t r e a m = f a l s e ;1754 }1755 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1756 ∗ L e t s e v a l u a t e what t o do1757 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1758 comm = nm−>TVPpose ( i n d e x ) ;1759 i f ( command . l e n g t h ( ) > 0)1760 comm = ” [ ” + comm + ” ] | [ ” + command + ” ] ” ;1761 / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1762 ∗ C r e a t e s t a t e s t r i n g1763 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1764 t c c −>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , i d s t a r t , comm , i d r e a d y , o s t a t e s ,1765 ocommands ) ;1766 }References[1] T. Lourens. Tivipe –tino’s visual programming environment. In The 28 th Annual InternationalComputer Software & Applications Conference, IEEE COMPSAC 2004, pages 10–15, 2004.[2] T. Lourens and E. I. Barakova. User-friendly robot environment for creation of social scenarios.In J. M. Ferrández, J. R. Álvarez, F. de la Paz, and F. J. Toledo, editors, IWINAC 2011, number6686 in Lecture Notes in Computer Science, pages 212–221, La Palma, Canary Islands, Spain,May-June 2011. Springer-verlag.