Humanoid Robo Simulaion wih a Join Trajecory Opimized Conroller José L. Lima, José C. Gonçalves, Paulo G. Cosa, A. Paulo Moreira Deparmen of Elecrical and Compuer Engineering Faculy of Engineering of Universiy of Poro jllima@ipb.p, goncalves@ipb.p, paco@fe.up.p, amoreira@fe.up.p Absrac This paper describes a join rajecory opimized conroller for a humanoid robo simulaor following he real robo characerisics. As simulaion is a powerful ool for speeding up he conrol sofware developmen, he proposed accurae simulaor allows o fulfil his goal. The simulaor, based on he Open Dynamics Engine and GLScene graphics library, provides insan visual feedback. The proposed simulaor, wih realisic dynamics, allows o design and es behaviours and conrol sraegies wihou access o he real hardware in order o carry ou research on robo conrol wihou damaging he real robo in he early sages of he developmen. The joins conroller echniques, such as acceleraion, speed and energy consumpion minimizaion are discussed and experimenal resuls are presened in order o validae he proposed simulaor. 1. Inroducion In las years, sudies of research in biped robos have been developed and resuled in a variey of prooypes ha resemble he biological sysems. Legged robos have he abiliy o choose opional landing poins, an advanage o move in rugged errains, and wo legged robos are also able o move in human environmen. So, sudies abou biped robos are very imporan and simulaing [1]. Locomoion under influence of exernal disurbances is a challenging ask for a humanoid robo, once if disurbances are large enough, a fall migh become unavoidable. Closed loop conrollers should minimize he number of falls [2] and if a fall happens, he robo should deec i and ge back ino an uprigh posure [3]. On he one hand, simulaion is a powerful ool for speeding up he conrol sofware developmen. On he oher hand, developing new conrol sofware for robos can be a difficul and challenge ask. The abiliy o rapidly prooype sofware, wihin a simulaion environmen, can be of grea benefi o develop robo conrol if he resuling sofware can be easily ransfered from simulaion o real world sysems. Therefore, he simulaor mus capure he mos imporan environmen characerisics; however, developing simulaors wih high-fideliy dynamic models ha can be simulaed in real-ime is a non rivial problem [4]. The simulaor mus also be able o measure he consumed energy providing a good efficiency planning. The planning for humanoid movemens should resul in minimum energy consumpion, like i happens in he human body. Joins angles and orques limis mus also be handled. There are several robo simulaors, such as Simspark, Webos and MURoSimF, ha provide humanoid simulaion capabiliy. Meanwhile, he developed simulaor allows o build and o es he low and high level conrollers, wih a configurable conrol period, in a way ha can be mapped wih he realiy, alhough wih some overhead. Furhermore, his simulaor can be conrolled by nework and scrip language avoiding insallaions of developmen applicaions. As an imporan feaure, robos can be buil wih a configurable srucure based on a xml descripion file. I is also possible o creae several humanoid robos in he environmen. Code migraion from general realisic simulaors o real world sysems is he key for reducing developmen ime of robo conrol, localizaion and navigaion sofware. Due o he complexiy of robo, world, sensors, and acuaors modelling i is no an easy ask o develop such simulaor. The moivaion of developing a realisic humanoid robo simulaor is o produce a personalized and versaile ool ha will allow in he fuure he producion and validaion of robo sofware reducing considerably he developmen ime. This simulaor deals wih robo dynamics and how i reacs for several conroller sraegies and syles. This paper proposes a simulaor, based on he Open Dynamics Engine [5], for a humanoid robo and presens is low level conroller. The proposed simulaor allows o design and es behaviours wihou access o real hardware in order o carry ou research on robo conrol once i is developed having in mind he real robo: dimensions, masses, inerias, joins angles and velociies limis are accuraely resembled. The paper is organized as follows: Iniially, he real robo, where mechanical design, communicaion and conrol applicaion are described, is presened. Then,
secion 3 presens he developed simulaor basis and how simulaor robo is buil. Secion 4 presens he join rajecory planning where minimum acceleraion, minimum speed and minimum energy consumpion mehods are described. Experimenal resuls are presened furher in secion 5. Finally, secion 6 rounds up wih conclusions and fuure work. 2.2. Communicaion Archiecure Muliple layers ha run on differen ime scales conain behaviours of differen complexiy. The layer map is presened in Figure 3. 2. Real Humanoid Robo The commercially available Bioloid [6] robo ki, from Robois, is he basis of he used humanoid robo. The overview of he proposed biped robo is shown in Figure 1. The suggesed robo was modified and differs from he original ki, o follow he dimensional rules of RoboCup [7] Humanoid League [8]. Nex subsecions presen he physical robo in which was based he developed humanoid simulaor. a) b) Figure 1. Real humanoid robo poses. 2.1. Mechanical Design The presened humanoid robo is driven by 19 servo moors: 6 per leg, 3 in each arm and one in he head. Three orhogonal servos se up he 3DOF (degree of freedom) hip join. Two orhogonal servos form he 2DOF ankle join. One servo drives he head (a vision camera holder). The shoulder is based on wo orhogonal servos allowing a 2DOF join and elbow has one servo allowing 1DOF. The oal weigh of he robo is abou kg and is heigh is 38 cm. The modelled humanoid robo is presened in Figure 2 ha allows o visualize real humanoid posure in he inerface sofware. a) b) Figure 2. Modelled robo. a) fronal view, b) back view. Figure 3. Layers diagram. The lowes level of his hierarchy, he conrol loop wihin he Dynamixel acuaors (AX-12), has been implemened by Robois [9]. Each servo is able o be programmed wih no only he goal posiion, he moving speed, he maximum orque, he emperaure and volage limis bu also wih he conrol parameers. This communicaion layer is based on a 1Mbps half-duplex serial bus where each individual servo can be addressed or a broadcas can be sen. These limiaions are placed in he simulaor for a faihful represenaion. A he nex layer, an inerface uniy CM-5 module, based on an Amel ATMega128 microconroller, allows a communicaion inerchange. I receives messages from he upper layer and ranslaes hem o he servos bus. Answers from servos are also ranslaed and sen back o he upper layer as presened in Figure 4. Figure 4. Inerface layer. The original firmware was replaced in order o ge higher performances and low level conroller achieve. A he higher layer, arge angle and moving speed for he individual joins are generaed from a personal compuer or from an embedded sysem. 2.3. Behaviour and Conrol Percepion assumes a major role in an auonomous roboics, and mus be herefore reliable or abundan [10]. For his robo, he join posiion, speed and orque percepion was planned. For enlarge he closed loop conrol, as a fuure feaure, he acceleromeers informaion and fee force sensing percepions were also planned [11]. A he presen, he real humanoid robo is
no equipped wih acceleromeers and for similariy he simulaor sensors were no included. As a firs approach, an open-loop sysem can be used (acceleromeers and fee force informaion are disabled). This can be done sending pre-programmed join angles and angular speeds for each join. Walk and sand up movemens can be achieved. The developed sofware ha communicaes wih CM-5 module and conrols he robo is presened in Figure 5. The main ask is o send preprogrammed joins angles and velociies ha compose a movemen and show, in a real-ime 3D window, he real robo posure. Preprogrammed posiions can be planned off-line (disconneced from he robo) once user can observe he robo posures. I also allows o obain he real aiude of he robo. Figure 5. Developed humanoid sofware conroller. 3. Open Dynamics Engine Simulaion Design behaviour wihou real hardware is possible due o a physics-based simulaor implemenaion. The physics engine is he key o make simulaion useful in erms of high performance robo conrol. Alhough here are a number of open source simulaion engines available, mos focus on producing fas pseudo realisic simulaions for use in compuer games. These engines are herefore fas, bu produce moions ha look good as opposed o being accurae. In conras, here exis a number of simulaion engines for rigid body moion ha are unusable for simulaing he mechanical ineracions of rigid pars [4]. For real-ime simulaion, an accurae bu fas simulaion engine mus be used. ODE, Open Dynamics Engine [5] checks hese requisies. As an open source rigid body simulaion engine, developed by Russell Smih, has reached a mauriy level ensuring ha produced code is sable. I is essenially a simulaion library ha provides suppor for rigid body moion, roaional ineria and collisions reamen where he world o be simulaed is buil. I also allows o use Open GL (graphics library) rouines o render he 3D simulaed environmen. The graphic rouines are based on Open GLScene library. I provides visual componens and objecs allowing descripion and rendering of 3D scenes in an easy, no-hassle, ye powerful manner. I has grown o become a se of founding classes for a generic 3D engine wih RAD (Rapid Applicaion Developmen) in mind [12]. 3.1. Humanoid Consrucion A complex humanoid model can be avoided due o he ODE usage. Humanoid body simulaor consrucion is based in body masses and join connecions. Each body mass imiaes he servo moors and connecion pieces weighs from he real robo. ODE joins, imiae he servo moors axis movemens and mus be defined is ypes, angles and orques limis. Joins ypes can be classified as hinges or universal joins: a hinge ha allows boh bodies o be conneced and roll such as arms and forearms, femur and leg; a universal joinus be inroduced when here are wo or more degrees of freedom beween wo bodies. I happens when wo servo moors are physically combined. A universal join allows wo bodies o roll on boh axes. As example, presened in he simulaor, hese joins connec runk and arms, runk and legs, legs and fee. 3.2. XML model descripion The Exensible Markup Language (XML) is a general specificaion language ha allows is users o define heir own elemens. I defines a generic synax used o mark up daa wih simple human-readable ags [13]. A descripion of he humanoid roboodel in done resoring o XML descripion language. Posiions, sizes, masses perform he descripion of bones and posiions, axis, limis and ypes perform he descripion of joins as presened in he nex XML excerp. <robo> <kind value='humanoid'/> <solids> <cuboid> <ID value='6'/> <pos x='-0.040' y='0' z='0.010'/> <size x='0.082' y='0.102' z='0.137'/> <mass value='0.635'/> <desc P='Tronco'/> <desc Eng='Trunk'/> </cuboid> </solids> <ariculaions> <join> <ID value='0'/> <pos x='-0.039' y='0.030' z='-0.170'/> <axis x='0' y='1' z='0'/> <connec B1='7' B2='17'/> <limis Min='-98' Max='32'/> <ype value='hinge'/> <desc P='Joelho Esq'/>
<desc Eng='Lef Knee'/> </join> </ariculaions> </robo> The humanoid robo simulaor is buil based on he XML descripion. By his way, i is an easy ask o modify he robo srucure once here is no necessiy o compile a new applicaion, making he simulaor useful o ohers beyond he programmer. The same language is used o sore he movemens of each join. GLScene is used o render he 3D graphics appearance enhancing visualizaion including shadows, exures, projecions, illuminaions and i also provides deph percepion. Zooming and camera posiioning become also an easy ask. A screensho of he developed simulaor is shown in Figure 6, where a 3D scene shows some humanoid robos and several obsacles, a able shows he desired join variables such as angle and angular speed. Figure 6. Developed simulaor screensho. 4. Humanoid Join Trajecory Conroller This conroller level acceps, for each servo, angles, angular speeds and imings requiremens from he higher level. The main objecive of his conroller is o build and o follow he rajecories esablished by angles and angular speeds requiremens having in mind he acceleraion, speed and energy consumpion minimizaion. 4.1. Servo-Moor model The servo moor response, such as dynamics, maximum acceleraion and speed, mus be known in order o draw simulaor rajecories compaible o he real robo. The joins closed loops conrollers mus also have he same response as servos have. An inpu sep, from 0 o 50 degrees wih a sample frequency of 30 Hz and an inerial mass, is presened in Figure 7 (orange lozenges) and allows o obain he desired parameers. The maximum speed can be found by he servo moor angle maximum derivaive (ax=281 deg/s) and he acceleraion can be found by maximum second derivaive (a max=1400 deg/s 2 ). This es was made assuming ha fricion and wind-up sauraion non lineariies are despised. Figure 7. Real and simulaor servo moors response o a sep inpu. The implemened servo moor model in simulaor, based in he real servo moor sep response, is esed for he same inpu sep and wih he same inerial mass in order o validae is similariy wih he real one and presened in he same figure (blue squares). The overlapped curves allow o validae he servo moor model implemened in he simulaor. 4.2.Trajecory planning The joins conroller finds he inermediae rajecories ha ake joins o he desired saes and follow hem. I is also able o minimize he acceleraions in order o save consumed energy. Suppose ha for = 1 (acual ime) i is measured angle θ 1 and angular speed, and for = (nex period) i is desired posiion θ 2 and angular speed ω 2, as illusraed in Figure 8 a) and b), ha shows he used symbology and some examples of possible rajecories, assuming a consan acceleraion in [ 1, ] and [, ]. θ() θ 2 θ 1 1 ω() ω 2 1 a) b) Figure 8. Join sae: a) angle ref. and b) speed ref. I is necessary o calculae he angle and angular velociies equaions ha resul in he desired condiions: he and, assuming a consan angular acceleraion ha allows angular speed o follow a piece-wise linear equaion: he angular speed is linear by pars, and () ω2 ()
angular acceleraion is consan by pars. The (for insan) mus be deermined and depends on he adoped mehod: acceleraion, speed or energy can be minimized as presened in nex subsecions. By his way, angular reference and angular speed equaions can be found as a smooh movemen, following he desired condiions. There are several soluions for, for he same iniial and final condiions as presened in nex equaions. The covered angle (θ 2 -θ 1 ) can be expressed by he riangle areas (A I, A II and A III ) presened in Figure 9 and equaion 1. Equaion 2 allows o find he linear funcion =f( ), represened by L2 line in Figure 9. equaions 5 and 6. By his way, can be placed in [ 1, ] inerval, according o equaion 2, ha allows o cover he desired angle. Nex subsecions discusses posiioning from he poin of view of acceleraion, speed and energy consumpion. Ref = 1 1 1 1 m 1 (5) 2 1 2 1 Ref = Ref = m 1 2 m 2 m (6) ω() A II L2 A III L1 4.3. Acceleraion minimizaion conroller mehod During he [ 1, ] inerval, he join angular acceleraion a 1 can be expressed in equaion 7, whereas during he [, ] inerval, a 2 can be expressed in equaion 8 as presened in Figure 10. ω 2 h A I ω() L2 1 a 2 ω 2 Figure 9. posiion freedom. 2 1 = A I A II A III = m 1 m 1 1 2 m 2 m 2 2 2 1 1 2 1 m 2 2 m = 1 (1) (2) In fac, equaion 2 is a linear funcion of and is slope (derivaive funcion) can be found by (ω 2-)/(- 1), he same slope as L1 line. These lines, L1 and L2 are deviaed by h ha depends on he covered angle and i can be found by equaion 3. h= m 1 1 So, he h value can be deermined by equaion 4. h=2 2 1 1 2 1 (3) (4) The angle equaion, θ ref(), can be found resoring o he formula of uniform linear acceleraed movemen for each ime inerval [ 1,] and [,] as presened in Figure 10. Acceleraion minimizaion. a 2 = 2 m (7) (8) I can be shown ha if moves from 1 o, a 1 becomes lower and, oherwise, a 2 becomes higher. The opimal can be found when boh acceleraions modules mach, as presened in equaion 9. a 1 = a 2 a 1 1 a 1 = m 1 1 (9) Equaion 9 allows o find he =f(()) funcion for a desired as presened in equaion 10, having in mind he paricular case of acceleraions minimizaion a 1 and a 2. Amin = m 1 1 2 1 1 2 2 m (10)
The (,) poin can be found by equaion 2 for he Amin iming, calculaed in equaion 10, ha resuls he desired presened in equaion 11. m = = 1 2 2 1 h ± 1 2 2 2 2 2 1 1 2 h 2 (11) The valid ω() soluion is he one ha akes ino he [ 1,] ime window. The rajecory references θ() and ω(), which minimize boh acceleraions a 1 and a 2, can now be drawn. As resul, presened in Figure 11, a Malab funcion draws he expressed equaions for his example: θ 1=0 deg θ 2=27 deg =20 deg/s ω 2=30 deg/s 1=3 s =4 s. The maximum reached acceleraion is abou 14.77 deg/s 2. As resul, presened in Figure 13, a Malab funcion draws he expressed equaions for he same desired condiions as presened in previous subsecion and an acceleraion of 200 deg/s 2. ω() ω 2 a max L3 Figure 12. Join rajecory (ax minimized). The maximum reached speed is 30 deg/s (he desired final speed ω 2). L2 1 Figure 11. Join rajecory - minimum acceleraion. 4.4. Speed minimizaion conroller mehod The way o minimize he speed is o reach he L2 line as fas as possible while keeping he maximum acceleraion limis. From all he feasible soluions, he one hainimizes he join speed is presened in Figure 12 where a max is he servo moor maximum acceleraion. The insan can now be deermined by L2 and L3 inersecion as expressed in equaion 12. Equaion 2 allows o find he value for he desired insan. = h a max 2 1 1 1 (12) Figure 13. Join rajecory - minimum speed. 4.5. Energy consumpion minimizaion conroller mehod As humanoid robo is powered by on-board baeries, energy consumpion mus be reduced as much as possible. Trajecories design ask should care energy consumpion and minimize i. Assuming ha he insan power consumpion by servo moor can be deermined as he orque and he angular speed ω produc (P=k.I.a.ω), where I is he momen of ineria, k a scalar gain and a he angular acceleraion, i is possible o place in he energy minimizaion insan as described in his subsecion. The momen of ineria depends on each join and robo posure bu can be considered consan in energy minimizaion iming achieve for simpliciy As example, an exended arm haeasures approximaely 0.2 m and
weighs 0.174 kg has an momen of ineria of 0.00232 kg.m 2 (I=mL 2 /3, where m is he mass, and L he body lengh). The k value allows he conversion from deg/s o S.I. unis expressed as 4π 2 /360 2. For he firs ime par (where [ 1,]) here is an angular speed () wih an acceleraion a 1 and for he second ime par (where [,]) here is an angular speed ω 2() wih an acceleraion a 2. The insan power consumpion can be described by equaion 13 where a 1 and a 2 depend on insan and he oal energy can be described by he power inegral as presened in equaion 14. Minimum energy consumpion funcion from 1 o 2 iming is also illusraed in Figure 15 ha shows he opimal value a 3.6 seconds. If he opimal iming is ouside [ 1,] inerval, maximum acceleraion should be done a 1 if < 1 or o if >. P 1 =k I a 1 1 P 2 =k I a 2 2 (13) E To = 1 P1 P2 m d (14) The minimum energy consumpion insan can be found when is firs derivaive equals zero as presened in equaion 15 ha allows o find he insan presened in equaion 16. Equaion 2 allows o find he value for he desired insan. d E To =0 d = w 1 1 2 w 2 2 1 2 2 w 2 1 w 2 w 1 (15) (16) As resul, presened in Figure 14, a Malab funcion draws he expressed equaions for he same desired condiions as presened in previous subsecions. The consumed energy is 0.1767 mj, following he previous presened condiions. Figure 15. Energy consumpion in [ 1,] inerval. 4.6. Trajecory planning mehods comparison In order o compare he effeciveness of he presened rajecory planning mehods, able 1 presens he insan, he value, he maximum acceleraion a max and he energy consumpion E cons for each mehod (A min -minimum acceleraion, in -minimum speed and E min -minimum energy consumpion). Noice ha for he in mehod is lower han he requesed angle (30 deg/s). Table 1. Trajecories planning comparison. Mehod (s) (deg/s) a max (deg/s 2 ) E cons (mj) E cons (%) A min 3.84 32.39 14.77 0.1807 2.26 in 3.02 24.21 (30) 200.0 0.2004 13.41 E min 3.6 30 16.67 0.1767 - The shown resuls allow o validae he presened mehods. Energy consumpion saving in he presened examples is abou 13 percen and can be done wihou any hardware change. Wih his, E min mehod is he mos suiable having in mind he energy efficiency as baery energy is a limied resource. On he oher hand, acceleraions minimizaion mehod also allows o decrease he energy spen and can be someimes adoped o minimize joins effors on he robo. Figure 14. Join rajecory - minimum energy consumpion.
5. Experimenal Resuls This chaper presens, in a shor way, he resuls ha simulaor can achieve when he condiions previously presened are applied. The energy consumpion and acceleraion minimizaions are esed wih successful resuls. As example, he same condiions were applied o he neck join. The angle and reference angle are presened in Figure 16 where, a he lef side, user can add which robo, join and variables are requesed o appear in he graphic a he righ side. minimize energy consumpion is presened. The presened resuls allow o validae he simulaor and show he realisic simulaion. The whole body conroller was implemened and humanoid simulaor behaves like real robo. As fuure work, more conrol sraegies will be implemened and esed using he high level programming based on a Pascal scrip dialec ha allows users o creae heir own conrol programs while resuls are real-ime presened. Enhancing he simulaor wih realisic sensors, such as acceleromeers and gyroscopes, is also a fuure sep. Figure 16. Neck joininimum consumpion energy es. In he presened example, neck join angle follows he reference from 0 o 27 degrees in 1 second using he minimum energy consumpion rajecory. Oher example can be shown in Figure 17 where neck join follows he reference array [0, 45, 90, -45, -90] degrees wih minimum acceleraion conrol mehod. Figure 17. Neck joininimum acceleraion es. 6. Conclusion and Fuure Work In his paper a humanoid simulaor, based on a dynamics engine and a 3D visualizaion engine, is presened. The real robo limiaions, such as servo moors angular speed and acceleraions are aken ino accoun. A low level rajecories conroller ha allows o References [1] T. Suzuki, K. Ohnishi, "Trajecory Planning of Biped Robo wih Two Kinds of Invered Pendulums", Proceedings of 12h Inernaional Power Elecronics and Moion Conrol Conference, 2006. [2] R. Renner, S. Behnke, "Insabiliy deecion and fall avoidance for a humanoid using aiude sensors and reflexes", Proceedings of Inernaional Conference on Ineligen Robos and Sysems, 2006. [3] J. Sückler, J. Schwenk, S. Behnke, "Geing Back on Two Fee: Reliable Sanding-up Rouines for a Humanoid Robo", Proceedings of 9h Inernaional Conference on Ineligen Auonomous Sysems, 2006. [4] B. Browning, E. Tryzelaar, "UberSim: A Realisic Simulaion Engine for RoboSoccer", Proceedings of Auonomous Agens and Muli-Agen Sysems, 2003. [5] Russell Smih, Open Dynamics Engine, hp://www.ode.org, 2000. [6] Triboix, hp://www.riboix.com/index.hml, 2004. [7] Robocup, hp://www.robocup.org/, 2007. [8] Humanoid League, hp://www.humanoidsoccer.org/, 2007. [9] S. Behnke, M. Schreiber, J. Sückler, R. Renner, H. Srasda, "See, Walk, and kick: Humanoid robos sar o play soccer", Proceedings of Inernaional Conference on Humanoid Robos, 2006. [10] V. Sanos, F. Silva, "Design and Low-Level Conrol of a Humanoid Robo Using a Disribued Archiecure Approach", Journal of Vibraion and Conrol, Vol., pp. 1431-1456, 2006. [11] S. Kagami, Y. Takahashi, K. Nishiwaki, M. Mochimaru, H. Mizoguchi,"High-speed marix pressure sensor for humanoid robo by using hin force sensing resisance rubber shee", Proceedings of IEEE Sensors Conference, 2004 [12] GLScene, hp://glscene.sourceforge.ne, 2000. [13] E. Harold and W. Means, XML in a Nushell, O'Reilly, 2004.