Optmzaton-based Moton Retargetng Integratng Spatal and Dynamc Constrants for Humanod Thomas Moulard, Ech Yoshda and Shn chro Nakaoka CNRS-AIST JRL (Jont Robotcs Laboratory), UMI38/CRT ({thomas.moulard, e.yoshda}@ast.go.p) Humanod Research roup (s.nakaoka@ast.go.p) Natonal Insttute of Advanced Industral Scence and Technology (AIST) Tsukuba Central, -- Umezono, Ibarak 305-8568 Japan (Tel: 8-9-86-708, Fax: 8-9-86-6507) Abstract-- In ths paper, we present an optmzatonbased retargetng method for precse reproducton of captured human motons by a humanod robot. We take nto account two mportant aspects of retargetng smultaneously: spatal relatonshp and robot dynamcs model. The former takes care of the spatal relatonshp between the body parts based on nteracton mesh to follow the human moton n a natural manner, whereas the latter adapts the resultng moton n such a way that the dynamc constrants such as torque lmt or dynamc balance are beng satsfed. We have ntegrated the nteracton mesh and the dynamc constrants n a unfed optmzaton framework, whch s advantageous for generaton of natural motons by a humanod robot compared to prevous work that performs those processes separately. We have valdated the basc effectveness of the proposed method wth a sequence of postures converted from captured human data to a humanod robot. Index Terms-- Humanod, Retargetng, Optmzaton. I. INTRODUCTION One of the advantages of human-sze humanod robots s ts ablty to generate whole-body motons mantanng smlar dynamcs to humans. Ths ablty allows a humanod robot to serve as an entertaner lke a dancer of an actor [], or also to use varous machnes and devces desgned for humans []. As an extenson of the latter use, a new applcaton has recently been studed: a humanod robot as an evaluator of human assstve devces [3], [4]. If a humanod reproduces human motons fathfully, t can be used to test the devces nstead of human subects. Ths brngs several benefts such as no need for ethcal process, repeated test wth exactly the same motons under the same condtons, and qualtatve evaluaton through sensory measurement lke torque and force. It has been demonstrated that the human- sze humanod HRP-4C [5] can evaluate the effect of load reducton quanttatvely by estmatng motor torque [4], takng an example of a supportve wear called Smart Sut Lte [6] desgned to reduce load at the lower back wth embedded elastc bands. The mportant ssue n those applcatons s how to generate natural motons of a humanod robot. There have been a number of studes on moton retargetng technques n or- der to generate humanod motons based on those of a human measured by a moton capture system. Retargetng captured moton to humanods has been actvely studed durng the last decade, thanks to the progress of ther dynamc capablty. The work of Pollard [7] s one of the poneerng studes that enable reproducton of human motons by a humanod, n ths case the upper body of Sarcos humanod robot, by takng nto account varous constrants. Nakaoka et al. developed a technque to transfer Japanese tradtonal dancng motons to a humanod by ntroducng a noton of leg task model [8], [9]. Mura et al. [0] devsed a walkng pattern generator that allows the humanod robot HRP-4C to walk n a manner extremely close to humans, ncludng stretched knees, swng- leg traectory and sngle support phase on the toe. Other mtaton methods have been proposed based on a dynamc controller [], [], moton recognton and prmtves [3] and extracton of upper-body moton from markerless moton nput [4], [5]. On the other hand, moton retargetng has been nvestgated ntensvely n computer graphcs doman, typcally to generate motons for new characters based on moton capture data usng space-tme constrants solver [6]. Recently, Ho et al. proposed a new retargetng method called nteracton mesh that preserves the spatal relatonshp between closely nteractng body parts and obects n the envronments [7]. Nakaoka and Komura extended ths method for retargetng to a humanod robot by takng advantage of ts capacty to adapt motons to a character wth hghly dfferent physcal propertes [8]. Usage of nteracton mesh brngs natural followng of orgnal human moton and self-collson avodance. Although ths method ncludes balance consderaton by shftng the wast, ths approach remans specfc nstead of general whole-body moton optmzaton and does not deal wth dynamc constrants such as torque lmts. In addton, those constrants are treated separately after generatng re- targeted moton to adapt to the humanod. Another related doman s the optmzaton technque that s more and more employed to generate robot traectores mnmzng certan cost functon under mechancal or dynamc constrants. Mossec et al. appled nonlnear optmzaton to dynamc whole-body moton lke a kckng moton of a human-sze humanod [9]. Recently the optmzaton s utlzed for generaton of mult-contact dynamc moton through modelng of dynamc constrants usng Taylor expanson [0]. For dynamc traectory optmzaton for dgtal human, an
Fg.. The two step retargetng process. The fnal nverse knematcs step s only here to convert the fnal moton to onts traectory. effcent algorthm has been proposed [], []. Suleman et al. proposed another traectory optmzaton technque based on Le algebra that allows effcent computaton through analytc ntegraton of dynamcs [3] and appled t to human moton mtaton [4]. The latter research ams at optmzng the humanod traectory to be as close as human moton, but selfcollson avodance s ncorporated as a post-processng to the optmzed moton lke prevous work [8]. Ths s dsadvantageous because separate applcaton of collson avodance may lead to unnatural motons. Methods n prevous research are, therefore, stll lackng the capablty to optmze humanod motons by takng nto account the assocated problems for retargetng n a unfed manner. The man contrbuton of the paper s the optmzaton process ntegratng retargetng, dynamcs constrants and self-collson at the same tme, n order to create the humanod moton as close to human motons as possble. In ths paper, we address ths retargetng by formulatng t as a nonlnear optmzaton problem under spatal and dynamc constrants. Frst, the captured moton s pre-processed to provde better ntal guess for the optmzaton. Then the full optmzaton problem s solved consderng the spatal relatonshp and dynamc robotc constrants smultaneously. The spatal relatonshp between body parts of captured moton s reserved by usng nteracton mesh as ntroduced by [7], whch acheves self-collson avodance n consequence. Ths paper s organzed as follows. Secton II descrbes the overall method and n partcular how the retargetng s performed. Secton III descrbes the detals of each step of the optmzatonbased retargetng method. Secton IV ntroduces RobOptm, an optmzaton framework for robotcs used to mplement the proposed method. Secton V presents the results of retargetng wth valdaton of dynamc smulatons, before concludng the paper. to ft the marker postons to the target robot structure, to obtan the ntal state that helps the optmzaton process converge quckly. Ths geometrc problem can be represented as a lnear problem usng the nteracton mesh [7] wth a quadratc cost renderng ts resoluton extremely effcent. The man optmzaton process s ntalzed usng the result of the frst step and then takes the full problem nto consderaton. Ths optmzaton ncorporates robotc constrants such as moton balance and torque constrants together wth the spatal relatonshp modeled also by nteracton mesh. The prevous work [8] adopts a two-step approach for ths optmzaton: frst the markers were optmzed before adaptng the moton by optmzng ont angles for the robot n order to satsfy robotc constrants. In contrast, n the proposed method the optmzaton varables are the marker poston throughout the retargetng process untl t s converted to ont angles at the last step. Ths s advantageous to make the constrant matrx sparse [7] and also to mantan a global optmzaton framework unfyng all the constrants smultaneously. Inverse knematcs computaton of the target robot s employed n order to estmate the nearest robot confguraton from the marker postons at each tme step. Ths allows evaluatng robotc constrants such as dynamc balance or ont torque lmt by usng deduced ont velocty and acceleraton. The robotc constrants beng non-lnear, ths man optmzaton problem s much more computatonally ntensve. The thrd post-processng step s converson of the resultant marker postons nto the robot ont confguratons that s done n a straghtforward manner usng nverse knematcs. II. METHOD OVERVIEW The proposed retargetng framework conssts of three steps as shown n Fg. and takes a tme seres of captured marker postons to generate a retargeted robot ont traectory. The man part of the retargetng s the second step, whch s the man optmzaton process whereas the frst and last steps are addtonal data processng for effcent optmzaton and moton converson respectvely. The frst pre-processng takes care of ntal guess for the optmzaton. It takes care of retargetng problem only Fg.. Examples of nteracton mesh representng the spatal relatonshp between body parts, appled to a dgtal character and a humanod [8].
III. OPTIMIZATION-BASED RETARETIN As descrbed n the prevous secton, our retargetng method has two man components: moton retargetng and robotc moton generaton that are ntegrated n the optmzaton. Ths secton wll detal ther computatonal aspects. A. Moton retargetng The retargetng algorthm employed n the frst two steps n the framework of Fg. reles on the noton of nteracton mesh to ensure that spatal relatonshp between bodes s preserved. We wll brefly descrbe how t s ncorporated n the optmzaton based on prevous work by Ho et al [7]. By applyng Delaunay Tetrahedralzaton [5] on the marker set, one can generate a mesh whch s parameterzed by the marker postons V = (p p m ) where n. n denotes here the number of frame composng the moton and m the number of markers n each frame. p represents the poston of the frst marker n the -th frame. ven a partcular nteracton mesh, one can compute the Laplacan Coordnate of one marker as follows: L( p ) = p w p () l N In Eq. (), N s the one-hop neghborhood of the marker n the nteracton mesh and w l s the weght of the marker l when computng the Laplacan Coordnate of marker. Ths weght s nversely proportonal to the dstance between and l. Consderng these two notons, t s possble to ntroduce the Laplacan Deformaton Energy assocated to a marker set whch serve as a cost functon n ths problem: E L ( ) = L( p ) l V L( p ) () The Laplacan Deformaton Energy s the square of the norm of the dfference between the Laplacan Coordnates of the orgnal marker set and the updated marker set V = (p p m ). In practce, ths cost functon penalzes moton of hghly connected markers whereas solated ones wll move for a lower cost. A second cost functon s added to the frst one to smooth the moton. To acheve ths goal, the marker set acceleraton energy s consdered: EA( V, V, V + ) = V V + V (3) + V -, V, V + beng the new marker set poston for the frame, and +. Acceleraton energy s always null for frst and last frame. The fnal cost functon C s by consequence expressed by: C( V, V, V ) E ( V ) α E ( V, V, V ) (4) + = L + A + where α s the weghtng coeffcent. Addtonally, a lnk length constrant s defned. Ths constrant ams l at retargetng the moton so that t fts the robot morphology. It s defned wth the target length l e of a lnk e as: e e p p = l (5) where p e and p e are the end vertces of the lnk e. Whenever there s a need for some part of the robot body to stay fxed, an optonal postonal constrant s also provded through an equalty constrant: V = P (6) for 0 m, wth P beng a vector representng desred postons. The quadratc problem s then solved to generate a new set of marker postons. The goal of the frst preprocessng step n Fg. s to obtan the marker postons that ft the robot structure suffcently well. Of course, the resultng moton may stll not be feasble due to physcal constrants. The output marker postons from the preprocessng s used as the ntal guess for the man optmzaton process consderng robotc constrants and retargetng at the same tme, as explaned n the next subsecton. B. Robotcs moton generaton We here defne a non-lnear optmzaton problem usng the moton derved wth the pre-processng. The same optmzaton varables, namely the set of marker postons, are used also wth the cost functon and the constrants ntroduced n the prevous secton. By keepng the prevous constrants, one can make sure that the good propertes ensured by the prevous preprocessng are kept durng ths optmzaton step. Moreover, the constrant matrx can be kept sparse by usng the marker postons as optmzaton varables [7], whch makes the optmzaton computaton effcent, whereas the ont Jacoban matrx for the robot s dense. To ensure that the moton s feasble by the robot, two addtonal constrants are added n the man optmzaton process: Dynamc balance constrant. Jont angle, velocty and torque constrants. We here need robot confguraton q from the marker postons to calculate the followng constrants wth dynamc property lke mass and moment of nerta of each lnk. Snce the output marker postons from the pre-processng are close enough to the feasble robot confguraton, we can beneft from the ont angle fttng usng damped least square method ntroduced n [8]. Fgure 3 shows the knematc structure of the humanod HRP-4C [5] the proposed retargetng s appled to. Once the robot confguratons are obtaned, the followng constrants are evaluated on the numercal bass, by usng ont velocty and acceleraton computed wth fnte dfference. The frst constrant constrans the ZMP (zero moment pont) poston so that t stays nto the robot support polygon: e
x y ZMP ZMP = x = y ( σ + mz y ( σ + mz x x ) m( z y ) m( z + g) + g) Fg. 3. Humanod robot HRP-4C and ts structure. where (x Z M P, y Z M P ) s the ZMP poston on the ground wth the coordnate system shown n Fg. 3, σ the varaton of the angular momentum around the center of mass, and (x, y, z ) the center of mass poston. The gravtatonal constant s denoted by g. Here we assume that the robot s movng on a flat floor. The ZMP acts as a crteron that allows decdng whether a moton can be executed stably or not. As long as t stays nsde the convex hull of the robot contact ponts wth the floor, the moton s dynamcally stable. Knowng whch foot of the robot s n contact wth the floor and the foot geometry, t s possble to nsert ths as an nequalty constrant (x Z M P, y Z M P ) S (x rf oot, y lf oot ) (8) to make sure that the ZMP s stayng nsde the current support polygon, denoted by S (x rfoot, y lfoot ) that s determned by the rght and left postons (x rf oot, y lf oot ). The second robotc constrant we take nto account n ths problem s the ont lmtatons. ven (q, q, q ), the set of torques τ = (τ,, τ o ) appled to each robot ont can be computed usng the classcal equaton of moton: (7) M(q)q + C(q, q ) + (q) = τ (9) where M(q) s the system mass matrx, C(q, q ) s the vector of Corols and centrfugal forces and (q) the vector of gravtatonal forces. Other robotc constrants for lmts of ont rotaton, velocty and torque are expressed as: qmn q qmax d q dq dq (0) mn τ mn τ τ max As mentoned earler, the output of the man optmzaton process s the set of marker postons. As we have already appled the robotc constrants, the resultant moton can be easly converted to the ont traectory to be executed by the robot through nverse knematcs. max IV. IMPLEMENTATION OF THE OPTIMIZATION PROBLEM USIN ROBOPTIM RobOptm s a general framework to assst the development and resoluton of optmzaton problems appled to robotcs. The optmzaton s more and more appled to robotcs feld to solve complex problems of moton plannng and generaton wth many constrants, as mentoned earler. Although number of state-of-theart optmzaton solver tools and lbrares are now avalable, they are not necessarly ready for mmedate use for robotcs. RobOptm has been developed to allow robotcsts to prototype ther optmzaton applcatons easly by provdng necessary nterfaces specfc to robotc problems n the form of C++ lbrares. It has a threelayer archtecture: the core, the solver and the applcaton layers. The core layer provdes a way to defne mathematcal functon and ther assocated dervatves, whle the solver layer encapsulates dfferent state-ofthe-art solvers so that they can solve problems defned usng the representaton proposed by the core layer. The applcaton layer contans dedcated mathematcal functons whch can be embedded nto dfferent optmzaton problems. An overvew of the framework archtecture s shown n Fg. 4. The core layer offers some useful hgher-level tools that help users defne the functons such as costs and constrants ntroduced n the prevous secton. By mplementng those functons nherted from the basc mathematcal functons of the core layer, RobOptm ensures the compatblty wth a number of state-of-theart solvers whose plug-ns are provded n the solver layer. Those tools nclude functon defnton tself and also dfferentaton of the functons that can be computed analytcally, or numercally when no analytcal gradent s provded by the user. Even f the gradent s provded, numercal dfferentaton can be used to ensure the computaton correctness. The user can beneft from the transparency of those hgher level tools to prototype ther problem wthout thnkng about ndvdual solvers used for optmzaton. Fg. 4. RobOptm archtecture.
Although not really utlzed n our problem of retargetng, the applcaton layer s convenent especally for plannng purpose. One of the practcal tools s the traectory toolbox, whch allows representaton of robot moton usng B-splne. Snce these are assocated wth mathematcal functons of the core layer and others lke of mnmal-tme optmzaton. RobOptm s dstrbuted as an open-source lbrary (LPL-3) through ts webste: http://www.roboptm.net/ V. RETARETIN RESULTS The proposed algorthm has been appled to a wholebody moton taken from CMU moton capture database [6]. We have ntegrated a RobOptm plug-n for nonlnear optmzaton tools of NA optmzaton lbrary [7] to solve the problem whle usng sparse matrces computaton. Ths s partcularly useful for traectory optmzaton where large matrces are nvolved as some constrants are only consderng one frame and thus assocated Jacoban are contanng a large proporton of null values. Fg. 5. The marker set before (purple markers) and after (whte markers) the ntal preprocessng phase. Fg. 7. ZMP values durng the moton sequence of 0 frames. In order to valdate ts basc capacty of dynamc constrant consderaton, we have appled the proposed method to a sequence of several human postures to be converted nto stable humanod confguratons. In ths valdaton we use the Laplacan Deformaton Energy E L n Eq. () only wth α = 0 n Eq. (4), wth dynamc balance constrant wth ZMP of Eq. (7). The support polygon S (x rf oot, y lf oot ) for the stablty s the square of 5 cm around the center of feet. The result after the optmzaton process s llustrated by Fg. 5 (preprocessng) and Fg. 6 (fnal result). In Fg. 5 we can observe that the markers postons are dsplaces to ft to the robot whose sze s much smaller than the dgtal character. In the optmzed posture n Fg. 6(b), the confguraton s modfed so that robotc constrants such as ont lmts or lnk length can be satsfed. We also verfed the dynamc balance durng the moton sequence by computng the resultant ZMP as shown n Fg. 7. As can be seen, the ZMP stays nsde the support polygon of the area ±7.5 cm along each x and y axs as specfed n the constrants. Although the profle n Fg.7 s a bt shaky as we have not yet ntegrated Acceleraton Energy E A n Eq. (3), the balance constrant tself s satsfed. We therefore beleve that smoother traectory wll be obtaned as the optmzed traectory by takng account Acceleraton Energy. Fg. 6. (a) (b) Optmzed posture for HRP-4C before (a) and after (b) the optmzaton. VI. CONCLUSION Ths paper presented a unfed approach combnng retargetng and robotcs constrants nto one sngle nonlnear optmzaton problem. For effcent computaton, a three-step approach s adopted ncludng pre- and post-processng of the moton. After obtanng pre-processed marker postons approxmately ftted to the robot structure, the man optmzaton process generates also marker motons that respect the orgnal spatal relatonshp of body parts as much as possble based on nteracton mesh, by satsfyng the robotc dynamc constrants throughout the moton. The optmzaton output moton can be converted to the robot traectory n a straghtforward manner by the last step.
The fundamental effectveness of the proposed method has been valdated by convertng a sequence of human postures nto humanod confguratons by mnmzng the cost of spatal relatonshp wth robotc constrants. Future work ncludes extensons to take nto account dfferent cost functons such as human-lkelness, or addtonal constrants such as collson avodance or others dependng on the task of the humanod. Applcaton to walkng moton retargetng wll also be addressed n future work. ACKNOWLEDMENT Ths research was partally supported by the Japan Socety for the Promoton of Scence (JSPS; rant-n-ad for JSPS Fellows P803). REFERENCES [] S. Nakaoka, S. Kata, and K. Yoko, Intutve and Flexble User Interface for Creatng Whole Body Motons of Bped Humanod Robots, n Proc. on IEEE Int. Conf. on Intellgent Robots and Systems, 00, pp. 675 68. [] K. Yoko, K. Nakashma, M. Kobayash, H. Mhune, H. Hasunuma, Y. Yanaghara, T. Ueno, T. okyuu, and K. Endou, A tele-operated humanod robot drves a backhoe, n Proc. 003 IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, 003, pp. 7. [3] Y. Ogura, H. Akawa, K. Shmomura, H. Kondo, A. Morshma, H. ok Lm, and A. Takansh, Development of a new humanod robot WABIAN-, n Proc. of 006 IEEE Int. Conf. on Robotcs and Automaton, 006, pp. 76 8. [4] K. Mura, E. Yoshda, Y. Kobayash, Y. Endo, F. Kanehro, K. Homma, I. Katan, Y. Matsumoto, and T. Tanaka, Humanod robot as an evaluator of assstve devces, n Proc. 03 IEEE Int Conf. Robotcs and Automaton, 03, pp. 67 677. [5] K. Kaneko, F. Kanehro, M. Morsawa, K. Mura, S. Nakaoka, and K. Yoko, Cybernetc human HRP-4C, n Proc. 009 IEEE-RAS Int. Conf. on Humanod Robots, 009, pp. 7 4. [6] Y. Imamura, T. Tanaka, Y. Suzuk, K. Takzawa, and M. Yamanaka, Moton-based-desgn of elastc materal for passve assstve devce usng musculoskeletal model, Journal of Robotcs and Mechatroncs, vol. 3, no. 6, pp. 978 990, 0. [7] N. Pollard, M. R. Jessca Hodgns, and C. Atkeson, Adaptng human moton for the control of a humanod robot, n Proc. of 00 IEEE Int. Conf. on Robotcs and Automaton, 00, pp. 390 397. [8] S. Nakaoka, A. Nakazawa, K. Yoko, and K. Ikeuch, Leg Moton Prmtves for a Dancng Humanod Robot, n Proc. IEEE Int. Conf. on Robotcs and Automaton, 004, pp. 60 65. [9] S. Nakaoka, A. Nakazawa, F. Kanehro, K. Kaneko, M. Morsawa, H. Hrukawa, and K. Ikeuch, Learnng from Observaton Paradgm: Leg Task Models for Enablng a Bped Humanod Robot to Imtate Human Dances, Int. J. of Robotcs Research, vol. 6, pp. 89 844, 007. [0] K. Mura, M. Morsawa, F. Kanehro, S. Kata, K. Kaneko, and K. Yoko, Human-lke walkng wth toe supportng for humanods, n Proc. 0 IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, 0, pp. 448 4435. [] K. Yamane, S. O. Anderson, and J. K. Hodgns, Controllng humanod robots wth human moton data: Expermental valdaton, n Proc. 0 IEEE-RAS Int. Conf. on Humanod Robots, 0, pp. 504 50. [] O. E. Ramos, L. Saab, S. Hak, and N. Mansard, Dynamc moton capture and edton usng a stack of tasks, n Proc. 0 IEEE-RAS Int. Conf. on Humanod Robots, 0, pp. 4 30. [3] C. Ott, D. Lee, and Y. Nakamura, Moton capture based human moton recognton and mtaton by drect marker control, n Proc. 008 IEEE-RAS Int. Conf. on Humanod Robots, 008, pp. 399 405. [4] B. Darush, M. enger, A. Arumbakkam, C. oerck, Y. Zhu, and K. Fumura, Onlne and markerless moton retargetng wth kne matc constrants, n Proc. 008 IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, 008, pp. 9 98. [5] M. Do, P. Azad, T. Asfour, and R. Dllmann, Imtaton of human moton on a humanod robot usng non-lnear optmzaton, n Proc. 008 IEEE-RAS Int. Conf. on Humanod Robots, 008, pp. 545 55. [6] M. lecher, Retargettng moton to new characters, n Proc. RAPH 98, 998, pp. 33 4. [7] E. S. Ho, T. Komura, and C.-L. Ta, Spatal relatonshp preservng character moton adaptaton, ACM Trans. on raphcs, vol. 9, no. 4, pp. 33: 8, 00. [8] S. Nakaoka and T. Komura, Interacton mesh based moton adaptaton for bped humanod robots, n Proc. 0 IEEE-RAS Int. Conf. on Humanod Robots, 0, pp. 65 63. [9] S. Mossec, K. Yoko, and A. Kheddar, Development of a software for moton optmzaton of robots applcaton to the kck moton of the hrp- robot, n Proc. 006 IEEE Int. Conf. on Robotcs and Bommetcs, 006, pp. 99 304. [0] S. Lengagne, J. Vallant, E. Yoshda, and A. Kheddar, eneraton of whole-body optmal dynamc mult-contact motons, Internatonal Journal of Robotcs Research, 03, to appear. [] Y. Tassa, T. Erez, and E. Todorov, Synthess and stablzaton of complex behavors through onlne traectory optmzaton, n Proc. 0 IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, 0, pp. 4906 493. [] T. Erez and E. Todorov, Traectory optmzaton for domans wth contacts usng nverse dynamcs, n Proc. 0 IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, 0, pp. 494 499. [3] W. Suleman, E. Yoshda, J.-P. Laumond, and A. Monn, On humanod moton optmzaton, n Proceedngs of 7th IEEE-RAS Internatonal Conference on Humanod Robots, December 007, pp. 80 87. [4] W. Suleman, E. Yoshda, F. Kanehro, J.-P. Laumond, and A. Monn, On human moton mtaton by humanod robot, n Proc. 008 IEEE Int Conf. Robotcs and Automaton, 008, pp. 697 704. [5] H. S and K. ärtner, Meshng pecewse lnear complexes by constraned delaunay tetrahedralzatons, n Proc. 4th Internatonal Meshng Roundtable, 005, pp. 47 63. [6] Carnege-Mellon unversty moton capture database, http://mocap.cs.cmu.edu/. [7] Numercal algorthms group, http://www.nag.co.uk/.