Configuration Space Distance Fields for Manipulation Planning

Con igu a ion Space Dis ance Fields o Manipula ion Planning Yiming Li1,2, Xuemin Chi1,3, Ami eza Razmjoo1,2, and Syl ain Calinon1,2 1Idiap Resea ch Ins i u e 2EPFL 3Zhejiang Uni e si y Abs ac —The signed dis ance ield (SDF) is a popula implici shape ep esen a ion in obo ics, p o iding geome ic in o ma ion abou objec s and obs acles in a o m ha can easily be combined wi h con ol, op imiza ion and lea ning echniques. Mos o en, SDFs a e used o ep esen dis ances in ask space, which co esponds o he amilia no ion o dis ances ha we pe cei e in ou 3D wo ld. Howe e , SDFs can ma hema ically be used in o he spaces, including obo con igu a ion spaces. Fo a obo manipula o , his con igu a ion space ypically co esponds o he join angles o each a icula ion o he obo . While i is cus oma y in obo planning o exp ess which po ions o he con igu a ion space a e ee om collision wi h obs acles, i is less common o hink o his in o ma ion as a dis ance ield in he con igu a ion space. In his pape , we demons a e he po en ial o conside ing SDFs in he obo con igu a ion space o op imiza ion, which we call he con igu a ion space dis ance ield (o CDF o sho ). Simila ly o he use o SDF in ask space, CDF p o ides an e icien join angle dis ance que y and di ec access o he de i a i es (join angle eloci y). Mos app oaches spli he o e all compu a ion wi h one pa in ask space ollowed by one pa in con igu a ion space (e alua ing dis ances in ask space and hen compu ing ac ions wi h in e se kinema ics). Ins ead, CDF allows he implici s uc u e o be le e aged by con ol, op imiza ion, and lea ning p oblems in a uni ied manne . In pa icula , we p opose an e icien algo i hm o compu e and use CDFs ha can be gene alized o a bi a y scenes. A co esponding neu al CDF ep esen a ion using mul ilaye pe cep ons (MLPs) is also p esen ed o ob ain a compac and con inuous ep esen a ion while imp o ing compu a ion e iciency. We demons a e he e ec i eness o CDF wi h plana obs acle a oidance examples and wi h a 7-axis F anka obo in in e se kinema ics and manipula ion planning asks. P ojec page: h ps://si es.google.com/ iew/cd mp/home I. INTRODUCTION Dis ances a e he mos undamen al and in ui i e me ics o exp essing he in e ela ion among mul iple a iables. In obo ics, hey a e ypically used o measu e he geome ic ela ionship among di e se ep esen a ions, such as poin s, poses, ajec o ies, su aces and shapes, which a e exploi ed in a ious asks including in e se kinema ics [22, 4] and manipula ion planning [18]. The signed dis ance ield (SDF) ep esen a ion has become a popula ep esen a ion, ha can o example be used o encode he Euclidean dis ance om a poin o an objec bounda y. The di e en iabili y and uni no m g adien p ope ies make i easy o in eg a e in o lea ning [41, 6, 37], op imiza ion [27, 28, 36], and con ol [5, 26, 17]. SDFs a e con en ionally employed in 3D ask spaces. In he con ex o manipula ion, a ypical con ol ask is composed o wo s eps, i s using an SDF in ask space o e alua e he dis ance o he objec , ollowed by an in e se kinema ics s ep o ind he join angle con igu a ion ha can educe his dis ance. Because o he nonlinea mapping be ween ask space and join space, his p oblem is ypically sol ed wi h a ew i e a ions by second-o de op imiza ion (co esponding o he use o a Jacobian pseudoin e se a each i e a ion). Figu e 1-le depic s his p ocess isualized in he con igu- a ion space, whe e he ull dis ance ield has been compu ed o be depic ed as colo ed le el se s (in p ac ice, we e alua e he o wa d kinema ics unc ion and associa ed Jacobian only a he cu en join con igu a ion o he obo ). While con en ionally employed in 3D ask spaces, SDFs can be conside ed in o he spaces, including obo join con igu a ion spaces in which planning and con ol p oblems ake place. We see in Figu e 1-le ha when we anspose an SDF om ask space o con igu a ion space, he p ope y o uni no m g adien disappea s, while in Figu e 1- igh , his p ope y is main ained. We will e e o his app oach as a Con igu a ion Space Dis ance Field (CDF), a scala ield measu ing he angula dis ance be ween join angles and he objec geome y in con igu a ion space (Fig. 1- igh ). Fo manipula ion asks, CDF di ec ly es ima es he minimum join mo ion equi ed by he obo o es ablish con ac wi h an objec , wi h g adien s consis en ly poin ing owa d he objec . Unlike SDFs in ask space, CDF is di ec ly o mula ed in con igu a ion space, p ese ing he Euclidean p ope y o he dis ance ield, ensu ing a uni o m span o dis ances and main aining uni magni udes in g adien di ec ions. CDF o e s se e al ad an ages. I na u ally b idges ask space and con igu a ion space, p o iding a uni ied app oach o sol ing p oblems ha adi ionally in ol e compu a ion in sepa a e spaces. Fo ins ance, he in e se kinema ics p oblem is usually sol ed by e alua ing he ask space dis ance and hen compu ing join space mo ions. In con as , CDF sol es his p oblem h ough one-s ep g adien p ojec ions, a oiding Gauss-New on i e a ions. I also o e s in ui i e geodesics ha e lec objec geome y in con igu a ion space, see Fig. 1. Fu he mo e, CDF inhe i s he me i s o SDFs, including im- plici s uc u e, Boolean ope a ions o composi ion in ol ing mul iple SDFs, e icien que ies, and di e en iabili y. We also p opose a neu al a ian called neu al CDF. Analogously o neu al SDFs, neu al CDFs also o e a compac , con inuous, analy ical, and la en space ep esen a ion, he eby acili a ing seamless in eg a ion in o lea ning, op imiza ion, and con ol amewo ks. Mos app oaches de eloped o SDFs can be di ec ly applied o CDF, which di ec ly sol es p oblems in con igu a ion space. a Xi :2406.01137 1 [cs.RO] 3 Jun 2024 (a) (b) Fig. 1: Di e ences be ween SDF and CDF. The colo ed le el se s depic he dis ances o he objec , whe e he black con ou s ep esen join angles leading o obo -objec su ace con ac . The ze o-le el-se o SDF and CDF is he same, bu he o he le el se s o CDF a e cha ac e ized by e enly expanded dis ances and uni no m g adien . This p ope y leads o g adien p ojec ion ha can di ec ly be compu ed, which sol es he in e se kinema ics p oblem o he con ac ask in one s ep (see ajec o ies in cyan and pink), whe eas SDF equi es mul iple i e a ions while encoun e ing singula i ies, which can e en ail due o g adien anishing (see ajec o y in yellow). Geodesics on he CDF na u ally w ap a ound he shape o he objec in con igu a ion space (see ajec o ies in blue, g een and pu ple). The o ganiza ion o his pape is as ollows. Sec ion II e iews he ela ed wo k. Sec ion III discusses he o mula ion, p ope ies, compu a ion and usion s a egy o CDF. Sec ion IV p esen s he implici neu al CDF ep esen a ion. Sec ion V and VI demons a e he e ec i eness o CDF in whole- body in e se kinema ics and manipula ion planning asks. Sec ion VII discusses and concludes he pape . The p ima y con ibu ions o his pape a e: •The in oduc ion o he CDF ep esen a ion, o e ing a uni ied amewo k o add essing obo manipula ion challenges wi hin con igu a ion space. •An e icien algo i hm o calcula ing he ze o-le el-se geome y o he objec in con igu a ion space, co e- sponding o he se o obo con igu a ions ha will lead o a collision. A subsequen usion s a egy is also p oposed o combine mul iple CDFs online, enabling he gene aliza ion o CDF o di e se scenes. •A neu al CDF a ian u ilizing a mul ilaye pe cep on (MLP), oge he wi h he design o he co esponding loss unc ions, esul ing in a concise and con inuous ep esen a ion. This a ia ion p o ides ade-o s ac oss e iciency, accu acy, and comp ession capabili ies while keeping a simple and lexible s uc u e. •Expe imen s compa ing CDF wi h SDFs and hei de i a- i es on a plana obo and a 7-axis F anka obo . The conduc ed expe imen s highligh he e iciency o CDF in sol ing in e se kinema ics asks h ough g adien p o- jec ion and i s e ec i eness in add essing manipula ion planning challenges, leading o he gene a ion o na u al obo ic mo ions. II. RELATED WORK SDFs ha e ga ne ed ex ensi e a en ion in he ield o compu e e sion and g aphics, pa icula ly in shape encod- ing [25, 9], mesh gene a ion [31], and di e en iable ende - ing [39, 20]. Thei e icacy in dis ance and g adien que ies has made hem popula in obo ics, wi h applica ions in planning [27, 45], mapping [10], and manipula ion [6]. Recen s udies p oposed o encode SDF wi h join angles [14], o lea n an SDF ep esen a ion o he swep olume o obo manipula o s [19], o model he SDF o a icula ed obo s and apply i o manipula ion asks [16]. The p e ailing ocus on SDFs in obo ics cen e s on ask space, whe e con igu a ion space ac ions a e ypically compu ed independen ly h ough mappings be ween he wo spaces [33, 29]. Exis ing app oaches o en model he con ig- u a ion space using bina y maps deno ing collision s a us o join con igu a ions [32, 42], o suppo sample-based mo ion planning algo i hms [43, 3, 11]. Despi e signi ican p og ess, hese con ol and planning s a egies a e compu a ionally ex- pensi e in high dimensional space due o he lack o g adien in o ma ion. In con as , conside ing a dis ance ield in he con igu a ion space in oduces new con ol and planning s a egies, by shi ing he ocus om con en ional bina y collision masks o con inuous and s uc u ed ep esen a ions. Fo ins ance, wi h his app oach, he in e se kinema ics p oblem simpli ies o an SDF pullback in con igu a ion space, equi ing only one- s ep g adien p ojec ion. Mo e gene ally, CDF enables he ansposi ion o SDF me hodologies de eloped o ask space o con igu a ion space. The compu a ion can be iewed as a poin -mass sys em, while obs acles o m opological holes in con igu a ion space and geodesics p oduce na u al cu ed pa hs a ound hem [28]. The app oach can be ex ended o geome ic mo ion planning amewo ks, including Riemannian mo ion policies [29], geome ic ab ics [38], and dynamic- awa e mo ion op imiza ion by assigning me ics on he dis- ance ield [13, 1]. III. CONFIGURATION SPACE DISTANCE FIELD (CDF) In his sec ion, we in oduce CDF and del e in o i s p ope - ies. We hen p esen an e icien algo i hm o compu e CDF, as well as a usion s a egy o online combina ion o mul iple CDFs. Algo i hm 1 Finding 0 le el-se con igu a ions Inpu : poin p, obo SDF model s Ou pu : join con igu a ion q′ ha sa is ies s(p,q′) = 0 Ini ializa ion: q←q0▷Ba ch ini ializa ion o = 1, . . . , T ▷ T i e a ions c←c(q)▷Compu e cos δq← −H−1∇qc ▷ Ba ch L-BFGS upda e q←q+αδq▷Line sea ch end q′←q: s(p,q)< ϵ ▷ Re u n inal con igu a ions A. P oblem Fo mula ion CDF is inspi ed by ecen wo k encoding SDF wi h obo join con igu a ions [17, 14, 16]. Le (q)deno e a obo a con igu a ion q∈Rnand p∈R3be a poin se in he obo wo kspace, o a obo wi h ndeg ees o eedom (DoF). The obo SDF sis a unc ion o pand q ha measu es he dis ance om p o he closes poin on he obo su ace ∂ (q)1: s(p,q) = ±min p′∈∂ (q)∥p−p′∥,(1) whe e ±indica es he sign o he dis ance, which is posi i e i pis ou side, ze o on he su ace, and nega i e o he wise. The di e en iabili y o he obo SDF wi h espec o bo h pand qenables a ious g adien -based manipula ion planning asks. The obo SDF ep esen a ion encodes he obo geome y h ough o wa d kinema ics, whe e he dis ance is Euclidean in he wo kspace bu highly nonlinea in con igu a ion space. In con as o using ask space dis ances, CDF is de ined as a unc ion c ha measu es he minimal dis ance in adians om q o ze o-le el-se join con igu a ions q′: s(p,q′) = 0 a p, which would es ablish con ac be ween he obo and he poin : c(p,q) = min q′∥q−q′∥.(2) This dis ance in adians co esponds o he mo emen o join angles, whe e he cons ain s(p,q′) = 0 implici ly sol es he in e se kinema ics p oblem by inding he con igu- a ion se q′on he ze o-le el-se o obo SDF model, gi en a poin p. CDF is unsigned acco ding o his de ini ion, as we ocus mo e on he alue o dis ance and g adien , whe e he sign can be de e mined ei he by combining i wi h SDF o by es ima ing he no mal di ec ion on bounda y samples. The de i a i e o CDF wi h espec o qco esponds o join eloci y. B. P ope ies o CDF An SDF sa is ies he eikonal equa ion ∥∇p s(p,q)∥= 1 almos e e ywhe e. Thus, he closes poin on he obo su ace o pcan be calcula ed by p ojec ing palong he g adien di ec ion: p′=p− s(p,q)∇p s(p,q).(3) 1All a iables suppo ba ch ope a ions, i.e. p∈Rb1×3and q∈Rb2×n accoun ing o s∈Rb1×b2. Fo su ace poin s, g adien s co espond o no mal di- ec ions. Simila ly, CDF sa is ies he eikonal equa ion ∥∇q c(p,q)∥= 1 almos e e ywhe e in con igu a ion space. The closes con igu a ion on he ze o-le el-se mani old can be ound by p ojec ing he cu en con igu a ion along he g adien di ec ion: q′=q− c(p,q)∇q c(p,q).(4) This p ope y makes CDF use ul in manipula ion planning asks. I allows o di ec compu a ion o ze o-le el-se join con igu a ions h ough g adien p ojec ion, e icien ly sol ing he in e se kinema ics p oblem in one-s ep compu a ion. Fo mo ion gene a ion asks, his implies ha ing a mo e s uc u ed dis ance ield in he con igu a ion space, whe e g adien s always poin owa d objec s o each o away om obs acles. Mo eo e , geodesics in he con igu a ion space will na u ally cu e a ound he ze o-le el-se s, which can o example be used o mo e a ound an objec while main aining a cons an join angle dis ance o he objec . F om a con ol pe spec i e, i means ha he objec emains eachable/a oidable wi hin he obo join angle eloci y limi s. C. Compu a ion o CDF The de i a ion o CDF is based on Eq. (2), in ol ing h ee componen s: 1. cons uc ing he SDF model so he obo ; 2. gi en a poin p, calcula ing ze o-le el-se con igu a ions q′ ha sa is y s(p,q′) = 0; 3. Gi en cu en join con igu a ion q, inding he closes con igu a ion on he ze o-le el-se q′, coupled wi h he calcula ion o he ℓ2no m dis ance o yield he CDF alue. We will discuss each s ep in de ail. 1) Robo SDF model: Va ious app oaches exis o calcu- la ing he signed dis ance om a poin in he obo wo kspace o he obo su ace. Ea ly app oaches in ol e ep esen ing he obo geome y using sphe es o meshes o app oxima e a coa se SDF. Recen in es iga ions employ deep neu al ne wo ks [17, 14] o encoding he obo SDF. We adop he me hod p esen ed in [16] ha exploi s kinema ic chains and basis unc ions o ep esen he obo SDF s, ading o accu acy and e iciency by p o iding a balance be ween explici and implici ep esen a ion. 2) Finding ze o-le el-se con igu a ions: The challenge o de e mining ze o-le el-se con igu a ions pa allels he in e se kinema ics (IK) p oblem. While IK only ocuses on he end- e ec o , CDF p o ides a mo e exp essi e app oach ha o- cuses on he whole obo geome y. We cas i as an op imiza- ion p oblem and employ he L-BFGS algo i hm [23]. L-BFGS is a quasi-New on me hod ha has demons a ed e ec i eness in obo mo ion planning [36]. The choice o L-BFGS is mo i a ed by i s ela i e simplici y and e icien pa alleliza ion. Al e na i e me hods based on Gauss-New on op imiza ion could also be chosen. We o mula e he cos unc ion as a squa ed sum o SDF alues, deno ed as c=P 2 s(p,q). The sea ch di ec ion is upda ed using s anda d L-BFGS s eps, and a line sea ch app oach is conduc ed o s able upda es. The al- go i hm, ou lined in Algo i hm 1, in ol es ini ializing a ba ch o join con igu a ions q, by concu en ly op imizing hem o es ablish dense ze o-le el-se con igu a ions. Addi ionally, ou SDF model p o ides he link index o he obo in con ac wi h he poin pa con igu a ion q′, o e ing aluable in o ma ion o subsequen compu a ions. 3) Re ie ing CDF alue: Gi en an inpu poin pand con igu a ion q, he p ocedu e ou lined in Sec ion III-C s ep 2 iden i ies ze o-le el-se con igu a ions q′co esponding o p. Calcula ing he CDF alue in ol es de e mining he closes q′ om q. Howe e , he spa se sampling o q′may esul in an o e ly smoo h CDF. To mi iga e his, we e o mula e (2) as c(p,q) = min k=1,...,K(min q′∥q:k−q′ :k∥),(5) whe e kdeno es he k h obo link in con ac wi h p,Kis he o al numbe o obo links, and q:k ep esen s all join con igu a ions be o e link k. This adjus men is oo ed in he obse a ion ha CDF is in luenced solely by p eceding join angles be o e he con ac link. This modi ica ion exploi s he inhe en kinema ic s uc u e o he obo , leading o a mo e accu a e app oxima ion o CDF and educing unce ain y, especially when q′samples a e limi ed. The co esponding g adien o CDF is exp essed as q′ min, kc= a g min q′,k ∥q:k−q′ :k∥, ∇q c(p,q) = q:k−q′ min,:kc ∥q:k−q′ min,:kc∥, (6) whe e q′ min is he closes join con igu a ion and kcis he co esponding con ac link. The g adien possesses a uni ℓ2 no m and poin s agains he di ec ion o he nea es join con igu a ion on he ze o-le el-se . D. Fusion o CDF The compu a ion o CDF desc ibed in Sec ion III-C is applicable o bo h single poin s and ba ches o poin s. How- e e , his p ocess ypically equi es 1–10 seconds o ind join con igu a ions and is scene-dependen . To add ess his chal- lenge, we in oduce a usion s a egy ha compu es he CDF independen ly o each poin and combines hem, yielding a scene-agnos ic CDF ep esen a ion conduci e o e icien online calcula ions. Speci ically, he poin cloud pwi h N poin s can be pa i ioned in o Msubse s (M≤N): p={p1,· · · ,pM},(7) whe e pi ep esen s a subse o pwi h Nipoin s. The CDF cis cons uc ed by using he CDFs i co each subse : c(p,q) = min i=1,...,M i c(pi,q).(8) Fo he ex eme case whe e M=N, each subse con ains only one poin , allowing o o line compu a ion and s o age. The online in e ence s age only in ol es sub ac ion and minimum ope a ions o use he CDFs based on he inpu , ensu ing simplici y and e iciency. Fo example, ini ializing he wo kspace in o a Ca esian g id, p e-compu ing co esponding join con igu a ions o each g id cell, and upda ing occupied cells du ing scene changes (see Figu e 2). This usion s a egy Fig. 2: Illus a ion o he compu a ion o CDF. Du ing he o line phase, we ini ialize he wo kspace o he obo as a olume ic g id and compu e ze o-le el-se join con igu a ions o each g id poin . Fo online compu a ion, gi en an objec O, we iden i y he closes con igu a ion in he se Qassocia ed wi h occupied g ids o calcula e he ℓ2dis ance. We u he encode he CDF wi h neu al ne wo ks o ob ain a compac and g id- ee ep esen a ion. enables a non-pa ame ic CDF ep esen a ion and can be gene alized o a bi a y en i onmen s. The usion o CDF also connec s o he union ope a ion o SDFs, albei pe o med in con igu a ion space. Consequen ly, o he Boolean ope a ions used o compose and ans o m SDFs can also be applied o CDF, such as sub ac ion, in e sec ion, epe i ion, and ounding. IV. NEURAL CONFIGURATION SPACE DISTANCE FIELD In his sec ion, we elabo a e on he ex ension o he CDF h ough a lea ning-based app oach o o mula e an implici ep esen a ion, e e ed o as neu al CDF. In con as o he online compu a ion de ailed in Sec ion III, employing neu al ne wo ks o CDF o e s addi ional ad an ages. I disen angles om spa ial esolu ion cons ain s, allowing o an exp essi e ep esen a ion wi h educed memo y equi emen s. The neu al CDF, being g id- ee, acili a es dis ance que ies be ween a bi a y join con igu a ions and poin s, enhancing lexibili y and e iciency. Addi ionally, i p esen s a con inuous ep esen- a ion, p o iding access o analy ical g adien s. Las ly, neu al CDF ope a es in la en space and se es as a ea u e ex ac o o downs eam asks. In summa y, neu al CDF in oduces ade-o s be ween accu acy, e iciency, and comp ession ca- pabili ies while enhancing lexibili y. Neu al CDF app oxima es he ba ched unc ion c(p,q) : Rb1×3×Rb2×n→Rb1×b2by lea ning he weigh s o a mul- ilaye pe cep on (MLP) ne wo k. I akes he conca ena ion o pand qas inpu , wi h size Rb1b2×(3+n)and ou pu s he CDF alue Rb1b2×1. The neu al CDF emains scene-agnos ic, allowing he s aigh o wa d online usion o di e en poin s. Subsequen sec ions will del e in o da a gene a ion p oce- du es, loss unc ion design, aining, and lea ning esul s. A. Da ase Gene a ion The da ase gene a ion p ocess aligns wi h he compu a- ional and usion p ocedu es de ailed in Sec ion III, comp ising bo h o line and online componen s. In he o line phase, we cons uc a T×T×T olume ic g id in he 3D obo wo kspace. U ilizing Algo i hm 1, join con igu a ions q′ ha sa is y s(p,q′)=0 o each g id poin pa e compu ed. Subsequen ly, a a hes poin sampling algo i hm is applied Algo i hm 2 Neu al CDF Da a Gene a ion Ini ializa ion: olume ic g id G ### o line da a o each p∈G:▷Fo each poin on he g id q′←q: s(p,q) = 0▷Find q′using Algo i hm 1 q′←Downsample(q′)▷Downsample q ### online da a o = 1, . . . , T ▷ I e a e o e T epochs p,q′←SampleO line() ▷Sample p,q′ om o line da a q←RandomSample() ▷Online sample q ha sa is ies join limi s Compu e c,∇q cusing (5) and (6) ▷G ound u h · · · Compu eLoss() ▷Ne wo k aining · · · end o o downsample he ob ained con igu a ions. The esul ing ze o- le el-se con igu a ions o each g id poin se e as empla es o online compu a ions. In he online phase, b1poin s and b2 join con igu a ions, andomly sampled wi hin join limi s, a e selec ed. The closes empla e is iden i ied, and he ℓ2no m dis ance is compu ed using (5). Simul aneously, he g adien conce ning he join con igu a ion is calcula ed. The da ase gene a ion p ocess is ou lined in Algo i hm 2. B. Loss Func ion We design a loss unc ion o aining he neu al CDF based on exis ing neu al SDF ep esen a ions [25, 9, 24]. The loss unc ion consis s o ou componen s: dis ance loss, g adien loss, eikonal loss and ension loss, each se ing a dis inc pu pose. Dis ance loss. The dis ance loss is cha ac e ized by he mean squa ed e o be ween he p edic ed CDF and he g ound u h, exp essed as Ldis =1 b1b2 b1 X i=1 b2 X j=1 ˆ c(pi, qj)− c(pi, qj)2,(9) whe e ˆ cand cdeno e he p edic ed and g ound u h C-space dis ances o poin piand con igu a ion qj, espec i ely. G adien loss. This e m cons ains he g adien o he p edic ed CDF o consis en ly poin agains he di ec ion o he closes join con igu a ion on he ze o-le el se . I employs cosine simila i y loss o penalize de ia ions, gi en by Lg ad =1 b1b2 b1 X i=1 b2 X j=1 1−∇qˆ c(pi, qj)⊤∇q c(pi, qj) ∥∇qˆ c(pi, qj)∥ ∥∇q c(pi, qj)∥!. (10) Eikonal loss. This e m egula es he p edic ed CDF by encou aging i s g adien s o ha e a uni ℓ2no m. This egu- la iza ion, inspi ed by he eikonal pa ial di e en ial equa ion, ensu es a alid signed dis ance ield [9, 24]. The eikonal egula iza ion e m is o mula ed as Leikonal =1 b1b2 b1 X i=1 b2 X j=1 ∥∇qˆ c(pi, qj)∥ − 1.(11) Tension loss. The ension loss e m aims o egula ize he cu a u e o he CDF, p omo ing smoo hness. I penalizes he squa ed sum o he Laplacian, which measu es he second de i a i es o he p edic ed CDF [12, 44], namely L ension =1 b1b2 b1 X i=1 b2 X j=1 ∥∇2 qˆ c(pi, qj)∥2,(12) whe e ∇2 qis he Laplacian ope a o compu ed ia au oma ic di e en ia ion. To al loss. The ne wo k is op imized o minimize he weigh ed sum o he ou loss e ms L o al =λ1Ldis +λ2Lg ad +λ3Leikonal +λ4L ension,(13) In he expe imen s, we se λ1= 5.0, λ2= 0.1, λ3= 0.01, λ4=0.01. C. Implemen a ion De ails Fo he aining o he neu al CDF model, we employ a simple ully connec ed MLP. To assess i s e ec i eness in handling high-dimensional inpu s, we e alua e he neu al CDF on a 7-axis F anka obo . The esolu ion o he olume ic g id Tis se o 20 o da a gene a ion. The inpu dimension is 3+7, and he ou pu co esponds o he con igu a ion space dis ance. In line wi h p e ious wo k [14], we adop a 5- laye MLP a chi ec u e, whe e he inpu da a is en iched wi h posi ion encoding [20]. Du ing aining, we andomly sample b1= 4000 poin s wi h co esponding join con igu a ions and b2= 100 con igu a ions. Thus, he ba ch size is 4000 ×100. The ne wo k is ained o 50,000 epochs using he Adam op imize wi h a lea ning a e o 0.001, decayed by a ac o o 0.5. The aining p ocess spans app oxima ely 2hou s on a single NVIDIA RTX 3090 GPU. D. Lea ning Resul s We e alua e he ained neu al CDF model h ough a comp ehensi e e alua ion o bo h accu acy and e iciency. The esul s a e p esen ed in Table I. Speci ically, we un he o wa d pass o he ne wo k, which ou pu s p edic ed C- space dis ance alues c o inpu pai s pand q. Then we compu e he g adien ia au oma ic di e en ia ion and p ojec con igu a ions q o poin s palong g adien di ec ion using (4). Acco ding o he de ini ion o CDF, he dis ance be ween he obo su ace, de ined by p ojec ed con igu a ions qp oj o inpu poin s q, should be 0. Thus, we measu e he mean absolu e e o (MAE) and oo mean squa ed e o (RMSE) as me ics. The success a e (SR) deno es he pe cen age o con igu a ions success ully p ojec ed o inpu poin s wi hin a h eshold o 3cm. The p ojec ion p ocess is designed o un i e a i ely o imp o ed accu acy. Each expe imen in ol es he andom sampling o 1000 poin s and 1000 con igu a ions ( he esul s a e epo ed as a e ages). The ou comes e eal ha ou model (a) (b) (c) (d) Fig. 3: Compa ison be ween CDF and SDF in sol ing whole-body in e se kinema ics p oblem. (a) The ini ial sampled join con igu a ions. (b) G adien p ojec ion by CDF. (c) Task space isualiza ion o easible solu ions in (b). (d) Resul s o dis ance que y-based me hod wi h L-BFGS op imize . We can see ha wi h he baseline SDF app oach, he sys em can ge s uck when he g adien o he SDF anishes o eaches he singula i y. TABLE I: Accu acy and compu a ion ime (GPU / CPU) o Neu al CDF on he F anka obo . Accu acy Compu a ion Time P ojec ion MAE (cm) RMSE (cm) SR(%) Ba ch In e ence P ojec ion I e a ions Size Time (ms) Time (ms) 14.99±1.93 8.59±3.15 60.3±12.20 10.49/0.37 0.71/0.34 21.64±0.62 2.80±1.20 87.8±9.50 10 0.51/0.59 0.72/0.66 31.39±0.51 2.09±0.94 91.1±8.12 1020.56/0.95 0.75/1.03 41.36±0.49 2.00±0.89 91.6±8.23 1030.58/10.20 0.97/5.48 51.34±0.48 1.92±0.79 91.8±8.31 1040.79/25.00 1.01/36.00 10 1.35±0.52 1.89±0.80 91.6±8.39 1054.61/329.00 11.30/310.00 accu a ely p edic s CDF alues and g adien s, acili a ing a p ojec ion p ocess ha success ully iden i ies he closes join con igu a ions on he ze o-le el-se . S abili y is achie ed a e 2 i e a ions. As o compu a ion ime, esul s a e p o ided o a single NVIDIA RTX 3090 GPU and a 30-co e 2.2GHz CPU. In e ence ime deno es he du a ion o a single o wa d pass o he ne wo k, while p ojec ion ime encompasses he ime o au oma ic di e en ia ion and he p ojec ion p ocess. These esul s unde sco e he e iciency, high pa allelizabili y, and scalabili y o ou neu al CDF, pa icula ly when dealing wi h la ge ba ch sizes. V. CDF FOR WHOLE-BODY INVERSE KINEMATICS CDF inhe en ly encodes he kinema ic s uc u e o he obo , o e ing a solu ion o he in e se kinema ics p oblem h ough g adien p ojec ion wi hou he need o i e a i e p ocedu es. Gi en i s holis ic modeling o he obo geome y, ou app oach ex ends he in e se kinema ics p oblem o whole- body in e se kinema ics p oblems, ins ead o only ocusing on he end-e ec o . We assess ou app oach wi h a plana obo and he 7-axis F anka obo . A. 2-DoF Plana Robo We s a wi h a s aigh o wa d example in ol ing a 2D plana obo wi h link leng hs l1=l2= 2 and join limi s q1, q2∈[−π, π]. The CDF is compu ed online using he me hodology ou lined in Sec ion III. Two ci cula objec s wi h adii 1= 0.8and 2= 0.5a e posi ioned a (1.8,−1.8) and (−2.0,3.0), espec i ely. The objec i e o he whole- body in e se kinema ics ask is o iden i y join con igu a ions ha make he obo each he objec s. We compa e ou CDF ep esen a ion wi h SDF and p esen quali a i e esul s in Figu e 3. The esul s demons a e ha in his 2D scena io, CDF e ec i ely sol es he p oblem h ough a one-s ep g adien p ojec ion, while SDF-based op imiza ion s uggles o ind solu ions when he g adien anishes and ge s s uck in local minima due o he nonlinea i y o he o wa d kinema ics unc ion. B. 7-DoF F anka Robo To u he e alua e he pe o mance o CDF, we conduc ed expe imen s wi h a 7-axis F anka obo , u ilizing he ained neu al CDF model ou lined in Sec ion IV. The e alua ion ocused on he whole-body in e se kinema ics pe o mance o a ious a ge poin s, employing 10′000 andomly ini ialized con igu a ions. The g adien p ojec ion p ocess was i e a i ely pe o med in h ee s eps o enhance pe o mance. Fo com- pa ison, wo baseline ep esen a ions we e included in he e alua ion: he whole-body SDF ep esen a ion p oposed in [16] and he neu al join space SDF ep esen a ion (Neu al- JSDF) p oposed in [14]. Bo h app oaches a e ollowed wi h an L-BFGS algo i hm o op imiza ion, which is also desc ibed in cuRobo [36], achie ing s a e-o - he-a pe o mance. Expe - imen s a e epea ed 100 imes and a e age esul s a e shown in Table II. CDF demons a ed he abili y o compu e o e 700′000 alid solu ions pe second, ou pe o ming he s a e- o - he-a dis ance que y-based app oach by 180 imes, which could only ind 3′700 solu ions. Addi ionally, he in e ence and p ojec ion p ocess o CDF ook only 1−2milliseconds, wi h he p ima y ime cos a ibu ed o he pos -p ocessing o dis ance checking o selec alid solu ions ha sa is y he e o h eshold. Figu e 4 shows he esul s o a 1-s ep p ojec ion o di e en a ge poin posi ions. C. Applica ions We demons a e wo applica ions ha le e age he g adien p ojec ion capabili ies o CDF. The i s applica ion in ol es a Fig. 4: G adien p ojec ion o whole-body in e se kinema ics using neu al CDF. The cen e s o he ed sphe es a e a ge poin s, whe e he adius o sphe es is se o 0.05m. Fig. 5: Goalkeepe ask in simula ion. Fig. 6: Planned con igu a ions o each he box. The i s image shows he ini ial con igu a ions o he wo a ms. TABLE II: Compa ison o CDF and SDFs in whole-body in e se kinema ics ask wi h a 7-axis F anka obo . CDF sol es 8773 solu ions in 10.6ms while he SDF based me hod only inds 3652 solu ions in 971 ms. Me hods Valid Solu ions Time (ms) CDF + 1-s ep P ojec ion 6089 8.72 CDF + 2-s ep P ojec ion 8773 10.60 CDF + 3-s ep P ojec ion 9163 12.70 SDF + L-BFGS op imize 3652 971.00 Neu al-JSDF + L-BFGS op imize 264 272.00 goalkeepe ask whe e he obo in e cep s a h own ball using i s a m links. The second one is a dual-a m li ing ask exploi - ing he whole-body s uc u e o he obo o es ablish con ac wi h a la ge box, which is ha d o accomplish con en ionally using an end-e ec o . 1) Goalkeepe ask: In con as o asks in ol ing apid obo esponses o a oid obs acles, he p esen ask en ails he obo ac ing as a goalkeepe , u ilizing i s a m o in e cep a p opelled ball. This ask poses inc eased di icul y as he obo mus p omp ly de e mine a whole-body in e se kinema ics so- lu ion and ansi ion o he equisi e con igu a ion o in e cep he ball. The expe imen al con igu a ion is ou lined as ollows: (1) a ec angula goal, measu ing 0.8min wid h and 0.6min heigh , is posi ioned behind he obo , whose con igu a ion is ini ialized in he middle o he join angle ange; (2) a ball is h own owa d he goal om he on o he obo wi h a andomly assigned di ec ion and eloci y; (3) he obo is asked o mo e i s a m o in e cep he ball. The conduc ed e alua ions a e pe o med in a simula ed en i onmen , wi h he assump ion ha he obo can only pe cei e he cu en posi ion o he ball, necessi a ing swi mo emen s o ensu e an e ec i e de ense. A join posi ion con olle is employed o go e n he obo a m, di ec ing i o he designa ed join con igu a ion. The ask is execu ed 100 imes, esul ing in an 82% success a e o ou CDF ep esen a ion. In compa ison, he SDF-based me hod achie es a success a e o only 35%. Snapsho s o ou app oach a e depic ed in Fig. 5. 2) La ge box li ing: The objec i e o his ask is o plan join con igu a ions o wo obo a ms o es ablish con ac wi h a designa ed box. We assume ha he con ac poin s on he box a e p ede ined, and he obo s can use any su ace poin s on hei body o es ablishing con ac . This ask yp- ically in ol es a mul i-objec i e op imiza ion p oblem wi h cons ain s, including join limi s, collision a oidance, and goal- eaching. The combina ion o hese objec i es in oduces non-con exi y and makes he p oblem ha d o sol e. Le e ag- ing he e icien and pa allelizable g adien p ojec ion inhe en o CDF, we ins ead p esen a s aigh o wa d sample- il e app oach o add ess his p oblem. Speci ically, we i e a i ely sample a ba ch o ini ial con igu a ions, p ojec hem on o he con ac poin s, and il e ou con igu a ions in collision wi h he box o iola ing join limi s. This p ocess con inues un il easible solu ions sa is ying all cons ain s a e iden i ied. An e alua ion o ou app oach, compa ed wi h he Gauss-New on op imiza ion me hod ou lined in [16], is p esen ed in Table III. The esul s indica e ha CDF educes he planning ime by a ac o o 7and gene a es sho e pa hs. Du ing he li ing phase, he Jacobian ma ix o he con ac poin w. . . he join con igu a ion is compu ed. A join impedance con olle is used in he expe imen . Quali a i e esul s a e shown in Fig. 6. TABLE III: Compa ison esul s on la ge box li ing ask. Me hods Planning Time(s) A e age Dis ance( ad) CDF + Fil e 7.65 1.37 SDF + Op imize 54.80 2.85 VI. CDF FOR MANIPULATION PLANNING In his sec ion, we in es iga e he use o CDF o ma- nipula ion planning asks. The key ad an age o CDF is he s uc u ed ep esen a ion ha alle ia es challenges a ising om nonlinea i y and singula i y, making mo ion op imiza ion in con igu a ion space easie . Simila ly o con en ional SDF, CDF p o ides e icien que ies o dis ances and g adien s, enabling la ge-scale pa allel compu a ion. To demons a e i s e icacy, we ini ially explo e quali a i e esul s h ough 2- DOF examples and hen p og ess o 7-DOF obo scena ios, including eal-wo ld expe imen s. A. Benchma k App oaches and E alua ion me ics We e alua e he CDF ep esen a ion on se e al g adien - based mo ion op imiza ion app oaches: 1) Quad a ic p og amming: We i s o mula e he mo ion planning ask as eac i e quad a ic p og amming (QP) p ob- lem, d awing inspi a ion om he wo k o Mi aza i e al. [21]. The QP o mula ion is: u∗ k= a g min q,u e(qk)⊤He(qk) + uk ⊤Ruk,(14a) s. . qk+1 =Aqk+Buk,(14b) qk∈ Q,uk∈ U,(14c) − ∇q c(p,q)uk∆ ≤ln( c(p,q) + γ),(14d) whe e qkand uka e he s a e and con ol inpu a ime s ep k, Hand Ra e he posi i e de ini e ma ices o acking e o s and con ol e o s, e(qk) = qk−qdesi e is he e o ec o be- ween he ini ial and goal con igu a ions, c(p,q)is he CDF, ∆ is he ime s ep, γis a scala hype pa ame e ha ac s as a sa e y bu e , and Qand Ua e he admissible s a e and con ol cons ain s. The cons ain (14d) ensu es collision a oidance, whe e he obo is allowed o ge close o he obs acle when a away, and i is o ced o ollow he angen o no mal di ec ion o he g adien ield when close. Fo implemen a ion, we use he CasADi [2] lib a y and sol e i wi h popula sol e s including OSQP [35], qpOASES [7]. Nonlinea p og amming sol e s like IPOPT [40] and QRQP [8] a e also es ed. 2) i e a i e Linea Quad a ic Regula o (iLQR): Ano he benchma k app oach in ol es employing an i e a i e Linea Quad a ic Regula o ha sol es he op imal con ol p oblem by i e a i ely linea izing he dynamics and cos unc ion a ound he cu en ajec o y [15]. The dynamic sys em is de ined as ∆qk+1 =A∆qk+B∆uk. We minimize he cos unc ion c(q,u) = e(qK)⊤Q1e(qK) + K−1 X k=1 h(qk)⊤Q2h(qk) + uk ⊤Ruk (15) whe e Q1and Q2a e p ecision ma ices o acking e - o s and collision a oidance, Ris he con ol e o ma ix. h(qk) = min( c(p,q)−γ, 0) ep esen s he collision a oidance e m. The solu ion o iLQR can be compu ed ei he in ba ch o ecu si e o m, see [1] o de ails. 3) Geome ic ab ics: Geome ic ab ics is a eac i e accele a ion-based con ol policy ¨ q=π(q,˙ q).¨ qis compu ed h ough he mo ion o equa ion M¨ q+F= 0, whe e M(q,˙ q)and F(q,˙ q)model he gene alized mass ma ix and ex e nal o ces based on posi ions and eloci ies , see [30] o de ails. The obs acle a oidance geome y is de ined as h=λ∥˙ q∥2∇qψ( c(p,q)). When he obo ge s close o he obs acle, he alue ψ(p,q)inc eases and epels he obo away om he collision bounda y. The geome ic ab ics a e adap ed om he open-sou ce implemen a ion o op imiza ion ab ics [34]. Fo a comp ehensi e e alua ion, we es ima e bo h CDF and SDF. The implemen a ion o SDF is achie ed by eplacing he dis ance unc ion cwi h s. Se e al e alua ion me ics a e adop ed o compa ison. •Success Ra e: he success a e shows he pe cen age o collision- ee ajec o ies gene a ed while eaching he goal. As he e alua ion in ol es andomly sampling ini ial and goal con igu a ions, o he cases when all algo i hms ailed, he co esponding samples we e excluded when epo ing he success a e. •T acking E o : As eac i e app oaches may ge s uck a a local minimum, we in oduce he acking e o o measu e he inal ℓ2no m dis ance be ween he inal con igu a ion and he desi ed con igu a ion. •Time S ep: The ime s ep deno es he a e age numbe o ime s eps he agen equi es o each he goal con igu a- ion. B. Plana Robo Tes Fo he 2D expe imen , we ollow he p e ious sec ion ha de ined a 2D plana obo wi h link leng hs l1=l2= 2 and join limi s q1, q2∈[−π, π]. Two ci cle obs acles wi h adius o 0.3a e placed a (2.3,−2.3) and (0.0,2.45). We andomly sample ini ial and goal con igu a ions o 100 cases and epo he expe imen al esul s in Table IV. I shows ha he app oach based on CDF has a highe success a e and be e acking accu acy han SDF. The a e age numbe o ime s eps is smalle , inc easing e iciency. To u he in es iga e he (a) CDF C-Space (b) SDF C-Space (c) CDF T-Space (d) SDF T-Space Fig. 7: CDF/SDF-based mo ion planning app oaches. Di e en me hods a e shown in di e en colo s. We also demons a e how he sa e y bu e a ec s he planning esul s o CDF and SDF. TABLE IV: Expe imen s o mo ion planning on SDF and CDF. 2D - CDF 2D - SDF 7D - CDF 7D - SDF Success Ra e T acking E o (cm) Time S ep Success Ra e T acking E o (cm) Time S ep Success Ra e T acking E o (cm) Time S ep Success Ra e T acking E o (cm) Time S ep IPOPT 92% 1.27 253 51% 2.19 284 93% 0.98 231 51% 1.12 290 QRQP 94% 1.16 245 52% 2.07 279 91% 0.99 226 38% 1.31 297 OSQP 88% 1.21 300 40% 2.19 321 91% 0.92 279 48% 1.04 301 qpOASES 91% 1.04 241 63% 1.74 263 93% 0.99 229 51% 1.13 289 Geome ic Fab ics 95% 1.03 - 68% 1.76 - 88% 2.02 - 74% 2.18 - iLQR 76% 0.06 - 55% 0.12 - 48% 0.02 - 38% 0.03 - mechanism behind his, we isualize some cases in Fig. 7- a,b. I shows ha he s uc u ed dis ance ield can bene i all app oaches men ioned abo e, as hei ajec o ies ollow he geodesics o CDF. In con as , he nonlinea i y o he con igu a ion space when using he con en ional SDF makes he op imize ge s uck in o local minima. Addi ionally, we isualize he planning esul s o IPOPT wi h di e en sa e y bu e s γ anging om 0.1 o 0.9. The planne exhibi s a mo e conse a i e beha io as he alue o γdec eases o ensu e sa e y. Wi h CDF, he planned ajec o y scaled well wi h di e en γ. In con as , o SDFs, he planne only inds a solu ion when γ= 0.9and he ajec o y in join space is e y close o he obs acle. I is also e lec ed in he ask space (Fig. 7-c,d), whe e he obo eaches a singula i y when i is close o he obs acle. C. 7-axis F anka Robo Expe imen s We u he conduc expe imen s on he F anka obo o demons a e he e ec i eness o CDF. We place se e al di e en obs acles such as sphe es, walls, and ings, wi h andomly sampled ini ial and goal con igu a- ions. Fo a ai compa ison, we use he neu al ep esen a ion o bo h CDF and SDF. Expe imen s a e also epea ed 100 imes and we epo he a e age esul s in Table IV. The CDF- based planne s demons a e be e pe o mance han SDF- based app oaches in e ms o success a e, acking e o and numbe o ime s eps. Fo QP con olle s, ou me hods can un o e 200Hz equency, hanks o he high e iciency o neu al ne wo ks. Fo eal-wo ld expe imen s, we se up wo di e en sce- na ios: s a ic and dynamic en i onmen s. In he s a ic en i- onmen , we place some obs acles in he obo wo kspace, such as blocks ( o simple cases) and a shel ( o ha d cases being highly non-con ex). We use a RealSense D435 came a o cap u e he poin cloud o he obs acles. Fo dynamic en i onmen s, we simply de ec he obs acle acco ding o he HSV colo and con e i o poin clouds. The same QP con olle wi h IPOPT op imize is used o mo ion planning. Fo s a ic scenes, we wai o he QP con olle o compu e he ull ajec o y and hen execu e i on he obo . Fo dynamic scenes, we es he eac i e mo ion gene a ion and send he con ol commands online. Quali a i e esul s a e shown in Fig. 8, showing he e ec i eness o ou app oach. VII. DISCUSSION AND CONCLUSION In his pape , we p oposed o conside he geome y o obo con igu a ion space as a dis ance ield, and p esen a new ep esen a ion called CDF o desc ibe he opological s uc u e o objec s in con igu a ion space. A e discussing he o mula ion and p ope ies, we in oduced an e icien algo i hm o compu e and use CDFs. An implici neu al ne wo k encoding was also p oposed o balance he accu acy, e iciency, and comp ession o CDF. We demons a ed he e ec i eness o CDF wi h plana examples and wi h a 7-axis F anka obo in whole-body in e se kinema ics and mo ion planning asks. CDF se es as a ep esen a ion implici ly encoding in e se kinema ics h ough a dis ance ield. Bo h Ca esian g id and neu al ne wo k ep esen a ions acili a e he o line compu- a ion o in e se kinema ics, hus enabling e icien online que ies. In con as , SDFs can be pe cei ed as ep esen a ions encoding he o wa d kinema ics o a obo . The pi o al s ep in CDF compu a ion in ol es iden i ying ze o-le el-se con igu a ions om a gi en poin o objec . Re- cen ad ancemen s in di e en iable obo SDF ep esen a ions ha e pa ed he way o e icien , accu a e, and pa allelizable