Supporting Document: Code Listings

SUPPORTING DOCUMENT: CODE LISTINGS

Listing 1: Behavior: Go to the Ball

b a l l p o s=0

ne t1 pos = 3

l o c a l b a l l = chaos . getLpo ( b a l l p o s )

linPo s = b a l l . rho _ math . cos ( b a l l . theta )

latPos = -b a l l . rho _ math . si n ( b a l l . theta )

chaos .l o c kOrient a t i on ( 0 )

chaos .s e tTargetPos ( l inPo s -300 , latPos , 0)

chaos .setStandardMode ( 0 )

chaos .setNeeded ( 0 , 1 )

Listing 2: Initializing the Robot and the Accelerometer

// i n i t i a l i z e the rob ot

wb r o b o t i n i t ( ) ;

// i n i t i a l i z e and ena b l e the ac c e l e rome t e r

a c c e l e r ome t e r = wb_r obo t_g e t_de v i c e ( " a c c e l e r ome t e r " ) ;

wb_a c c e l e r ome t e r_enabl e ( ac c e l e rome t e r , TIME_STEP) ;

Listing 3: Updating Accelerometer

// Re t r i e v e ac c e l e rome t e r v a l u e s

// Webots r e turns v a l u e s in m/ s ^2

const double * acc = wb a c c e l e r ome t e r g e t v a lue s ( a c c e l e r ome t e r ) ;

a c c e l e r ome t e rValue s . c l e a r ( ) ;

a c c e l e r ome t e rValue s . push back ( ( f loat ) acc [ 0 ] ) ;

a c c e l e r ome t e rValue s . push back ( ( f loat ) acc [ 1 ] ) ;

a c c e l e r ome t e rValue s . push back ( ( f loat ) acc [ 2 ] ) ;

i f ( ! naoqiSimulated?>updateAc c e l e rome t e r s ( a c c e l e r ome t e rValue s ) )

c e r r < " updateAc c e l e rome t e r s ( ) f a i l e d . nn" ;

// I n e r t i a l v a l u e s ( computed from ac c e l e rome t e r v a l u e s )

i n e r t i a lVa l u e s . c l e a r ( ) ;

i n e r t i a lVa l u e s . push back ( ( f loat ) (-PI 2 + acos ( acc [ 1 ] /GRAVITY) ) ) ;

i n e r t i a lVa l u e s . push back ( ( f loat ) ( PI 2 -acos ( acc [ 0 ] /GRAVITY) ) ) ;

i f ( ! naoqiSimulated-upda t e Ine r t i a lValue s ( i n e r t i a lVa l u e s ) )

c e r r < " upda t e Ine r t i a lValue s ( ) f a i l e d . nn" ;

Listing 4: Updating Actuators

// Get body ang l e s from Motion

bodyAngles = naoqiSimulated-getBodyAngles ( ) ;

i f ( bodyAngles . s i z e ( ) == 0)

c e r r < "ALNaoSimulationInter face : : getBodyAngles ( ) f a i l e d . nn" ;

// update r e a l body ang l e s

bodyAnglesUpdate .c l e a r ( ) ;

for( inti = 0 ; i < SERVOMAX; i++)

{

doubleangl e = bodyAngles . at ( i ) ;

switch ( i ) {

caseRHipYawPitch :

// NaoQi y i e l d s always z e ro f o r t h i s j o i n t :ignor e i t . . .

break ;

caseLHipYawPitch : {

// . . . but use the v a lue from the l e f t s i d e ins t e a d

s e t S e r v oPo s i t i o n ( s e r v o s [ LHipYawPitch ] , angl e ) ;

s e t S e r v oPo s i t i o n ( s e r v o s [ RHipYawPitch ] , angl e ) ;

break ;

}

caseRHand : f

// we must a c t i v a t e the 8 phalanx motors

for ( int j = 0 ; j < PHALANXMAX; j++)

s e t S e r v oPo s i t i o n ( rphalanx [ j ] , angl e ) ;

break ;

}

caseLHand : {

// we must a c t i v a t e the 8 phalanx motors

for ( int j = 0 ; j < PHALANXMAX; j++)

s e t S e r v oPo s i t i o n ( lphalanx [ j ] , angl e ) ;

break ;

g

default :

// r e g u l a r j o i n t

s e t S e r v oPo s i t i o n ( s e r v o s [ i ] , angl e ) ;

break ;

}

i f ( s e r v o s [ i ] )

bodyAnglesUpdate .push back ( wb s e r v o g e t p o s i t i o n ( s e r v o s [ i ] ) ) ;

el se

bodyAnglesUpdate .push_back( 0 . 0 ) ;

}

Listing 5: Pseudocodefor get ball position() Function

Po s i t i on g e t b a l l p o s i t i o n ( ) {

i f ( ROBOT SIMUL) {

return we b o t s g e t b a l l p o s i t i o n ( ) ;

} else

return r o b o t g e t b a l l p o s i t i o n ( ) ;

}

}

Listing 6: Stimulating Chest Button

voidChaosModule : : s t a r tPl a y i n g ( )

{

cout<" [ ChaosModule ] Inf o : I n i t i a l i z i n g robot "<endl ;

comm = newCommanderNaoqi ( pBroker ) ;

cout<" [ ChaosModule ] CommanderNaoqi Ready"<endl ;

robot = newRobotManager (comm) ;

cout<" [ ChaosModule ] RobotManager Ready"<endl ;

cout<" [ ChaosModule ] Inf o : St a r tPlaying ! ! "<endl ;

cout<" [ Mustafa ] : St imulatingche s t pr e s s "<endl ;

robot->g e tCt r l ( )>onChestBumperPressed ( ) ;

robot->g e tCt r l ( )>onChestBumperPressed ( ) ;

robot->g e tCt r l ( )>onChestBumperPressed ( ) ;

}

Listing 7: autoload.ini

[ cor e ]

alba s e

[ ext r a ]

devicecommunicationmanager

s e n s o r s

motion

l e d s

s e n t i n e l

n a o_s o c c e r_p l a y e r_r e d

Listing 8: Initializing GPS and Compass

// header f i l e s o f the gps and compass d e v i c e s in webot s

#includewebots / gps . h>

#includewebots /compass . h>

// i n i t i a l i z e the d e v i c e s

d e v i c e s [ " gps " ] = wb_r obo t_g e t_de v i c e ( " gps " ) ;

d e v i c e s [ "compass" ] = wb_r obo t_g e t_de v i c e ( "compass" ) ;

// ena b l e the d e v i c e s

Wb_gps_enabl e ( d e v i c e s [ " gps " ] , TIME_STEP) ;

Wb_compass_enable( d e v i c e s [ "compass" ] , TIME_STEP) ;

Listing 9: Storing GPS and Compass Outputs into ALMemory

// update the p o s i t i o n informat ion

//ALMotion and Webots do not have the same c o o r d ina t e system

const double *value s = wbgps g e t value s ( d e v i c e s [ " gps " ] ) ;

na o sPo s i t i on . c l e a r ( ) ;

na o sPo s i t i on . push back ( ( f loat ) value s [ 0 ] ) ;

na o sPo s i t i on . push back ( ( f loat )-value s [ 2 ] ) ;

na o sPo s i t i on . push back ( ( f loat ) value s [ 1 ] ) ;

i f ( ! naoqiSimulated?>updatePosi t ionEs t imat e ( na o sPo s i t i on ) )

{

s td : : c e r r < " f a i l to update NaosPosi t ion nn" ;

}

// update the o r i e n t a t i o n informat ion

const double *o r i e n t = wbcompas s ge t value s ( d e v i c e s [ "compass" ] ) ;

naosOrient at ion . c l e a r ( ) ;

naosOrient at ion . push back ( ( f loat ) o r i e n t [ 0 ] ) ;

naosOrient at ion . push back ( ( f loat ) o r i e n t [ 1 ] ) ;

naosOrient at ion . push back ( ( f loat ) o r i e n t [ 2 ] ) ;

doublerad = atan2 ( naosOrient at ion [ 0 ] , naosOrientat ion [ 2 ] ) ;

double o r ient a t ionAng l e = ( rad-1 . 5 7 0 8 ) / M PI * 1 8 0 . 0 ;

i f ( o r ient a t ionAng l e < 0 . 0 ) {

o r ient a t ionAng l e = o r ient a t ionAng l e + 3 6 0 . 0 ;

}

insertFloatInMemory ( " Simulator /Or ient a t i on /Angle " , o r ient a t ionAng l e ) ;

Listing 10: Prototype of Ball

PROTO Ba l l [

fi e l d SFVec3f t r a n s l a t i o n 0 0.0325 0

fi e l d SFColor c o l o r 1 . 0 0.54 0. 08

fi e l d SFFloat r adius 0.0325

fi e l d SFFloat mass 0.055

fi e l d SFVec3f centerOfMass 0 -0.0001 0

fi e l d SFSt r ing c o n t r o l l e r " void "

f i e l d SFSt r ing c o n t r o l l e rAr g s ""

fi e l d SFFloatgpsResolut ion 0 . 0

]

{

Robot {

t r a n s l a t i o n IS t r a n s l a t i o n

r o t a t i o n 0 1 0 0

c o n t r o l l e r IS c o n t r o l l e r

c o n t r o l l e rAr g s IS c o n t r o l l e rAr g s

name " b a l l "

c h i l d r e n [

DEF BALL SHAPE Shape f

appearanceAppearance f

ma t e r i a l Mat e r ial f

ambientIntensity 0 . 4

di f f u s eCo l o r IS c o l o r

s h i n i n e s s 0 . 8

specul a rCo l o r 1 11

}

}

geometry Sphere {

radius IS r adius

s u b d i v i s i o n 2

}

}

DEF GPS GPS {

name " gps "

r e s o l u t i o n IS gpsResolut ion

}

DEF COMPASS Compass {

name "compass"

}

]

boundingObject USE BALL SHAPE

phy s i c s Phys i c s {

densi t y -1

mass IS mass

bounce 0 . 9

coulombFri c t ion 0 . 5

for c eDependentSlip 0 . 5

centerOfMass IS centerOfMass

}

}

}

Listing 11: Ball Controller for Emitting its Position

#includewebots / robot . h>

#include <webots / gps . h>

#includewebots / emi t t e r . h>

#include <s t d io . h>

#define TIME_STEP 1000

intmain ( intargc , char **argv ){

wb r o b o t i n i t ( ) ;

WbDeviceTaggps_de v i c e = wb_r obo t_g e t_de v i c e ( "gps " ) ;

WbDeviceTagemi t t e r_d e v i c e = wb_r obo t_g e t_de v i c e ( "emi t t e r " ) ;

Wb_gps_enabl e ( gps_devi c e , TIME_STEP) ;

wbemi t t e r s e t channe l ( emi t t e r_de v i c e , 1) ;

do {

const double *b a l lPo s i t i o n = wb_gps_g e t_value s ( gps_de v i c e ) ;

// send the p o s i t i o n informat ion to p l a y e r s

wbemi t t e r s end ( emi t t e r de v i c e , b a l lPo s i t i o n , 3*sizeof ( const

double ) ) ;

} while( wb_robot s t ep (TIME_STEP) != -1) ;

wb robot c l eanup ( ) ;

return 0 ;

}

Listing 12: Receiving Ball Position

while ( wb_r e c e i v e r g e t q u e u e l e n g t h ( d e v i c e s [ " r e c e i v e r " ] ) > 0) {

const double * b u f f e r = ( const double *) wb_r e c e i v e r_g e t_d a t a ( d e v i c e s [" r e c e i v e r " ] ) ;

insertFloatInMemory ( " Simulator / Ba l lPo s i t i o n /X" , b u f f e r [ 0 ] ) ;

insertFloatInMemory ( " Simulator / Ba l lPo s i t i o n /Y" , b u f f e r [ 1 ] ) ;

insertFloatInMemory ( " Simulator / Ba l lPo s i t i o n /Z" , b u f f e r [ 2 ] ) ;

wb r e c e i v e r ne x t pa c k e t ( de v i c e s [ " r e c e i v e r " ] ) ;

}