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