Tactile Ergodic Coverage on Curved Surfaces

1 Tac ile E godic Co e age on Cu ed Su aces Cem Bilaloglu∗, Tobias Löw∗, and Syl ain Calinon Abs ac —In his a icle, we p esen a eedback con ol me hod o ac ile co e age asks such as cleaning o su ace inspec ion. Al hough hese asks a e challenging o plan due o he com- plexi y o con inuous physical in e ac ions, he co e age a ge and p og ess can be e ec i ely measu ed using a came a and encoded in a poin cloud. We p opose an e godic co e age me hod ha ope a es di ec ly on poin clouds, guiding he obo o spend mo e ime on egions equi ing mo e co e age. Fo obo con ol and con ac beha io , we use geome ic algeb a o o mula e a ask-space impedance con olle ha acks a line while simul aneously exe ing a desi ed o ce along ha line. We e alua e he pe o mance o ou me hod in kinema ic simula ions and demons a e i s applicabili y in eal-wo ld expe imen s on ki chenwa e. Ou sou ce codes, expe imen ideos, and da a a e a ailable a h ps://si es.google.com/ iew/ ac ile-e godic-con ol/. Index Te ms—Tac ile Robo ics, E godic Co e age, Geome ic Algeb a I. INTRODUCTION The long- e m ision o obo ics is o assis humans wi h daily asks. The success o obo acuum cleane s and lawn- mowe s as consume p oduc s highligh s he po en ial o obo ic assis ance o common household cho es [1]. These asks in ol e co e ing a egion in a epe i i e and exhaus- i e manne . Cu en ly, hese obo s a e limi ed o ela i ely la ge, plana su aces, and e en na iga ing slopes emains challenging [2], [3]. O he daily asks, such as washing dishes o g oce y i ems, p esen e en g ea e challenges due o he complex physical in e ac ions wi h in ica e, cu ed su aces. Simila ly, nume ous co e age asks on cu ed su aces a ise in indus ial and medical applica ions. In indus ial se ings, such asks include su ace ope a ions ha emo e ma e ial, such as sanding [4], polishing [5], [6] o debu ing [7] as well as su ace inspec ion asks le e aging con ac [8]. In medical se ings, simila applica ions ange om mechanical palpa ion [9], [10] and ul asound imaging [11], [12] o massage [13], [14] and bed ba hing [15], [16]. Las bu no leas , da ase s combining he ac ile p ope ies o objec s wi h hei shape and isual appea ance emain sca ce and expensi e o collec , as hey ely on eleope a ion [17]. Thus, ac ile co e age is c i ical o au oma ing he collec ion o ac ile da ase s ha complemen isual ones. The p oblem de ini ions o his di e se ange o se ings and applica ions can be dis illed in o wo key equi emen s: (i) ac ile in e ac ions wi h This wo k was suppo ed by he S a e Sec e a ia o Educa ion, Resea ch and Inno a ion in Swi ze land o pa icipa ion in he Eu opean Commis- sion’s Ho izon Eu ope P og am h ough he INTELLIMAN p ojec (h ps: //in elliman-p ojec .eu/, HORIZON-CL4-Digi al-Eme ging G an 101070136) and he SESTOSENSO p ojec (h p://ses osenso.eu/, HORIZON-CL4- Digi al-Eme ging G an 101070310). ∗Equal con ibu ion. The au ho s a e wi h he Idiap Resea ch Ins i u e, Ma igny, Swi ze land and wi h he Ecole Poly echnique Fédé ale de Lausanne (EPFL), Swi ze land. [email p o ec ed]; [email p o ec ed]; [email p o ec ed] Di usion Co e age G adien ield Measu e co e age Upda e a ge Co e age Loop Vision Tac ile Con ol Fo ce Fig. 1: O e iew o ou eedback con ol me hod o ac ile co e age. Le : We measu e he su ace and he ed a ge using he came a and encode hem in a poin cloud. Bo om- igh : We di use he a ge and use i s g adien ield o guide he co e age. Then, we close he loop by measu ing he ac ual co e age wi h he came a and use i as he nex a ge . Top- igh : We measu e he ac ile in e ac ion o ces using he o ce senso and he ool o ien a ion using he join posi ions. We sol e he geome ic ask-space impedance con ol p oblem using a line a ge and a o ce a ge along he line. a possibly non-plana su ace and (ii) a con inuous ajec o y o con ac poin s co e ing a egion o in e es on he su ace. Acco dingly, his a icle add esses he o e a ching p oblem o ac ile co e age on cu ed su aces. Tac ile asks in ol e mul iple con ac in e ac ions wi h he en i onmen , making hese sys ems no o iously di icul o con ol [18]. While humans sol e hese asks e o lessly, hey emain ex emely challenging o obo s. Fo ins ance, when cleaning an objec , achie ing adequa e co e age depends on ecognizing di , unde s anding he objec ’s ma e ial, and assessing hei in e ac ion o de e mine he equi ed con ac o ce o emo al. Consequen ly, he success o co e age de- pends on unknown o di icul - o-measu e pa ame e s, making i challenging o model all in e ac ions. Wi hou an accu a e model, mo ion planning is p one o ailu e. By analyzing p e- ious esea ch [19] and obse ing how humans add ess hese challenges, we a gue ha humans bypass he complexi ies o planning by sol ing he simple closed-loop con ol p oblem. Humans le e age isual and ac ile eedback o online adap a- ion. Simila ly, obo s can measu e p og ess in ac ile co e age asks using ision, u ning he ask o iden i ying unco e ed egions in o an image segmen a ion p oblem which has been add essed using a ious model-based [20], [21] o lea ning- based algo i hms [16], [22]. Howe e , de e mining how o con ol a obo o co e hese a ge egions on cu ed su aces emains an open challenge. a Xi :2402.04862 3 [cs.RO] 31 Ma 2025 2 Exis ing esea ch on co e age has p ima ily ocused on co e age pa h planning, which in ol es op imizing a pa h o ensu e ha a speci ied egion o in e es is co e ed wi hin a se ime ame. T adi ionally, he unde lying assump ion is ha isi ing each poin in he egion o in e es only once is su icien o ull co e age, an assump ion ha is easonable o simple in e ac ions, bu no o many ac ile asks. Tac ile in e ac ions a e o en oo complex o model de e minis ically, making i challenging o ensu e ull co e age a e a single isi . Ins ead, o a cleaning ask, a ela i ely di y egion equi es mo e isi s compa ed o a less di y egion. Simila ly, in a su ace inspec ion ask, egions equi - ing highe p ecision demand mo e isi s o compensa e o senso unce ain y. Fu he mo e, he obo is expec ed o keep in con ac wi h he su ace while mo ing, which signi ican ly inc eases he cos o mo emen . This cos depends on he geodesic dis ance on he su ace a he han he Euclidean dis ance. The e o e, nai e sampling s a egies ha ail o accoun o he cos o cons ain s o mo emen and/o su ace geome y a e unsui able o ac ile co e age asks. In con as , e godic co e age [23] con ols he ajec o ies o dynamical sys ems by co ela ing he a e age ime spen in a egion o he a ge spa ial dis ibu ion. The e o e, e godic co e age inco po a es he mo ion model as he sys em dynamics and di ec ly con ols he co e age ajec o ies by using he spa ial dis ibu ion measu ed by he ision sys em. Conside ing hese challenges, we p esen a closed-loop ac ile e godic con ol me hod ha ope a es on poin clouds o ac ile co e age asks. Using poin clouds enables us o acqui e he a ge objec and spa ial dis ibu ion a un ime using ision, measu e co e age p og ess, and compensa e o un- modeled dynamics in ac ile co e age asks. Ou me hod hen cons ains he e godic con ol p oblem o a bi a y su aces o co e a a ge spa ial dis ibu ion on he su ace. We p opaga e co e age in o ma ion by sol ing he di usion equa ion on poin clouds, which we compu e in eal- ime by exploi ing he su ace’s in insic basis unc ions called Laplacian eigen- unc ions. These eigen unc ions gene alize he Fou ie se ies o mani olds (i.e., cu ed spaces). In o de o exe a desi ed o ce on he su ace while mo ing, we o mula e a geome ic ask-space impedance con olle using geome ic algeb a. This con olle uses su ace in o ma ion o ack a line a ge ha is o hogonal o he su ace while simul aneously exe ing he desi ed o ce in he di ec ion o ha line. No ably, he geome ic o mula ion ensu es ha hese wo objec i es do no con lic wi h each o he and can he e o e be included in he same con ol loop wi hou equi ing exhaus i e pa ame e uning. In summa y, ou p oposed closed-loop ac ile e godic con ol me hod o e s he ollowing con ibu ions: • o mula ing he ac ile co e age as closed-loop e godic con ol p oblem on cu ed su aces; •closing he co e age loop by sol ing e godic con ol p oblem on poin clouds using di usion; •achie ing eal- ime equencies by compu ing he di u- sion using Laplacian eigen unc ions; •con ac line and o ce acking wi hou con lic ing objec- i es. The es o he a icle is o ganized as ollows. Sec ion II desc ibes ela ed wo k. Sec ion III p o ides he ma hema ical backg ound. Sec ion IV p esen s ou me hod. In Sec ion V, we demons a e he e ec i eness o ou me hod in simula ed and eal-wo ld expe imen s. Finally, we discuss ou esul s in Sec ion VI. II. RELATED WORK The majo i y o he co e age me hods conside he p oblem om a planning pe spec i e and a e gene ally known as co e age pa h planning (CPP) algo i hms [24]–[26]. Al hough hese me hods can handle plana egions wi h a ious bound- a ies [27]–[29], hei ex ension o cu ed su aces imposes lim- i ing assump ions, such as p ojec i ely plana [30] o pseudo- ex uded su aces [31]. Addi ionally, CPP me hods assume ha he co e age a ge is uni o mly dis ibu ed in space. Ex ending CPP me hods o accoun o spa ial co ela ions in he in o ma ion leads o in o ma i e pa h planning (IPP) [32]. Mos IPP and CPP app oaches add ess a a ian o he NP-ha d a eling salesman p oblem [33], which limi s hei scalabili y as domain complexi y inc eases. Consequen ly, exis ing me h- ods a e ei he open-loop [34] o impose limi ing assump ions, such as con exi y, o online planning upda es [32]. Closely ela ed o co e age is he p oblem o explo a ion, whe e he en i onmen is ini ially unknown, and obo s ga he in o ma ion using onboa d senso s [35], [36]. Tac ile ex- plo a ion is pa icula ly necessa y o ga he ing in o ma ion on su aces ha can only be acqui ed h ough con ac [37]. A no able example is non-in asi e p obing (palpa ion) o issue s i ness, which aids in disease diagnosis o su ge y by p o iding addi ional ana omical in o ma ion. Fo his pu pose, Gaussian p ocesses (GP) ha e been used o disc e e [9] and con inuous [38] p obing o map issue s i ness. While GP- based app oaches e ec i ely guide sampling loca ions, hey do no accoun o he obo ’s dynamics. This limi a ion was la e add essed by using ajec o y op imiza ion o ac i ely sea ch o issue abno mali ies [39]. Unlike o he sensing modali ies ha depend solely on posi ion, ac ile in e ac ions also depend on condi ions such as ela i e eloci y and con ac p essu e [40]. To add ess his, me hods ha e been de eloped o model o ces [41] and mo e complex in e ac ions be ween obo ic ools and su aces [42]. The complexi y o he p oblem inc eases u he i we conside scena ios wi h a obo physically in e ac ing wi h he en i onmen . Fo example, in asks like su ace inishing (e.g., polishing, sanding, g inding), he su ace i sel changes, as ma- e ial is emo ed [43]. Simila ly, in cleaning asks, he obo ’s ac ions a ec he dis ibu ion o di on he su ace [44]. To a oid complex modeling, he e a e app oaches ei he elying on ein o cemen lea ning [45] o deep lea ning [46]. In a e y simila se ing o ou s, a manipula o was used o clean he s ains on a cu ed su ace by pe o ming mul iple passes [20]. Howe e , his wo k used a sampling-based planne , which equi ed o p ede ine he maximum numbe o cleaning passes. In con as , we ela e he a ge dis ibu ion (e.g., s ain) di- ec ly o eedback con ol wi hou equi ing any ask-speci ic assump ions. 3 In ac ile co e age scena ios, isi ing a egion once can no gua an ee ull co e age, and p edic ing how many imes he obo should e isi a pa icula spo is challenging. Conse- quen ly, de ining a ime ho izon o ajec o y op imiza ion is di icul , as he quali y o he esul would be signi ican ly a ec ed by his ha d- o-make choice. Ins ead, e godic con ol ela es how o en he obo should e isi a pa icula spo o he a ge densi y a ha spo . In his con ex , e godic desc ibes a dynamical sys em in which he ime a e ages o unc ions along i s ajec o ies a e equal o hei spa ial a e ages [47]. The key ad an age o e godic con ol is i s abili y o handle a bi a y spa ial a ge dis ibu ions wi hou equi ing a p ede ined ime ho izon. When he spa ial a ge dis ibu ion is measu able, he e godic con olle can use his eedback o di ec he sys em o isi egions wi h highe spa ial p obabili ies mo e equen ly. Recen indings ha e demons a ed ha e godici y is no me ely a heu is ic [48]; i is he op imal me hod o collec ing independen and iden ically dis ibu ed da a while accoun ing o sys em dynamics. E godic con ol was in oduced in he seminal wo k by Ma hew and Mezi´ c [23], which p esen ed he spec al mul- iscale co e age (SMC) algo i hm. SMC is a eedback con ol law based on he Fou ie decomposi ion o he a ge dis ibu- ion and obo ajec o ies, whe e mul iscale aspec p io i izes low- equency componen s o e high- equency ones, co e- sponding o s a ing wi h la ge-scale spa ial mo ions be o e e ining ine de ails. Since his beha io is achie ed h ough a myopic eedback con olle a he han an o line planne , he e godic con olle emains e ec i e e en when mo ion is obs uc ed [49]. Recen wo ks ha e adap ed SMC’s objec i e wi hin a ajec o y op imiza ion amewo k o inco po a e addi ional objec i es, such as obs acle a oidance [50], ime- op imali y [51] and ene gy-awa eness [52]. E godic con ol has been used o ac ile co e age and explo a ion in applica ions such as non-pa ame ic shape es ima ion [53] and able clean- ing h ough lea ning om demons a ion [54]. Howe e , all hese o mula ions, which ely on he Fou ie decomposi ion- based e godic me ic, a e limi ed o ec angula domains in Euclidean space. The i s a emp o ex end he e godic con ol o Rie- mannian mani olds [55] u ilized Laplacian eigen unc ions, which gene alize he Fou ie se ies o cu ed spaces. Howe e , his app oach was es ic ed o homogeneous mani olds, such as sphe es and o i, whe e closed- o m exp essions o he Laplacian eigen unc ions a e a ailable. Mo e ecen ly, he ke nel e godic me ic [56] was in oduced as an al e na i e o SMC’s e godic me ic, enabling ex ensions o Lie g oups and o e ing imp o ed compu a ional scalabili y. None heless, a bi a y cu ed su aces collec ed using senso s, such as poin clouds, lack bo h he g oup s uc u e and he homogeneous mani old p ope ies, p esen ing addi ional challenges. Ano he al e na i e o SMC is he hea equa ion-d i en a ea co e age (HEDAC) algo i hm [57], which uses he di - usion equa ion, a second-o de pa ial di e en ial equa ion (PDE), o p opaga e in o ma ion abou unco e ed egions o agen s ac oss he domain. Simila o SMC, he o iginal HEDAC implemen a ion was es ic ed o ec angula domains and lacked collision a oidance. Subsequen ex ensions ha e adap ed HEDAC o plana meshes wi h obs acles [58], maze explo a ion [59], and CPP on non-plana meshes [60]. How- e e , i s applica ion on cu ed su aces emains limi ed o meshes and o line planning due o he hea y p e-p ocessing equi ed. In addi ion o i s use in HEDAC, he di usion equa ion is widely used in geome y p ocessing asks, anging om geodesic compu a ion [61] o lea ning on su aces [62]. I s key ad an age lies in i s abili y o accoun o su ace geome y while emaining agnos ic o he unde lying ep esen a ion and disc e iza ion [62]. The di usion equa ion is go e ned by a second-o de di e en ial ope a o called he Laplacian which can be compu ed o a bi a y su aces ep esen ed as meshes o poin clouds using a ious disc e iza ion schemes [63]–[65]. In his wo k, we use a ecen app oach p oposed by Sha p e al. which p o ides a obus and e icien implemen a ion [66], capable o handling pa ial and noisy poin clouds. III. BACKGROUND A. E godic Con ol using Di usion The e godic con ol objec i e co ela es he ime ha a co e age agen spends in a egion o he p obabili y densi y speci ied in ha egion. The HEDAC me hod [57] encodes he co e age objec i e in he domain x∈Ωa ime using a i ual sou ce e m s(x, ) = max p(x)−c(x, ),02,(1) whe e p(x)is he p obabili y dis ibu ion co esponding o he co e age a ge and c(x, )is he no malized co e age o he N i ual co e age agen s o e he domain c(x, ) = ˜c(x, ) RΩ˜c(x, )dx.(2) A single agen ’s co e age is he con olu ion o i s oo p in φ( )wi h i s ajec o y xi( ′). Then, he o al co e age becomes he ime-a e aged sum o hese con olu ions ˜c(x, ) = 1 N N X i=1 Z 0 φx−xi( ′)d ′.(3) HEDAC di uses he sou ce e m ac oss he domain Ωand compu es he po en ial ield u(x, )using he s a iona y (˙u(x, ) = 0) di usion α∆u(x, )−u(x, ) + s(x, )=0,(4) wi h he di usion coe icien α > 0and he Laplacian ope - a o ∆. The Laplacian is a second-o de di e en ial ope a o which educes o he sum o he second pa ial de i a i es in Euclidean spaces ∆ =∇·∇ = n X i=1 ∂2 ∂x2 i ,∀x∈Rn.(5) The s a iona y di usion (4) go e ns he po en ial ield wi hin he in e io o he domain Ω, while he beha io on he bounda y ∂Ωis dic a ed by he ze o-Neumann bounda y condi ion n· ∇u(x, ) = 0,∀x∈∂Ω,(6) 4 whe e n ep esen s he ou wa d uni no mal ec o o he bounda y ∂Ω. To guide he i- h co e age agen , HEDAC u ilizes he smoo h g adien ield o he di used po en ial u(x, )and simula es i s -o de dynamics [67] ˙ xi=∇u(xi, ).(7) B. Con o mal Geome ic Algeb a He e, we in oduce con o mal geome ic algeb a (CGA) wi h a ocus on he ma hema ical backg ound necessa y o unde s and he me hods used in his a icle. We will use he ollowing no a ion h oughou he pape : x o deno e scala s, x o ec o s, X o ma ices, X o mul i ec o s and X o ma ices o mul i ec o s. The inhe en algeb aic p oduc o geome ic algeb a is called he geome ic p oduc ab =a·b+a∧b,(8) which ( o ec o s) is he sum o an inne ·and an ou e ∧ p oduc . The inne p oduc is he me ic p oduc and he e o e depends on he me ic o he unde lying ec o space o e which he geome ic algeb a is buil . The unde lying ec o space o CGA is R4,1, which means he e a e ou basis ec o s squa ing o 1 and one o -1. The ou e p oduc , on he o he hand, is a spanning ope a ion ha e ec i ely makes subspaces o he ec o space elemen s o compu a ion. These subspaces a e called blades. In he case o CGA, he e a e 32 basis blades o g ades 0 o 5. The e m g ade e e s o he numbe o basis ec o s in a blade ha a e ac o izable unde he ou e p oduc . Vec o s, consequen ly, a e o g ade 1 and he ou e p oduc o wo independen ec o s, called bi ec o s, a e o g ade 2. A gene al elemen o geome ic algeb a is called a mul i ec o . In p ac ice, CGA ac ually applies a change o basis by in oducing he wo null ec o s e0and e∞, which can be hough o as a poin a he o igin and a in ini y, espec i ely. Since he Euclidean space is embedded in CGA, we can embed Euclidean poin s x o con o mal poin s P ia he con o mal embedding P=C(x) = e0+x+1 2x2e∞.(9) In gene al, geome ic p imi i es in geome ic algeb a a e de ined as nullspaces o ei he he inne o he ou e p oduc , which a e dual o each o he . The ou e p oduc nullspace (OPNS) is de ined as NOG(X) = x∈R3:C(x)∧X= 0.(10) A simila exp ession can be ound o he inne p oduc nullspace. The con o mal poin s a e he basic building blocks o cons uc o he geome ic p imi i es in hei OPNS ep e- sen a ion. The ele an p imi i es o his wo k a e lines L=P1∧P2∧e∞,(11) which can be cons uc ed om wo poin s and a poin a in ini y, planes E=P1∧P2∧P3∧e∞,(12) which can be cons uc ed om h ee poin s and a poin a in ini y and sphe es S=P1∧P2∧P3∧P4,(13) which can be cons uc ed om ou poin s. Rigid body ans o ma ions in CGA a e achie ed using mo o s M, which a e exponen ial mappings o dual lines, i.e. bi ec o s (essen ially, he sc ew axis o he mo ion). No e ha mo o s can be used o ans o m any objec in he algeb a, i.e. hey can di ec ly be used o ans o m he p e iously in oduced poin s, lines, planes and sphe es, by a sandwiching ope a ion X′=MX M, (14) whe e is Mis he e e se o a mo o . The o wa d kinema ics o se ial kinema ic chains can be ound as he p oduc o mo o s, i.e. M(q) = N Y i=1 Mi(qi) = N Y i=1 exp(qiBi),(15) whe e qis he cu en join con igu a ion and Bia e sc ew axes o he join s. The geome ic Jacobian JG(q)∈B1×N⊂ G1×N 4,1is a bi ec o alued mul i ec o ma ix and can be ound as JG=B′ 1. . . B′ N,(16) whe e he bi ec o elemen s can be ound as B′ i= i Y j=1 Mj(qj)Bi i Y j=1 Mj(qj).(17) Twis s Vand w enches Wa e also pa o he algeb a and hence bo h can be ans o med in he same manne as he geome ic p imi i es using (14). No e ha , con a y o classic ma ix Lie algeb a, no dual adjoin ope a ion is needed o ans o m w enches. The e is, howe e , s ill a duali y ela ionship be ween wis s and w enches, which can be ound ia mul iplica ion wi h he conjuga e pseudoscala Ic=Ie0[68]. Bo h wis s and w enches a e bi ec o s and he space o w enches can be ound as W ∈ span{e23,e13,e12,e01,e02,e03}.(18) The inne p oduc o wis s and w enches V · W =−pyields a scala , whe e pis he powe o he mo ion. Simila ly, he inne p oduc o a sc ew axis and a w ench B·W =−τyields a o que τ, which we will use o he ask-space impedance con ol in his a icle. IV. METHOD We p esen ou closed-loop ac ile e godic co e age me hod in h ee pa s: (i) su ace p ep ocessing; (ii) ac ile co e age; and (iii) obo con ol. The su ace p ep ocessing compu es he quan i ies ha need o be calcula ed only once when he su ace is cap u ed. Tac ile co e age gene a es he mo ion commands o he i ual co e age agen using he p ecom- pu ed quan i ies om he su ace p ep ocessing and he obo con olle acks he gene a ed mo ion commands wi h a manipula o using impedance con ol. 5 A. P oblem S a emen We o mula e a ac ile e godic con olle ha co e s a - ge spa ial dis ibu ions on a bi a y su aces. Simila ly o HEDAC, we p opaga e he in o ma ion encoding he co e age objec i e by di using he sou ce e m. Howe e , we u ilize he non-s a iona y ( ˙u= 0) di usion equa ion ˙u(x, τ)=∆Mu(x, τ),(19) as i allows con ol o e he desi ed smoo hness [69]. Since he di usion equa ion depends on ime, we in oduce an addi ional ime a iable τ. The di usion ime τis independen o he co e age ime used by he HEDAC algo i hm and unlike HEDAC, we equi e an ini ial condi ion u(x,0). We se he ini ial condi ion using he sou ce e m gi en in Equa ion (1) which encodes he co e age objec i e a he - h imes ep o he co e age, i.e., u(x,0) = s(x, τ). Addi ionally, he e we use ∆M, which gene alizes he Laplacian o Euclidean spaces ∆ o non-Euclidean mani olds M. This ope a o ∆Mis also known as Laplace-Bel ami ope a o bu o conciseness we will use he e m Laplacian. Ou co e age domains a e cu ed su aces (i.e. 2-mani olds) and we cap u e he unde lying mani old Mas a poin cloud Pcomposed o nPpoin s using an RGB-D came a P:= ((xi,ci) xi∈R3,ci∈ {0,...,255}3 o i= 1, . . . , nP),(20) whe e xiis he posi ion o he i- h su ace poin in Euclidean space and ciis he ec o o RGB colo in ensi ies. We assume he e is a p ocessing pipeline (i.e., such as [20], [62], [70]) which maps he poin posi ions and colo s o he p obabili y mass pio he spa ial dis ibu ion encoding he co e age objec i e. Acco dingly, ou co e age a ge becomes a disc e e spa ial dis ibu ion p(xi) = pion he poin cloud P. In o de o sol e (19) on i egula and disc e e domains, such as poin clouds, we disc e ize he p oblem in space and ime. Hence, we use ui,τ o deno e he alue o he po en ial ield a he i- h poin a he τ- h imes ep. We omi he subsc ip ii we e e o all poin s. B. Su ace P ep ocessing Fi s , we compu e he spa ial disc e iza ion o he Laplacian ∆M. No e ha he e a e a ious app oaches o disc e izing he Laplacian on poin clouds [63]–[66]. In his wo k, we ollow he app oach p esen ed in [66] and show a simpli ied e sion o i he e, bu e e he eade s o he o iginal wo k o mo e de ails. Using his me hod, he disc e e Laplacian is ep esen ed by he ma ix L∈RnP×nP L=M−1C,(21) whe e Mis he diagonal mass ma ix and Cis a spa se symme ic ma ix called he weak Laplacian. The en ies o Mco espond o he Vo onoi cell a eas in he local angen plane a ound each poin o P. Simila ly, he en ies o C a e de e mined by he connec i i y o he poin s on he local angen space and he dis ance be ween he connec ed poin s. No e ha he local angen space s uc u e also iden i ies he bounda y poin s. Fo a gi en poin , he lines be ween he o iginal poin and i s neighbo s a e cons uc ed. I he angle be ween wo consecu i e lines is g ea e han π/2, he poin is a bounda y and i s bounda y condi ion is se as ze o-Neumann. Nex , we disc e ize he di usion equa ion (19) in ime and inco po a e he disc e e Laplacian L. Using he backwa d Eule me hod, we de i e he implici ime-s epping equa ion, which emains s able o any imes ep τ 1 τ(uτ−u0) = Luτ,(22) whe e u0and uτa e column ec o s con aining he po en ial ield alues a he e ices o he poin cloud a he ini ial and inal imes, espec i ely. Then, combining (21) and (22) and sol ing o uτwe ob ain he linea sys em uτ= (M−τC)−1Mu0.(23) No e ha sol ing (23) equi es in e ing a la ge spa se ma ix, which migh be compu a ionally expensi e depending on he size o he poin cloud and equi es he imes ep o be se be o e he in e sion. Al e na i ely, we can sol e he p oblem in he spec al domain by p ojec ing he o iginal p oblem and ep ojec ing he solu ion back o he poin cloud. This p ocedu e gene alizes using he Fou ie ans o m o sol - ing he di usion equa ion on a ec angula domain in Rn o a bi a y mani olds. No e ha he Fou ie se ies a e he eigen unc ions o he Laplacian ∆in Rn. The e o e, we can use he eigen ec o s o he disc e e Laplacian L o sol ing he di usion equa ion on poin clouds. We can w i e he gene alized (i.e., M=I) eigen alue p oblem o he Laplacian as Cϕm=λmMϕm,(24) whe e {λm,ϕm}a e he eigen alue/eigen ec o pai s. Since Mis diagonal and Cis symme ic posi i e de ini e, by he spec al heo em, we know ha he eigen alues a e eal, non- nega i e, and in ascending o de analogous o he equency. The e o e, we can use he i s nMeigen alue/eigen ec o pai s as a low- equency app oxima ion o he whole spec um. Fu he mo e, he eigen ec o s a e o hono mal wi h espec o he inne p oduc de ined by he mass ma ix M. Acco dingly, we can s ack he i s nMeigen ec o s ϕmas column ec o s o cons uc he ma ix Φ∈RnP×nMencoding an o hono - mal ans o ma ion Φ⊤MΦ=I. Then, we can ans o m he coo dina es (shown wi h supe sc ip s) om he poin cloud o he spec al domain uϕ=Φ⊤Mux.(25) No e ha his s ep is equi alen o compu ing he Fou ie se ies coe icien s o a a ge dis ibu ion in SMC. Due o he o hono mal ans o ma ion, he PDE on he poin cloud becomes a sys em o decoupled ODEs in he spec al domain. I is well known ha he solu ion o a i s -o de linea ODE ˙x(τ) = −cx(τ)is gi en by x(τ) = e−cτ x(0), whe e cis a cons an and x(0) is he ini ial condi ion. The e o e, he solu ion o he sys em o ODEs in he spec al domain is gi en in ma ix o m as uϕ τ=e−λ1τ. . . e−λmτ⊤⊙uϕ 0,(26) 6 whe e ⊙deno es he Hadama d p oduc . We obse e om (26) ha he exponen ial e ms wi h la ge eigen alues (i.e., highe equencies) will decay as e . The e o e, app oxima ing he di usion using he i s nMcomponen s in oduces minimal e o . Secondly, simila o he mixed no m used in SMC, he low- equency spa ial ea u es a e p io i ized. Nex , we ans o m he solu ion back o he poin cloud o ge he di used po en ial ield ux=Φuϕ.(27) We can combine (25), (26) and (27) in o a uni ied spec al scheme uτ= Φ e−λ1τ. . . e−λmτ⊤⊙(Φ⊤Mu0).(28) We omi he supe sc ip s when wo king on he poin cloud o b e i y. No e ha τis he only ee pa ame e in he di usion compu a ion. Howe e , i s alue should be adap ed acco ding o he mean spacing be ween he adjacen poin s hon he poin cloud. Fo ha pu pose, we in oduce he hype pa ame e α > 0and embed i in o he imes ep calcula ion τ=αh2.(29) Acco dingly, we can con ol he di usion beha io indepen- den ly o he poin cloud size. Inc easing α esul s in longe di usion imes and a enua es he high- equency spa ial ea- u es (see (26) o de ails). This co esponds o a mo e global co e age [69]. Con e sely, dec easing α esul s in sho e di usion imes, which leads o p ese ing he high- equency spa ial ea u es, hence mo e local co e age beha io . No e ha he Laplacian is de e mined comple ely by he connec i i y on he local angen space and he dis ance be ween hese connec ed poin s. The e o e, i is in a ian o dis ance p ese ing (i.e., isome ic) ans o ma ions such as igid body mo ion o de o ma ion wi hou s e ching. Acco d- ingly, we compu e C,Mand de i ed quan i ies only once in he p ep ocessing s ep o a gi en su ace. Recompu a ion is no necessa y i he objec s ays s ill, mo es igidly, o he a ge dis ibu ion pichanges. C. Tac ile E godic Co e age We model he ac ual co e age ool/senso as a complian i ual co e age agen shaped as a disk wi h adius a. No ably, one can ep esen a bi a y ool/senso oo p in s as a combina ion o disks [69]. We posi ion ou agen a he end- e ec o o ou manipula o . Thus, o a gi en kinema ic chain and join con igu a ion q, we can use he o wa d kinema ics o compu e he posi ion o ou agen as a con o mal poin Pa Pa=M(q)e0 M(q).(30) Since he poin cloud is disc e e and he agen should mo e con inuously on he su ace, we p ojec ou agen Paand i s oo p in o he closes local angen space on he poin cloud. 1) Local Tangen Space and Co e age Compu a ion: Gi en he agen ’s posi ion Pa, we i s compu e he closes angen space on he poin cloud. Fo ha , we que y a K-D ee T(P) o he poin s xi∈ P ha a e wi hin he adius ao he agen . Then, we compu e he con o mal embeddings Pio he neighbo ing Euclidean poin s xiusing (9). We e e o he se composed o poin s Pias he local neighbo hood. Then, we i a angen space o he local neighbo hood by minimizing he classical leas squa es objec i e min nN X i=1 (Pi·X∗)2,(31) whe e X∗is he dual ep esen a ion o ei he a plane o a sphe e and he inne p oduc ·is a dis ance measu e. In CGA, planes can be seen as limi cases o sphe es, i.e. planes a e sphe es wi h in ini e adius. This is also easy o obse e by looking a Equa ions (12) and (13) which cons uc hese geome ic p imi i es. No e ha i ing a local angen sphe e wi h he adius de e mined by he local cu a u e would always esul in smalle o equal esiduals han i ing a plane. I has been shown in [71] ha he solu ion o he leas squa es p oblem gi en in (31) is he eigen ec o co esponding o he smalles eigen alue o he 5×5ma ix bj,k = nN X i=1 wi,jwi,k,(32) whe e wi,k =     pi,k i k∈ {1,2,3} −1i k= 4 −1 2p2 ii k= 5. (33) Using he i e componen s io his eigen ec o we can ind he geome ic p imi i e as X= ( 0e0+ 1e1+ 2e2+ 3e3+ 4e∞)∗.(34) No e ha i Xis a plane hen 0= 0, o he wise Xis a sphe e. Nex , we wan o p ojec Pa o Xby using he gene al subspace p ojec ion o mula o CGA Ppai =(Pa∧e∞)·XX−1.(35) He e we i s cons uc he poin pai Pa∧e∞, whe e e∞ co esponds o he poin a in ini y. Pa∧e∞is also called a la poin . No e ha he p ojec ion essen ially amoun s o i s cons uc ing he dual line (Pa∧e∞)·X ha passes h ough he poin Paand is o hogonal o X, hen in e sec ing his line wi h he p imi i e X. I Xis a sphe e, hen he in e sec ion o he line and he sphe e will esul in wo poin s on he sphe e. I Xis a plane, i will esul in ano he la poin , i.e. one poin on he plane and one a in ini y. In any case, we can e ie e he close one o he agen posi ion Pausing he spli ope a ion P′ a=spli [Pp].(36) He e, P′ ais he p ojec ed agen posi ion on he angen space X. Nex , we compu e ou agen ’s oo p in (i.e., ins an aneous co e age) by p ojec ing i s su ace o he poin cloud. I he a ge su ace was la , all he poin s wi hin he adius ao 7 ou agen P′ awould be co e ed by he oo p in . Howe e , in he gene al case, bo h he ool and he su ace can be cu ed and de o mable. Fo simplici y, we assume ha he su ace is igid, and i de o ms he ool wi h a cons an bending adius. We use he adius o he local angen sphe e ha we compu ed using CGA as an app oxima ion o he bending adius. Acco dingly, we can quan i y he e o o he local angen space app oxima ion o he i- h neighbo Piby he no malized esiduals eio he leas squa es compu a ion (31). We encode his app oxima ion e o in o he oo p in by weigh ing he i- h neighbo by he Gaussian ke nel φ( )using he no malized esiduals i=ei/max(e) φ( i) = exp −ε2 2 i,(37) whe e he hype pa ame e ε > 0con ols he co e age allo . Nex , we subs i u e he Gaussian ke nel weigh ed oo p in in o (3) o compu e he co e age c , which is hen used o calcula e he i ual sou ce e m s ia (1). As men ioned ea lie , his i ual sou ce e m se es as he ini ial condi ion o he di usion equa ion (19) a each i e a ion o he ac ile co e age loop, i.e., u0=s . 2) G adien o he Di used Po en ial Field: We guide he co e age agen using he g adien o he di used po en ial ield as he accele a ion command ¨ P′ a=∇uP′ a,τ ,(38) whe e ∇uP′ a,τ deno es he g adien o he di used po en ial ield a he p ojec ed agen posi ion P′ a. Howe e , compu ing he g adien on he poin cloud is mo e in ol ed han a egula g id o a mesh. Recall ha in Sec ion IV-C1, we al eady compu ed he p ojec ed agen posi ion P′ a, he local neighbo hood and he angen space X∗. As he i s s ep, we compu e he angen plane Ea,τ a P′ a, namely Ea,τ =L∗ a,⊥∧P′ a∧e∞,(39) using he line La,⊥, which is o hogonal o he su ace and passes h ough P′ a. I is ound by wedging he dual p imi i e Xwi h P′ a o in ini y wi h La,⊥=X∗∧P′ a∧e∞.(40) Then, we p ojec he poin s Piin he local neighbo hood o he angen plane Ea,τ using (35) and (36), by se ing Ea,τ as he p imi i e X. Nex , we use he alues o he po en ial ield a he neighbo loca ions as he heigh hi=ui,τ o a second su ace om he angen plane. Then, we i a 3- d deg ee polynomial o his su ace as shown by using he weigh ed leas squa es objec i e ˆ A= a g min A (Y−XA)⊤W(Y−XA),(41) wi h he diagonal weigh ma ix W W=diagφ( 1), φ( 1),...φ( m),(42) whose en ies a e gi en by he Gaussian ke nel (37). One can e e o [72] o he de ails. Las ly, we calcula e he g adien a he p ojec ed agen ’s posi ion using he analy ical g adien s o he polynomial. We depic he app oach isually in Figu e 2. Fig. 2: Blue- ed poin s show he alue o he po en ial ield uτ on he poin cloud Pand he yellow poin is he p ojec ed agen posi ion P′ a. We also p ojec he agen ’s neighbo s Pi o he angen plane Ea,τ , shown in g een. Nex , we use he heigh unc ion hi=ui,τ which uses he alues o he po en ial ield o li he p ojec ed poin s in he no mal di ec ion o he angen plane. We show he li ed poin s wi h la ge blue- ed poin s. We i a polynomial o his li ed su ace and compu e i s analy ical g adien s a he neighbo loca ions ∇ui,τ , as shown wi h a ows in he de ail iew. D. Robo Con ol The e a e se e al aspec s ha he con ol o he physical obo needs o achie e. The i s is o ack he i ual co e age agen on he a ge su ace, while keeping he end-e ec o no - mal o he su ace. The second is o exe a desi ed o ce on he su ace. To do so, we design a ask-space impedance con olle while u he exploi ing geome ic algeb a o e iciency and compac ness. The con ol law is o he ollowing o m τ=−J⊤· W,(43) whe e J∈B1×N⊂G1×N 4,1is he Jacobian mul i ec o ma ix wi h elemen s co esponding o bi ec o s, Wis he desi ed ask-space w ench and τa e he esul ing join o ques. Be o e composing he inal con ol law, we will explain i s componen s indi idually. 1) Su ace O ien a ion: F om Equa ion (40), we ob ained a line La,⊥ ha is o hogonal o he su ace ha we wish o ack. In [73], i was shown how he mo o be ween con o mal objec s can be ob ained. We use his o mula ion o ind he mo o be ween he a ge o hogonal line and he line ha co esponds o he z-axis o he end-e ec o o he obo in i s cu en con igu a ion, which is ound as Lee =M(q)(e0∧e3∧e∞) M(q).(44) Then, he mo o MLeeLa,⊥, which ans o ms Lee in o La,⊥ can be ound as MLeeLa,⊥=1 C(1 + La,⊥Lee),(45) whe e Cis a no maliza ion cons an . No e ha Cdoes no simply co espond o he no m o 1 + La,⊥Lee, bu equi es a mo e in ol ed compu a ion. We he e o e omi i s exac compu a ion he e o b e i y and e e eade s o [73]. We can now use he mo o MLeeLa,⊥in o de o ind a con ol command o he obo ia he loga i hmic map o mo o s, i.e. VLa,⊥= log MLeeLa,⊥.(46) 8 O cou se, i he lines a e equal, MLeeLa,⊥= 1 and conse- quen ly VLa,⊥= 0. No e ha VLa,⊥is s ill a command in ask space (we will explain how o ans o m i o a join o que command once we ha e de i ed all he necessa y componen s). Ano he issue is ha algeb aically, VLa,⊥co esponds o a wis , no a w ench. Hence, we need o ans o m i acco dingly. F om physics, we know ha wis s ans o m o w enches ia an ine ial map, which we could use he e as well. In he con ex o con ol, his ine ia enso is, howe e , a uning pa ame e and does no ac ually co espond o a physical quan i y. Thus, in o de o simpli y he inal exp ession, we will use a scala ma ix alued ine ia, ins ead o a geome ic algeb a ine ia enso and choose o ans o m he wis command o w ench command pu ely algeb aically. As i has been shown be o e, his can be achie ed by he conjuga e pseudoscala Ic=Ie0[68]. I ollows ha WLa,⊥=VLa,⊥Ic,(47) and WLa,⊥now algeb aically co esponds o a w ench. 2) Ta ge Su ace Fo ce: Since his a icle desc ibes a me hod o ac ile su ace co e age, he goal o he obo con ol is o no simply s ay in con ac wi h he su ace, bu o ac i ely exe a desi ed o ce on he su ace. Fi s o all, we deno e he cu en measu ed w ench as Wm( )and he desi ed w ench as Wd. Bo h a e bi ec o s as de ined by Equa ion (18). We use Wdw. . . end-e ec o in o de o make i mo e in ui i e o de ine. Hence, we need o ans o m Wm( ) o he same coo dina e ame, i.e. W′ m( ) = M(q)Wm( )M(q).(48) In o de o achie e he desi ed in e ac ion o ce, we simply apply a s anda d PID con olle in w ench space, i.e. WC=Kp,WWe+Ki,WZ⊤ 0 We(τ)dτ+Kd,W d d We( ), (49) whe e he w ench e o is We( ) = Wd− W′ m( ),(50) whe e Kp,W,Ki,Wand Kd,Wa e he co esponding gain ma ices, and WCis he esul ing con ol w ench. Since he desi ed w ench is de ined in end-e ec o coo di- na es, i usually amoun s o a linea o ce in he z-di ec ion o he end-e ec o ame, i.e. Wd= de03. Addi ionally, o an imp o ed cleaning beha io one could also se a desi ed o que a ound ha axis by adding τde12. The pa e n o how o se his o que, howe e , would be subjec o u he in es iga ion. 3) Task-Space Impedance Con ol: Recalling he con ol law om Equa ion (43), we now collec he e ms om he p e ious subsec ions in o a uni ied ask-space impedance con- ol law. We s a by looking in mo e de ail a he Jacobian J. P e iously, we men ioned ha we a e using he cu en end- e ec o mo o as he e e ence, hence, we equi e he Jacobian o be compu ed w. . . ha e e ence. This is he e o e no he geome ic Jacobian ha was p esen ed in Equa ion (16), bu a a ia ion o i . The end-e ec o ame geome ic Jacobian Jee Gcan be ound as Jee G=Bee 1. . . Bee N,(51) whe e he bi ec o elemen s can be ound as Bee i= Mee i(q)BiMee i,(52) wi h Mee i= i Y j=N Mi(qi).(53) Hence, he ela ionship be ween JGand Jee Gcan be ound as Jee G= M(q)JGM(q).(54) The w ench in he con ol law is composed o he h ee w enches ha we de ined in he p e ious subsec ions. As commonly done, we add a damping e m ha co esponds o he cu en end-e ec o wis and as be o e, we ans o m i o an algeb aic w ench, i.e. WV=Jee G˙qe0∞.(55) Wi h his, we now ha e e e y hing in place o compose ou inal con ol law as τ=−Jee,⊤ G·KLa,⊥WLa,⊥−DVWV+WC,(56) whe e KLa,⊥is a s i ness and DVa damping gain. V. EXPERIMENTS Ou expe imen al se up comp ises a Bo aSys SensOne 6- axis o ce o que (F/T) senso a ached o he w is o a 7- axis F anka obo manipula o and a cus om 3-D p in ed pa a ached o he F/T senso . The cus om pa in e aces an In el Realsense D415 dep h came a and a sponge a i s ip. We conside he sponge’s cen e poin o be he co e age agen ’s posi ion Pa. Be o e he ope a ion, we pe o m ex insic calib a ion o he came a o combine he dep h and RGB eeds om he came a and o ob ain i s ans o ma ion wi h espec o he obo join s. Addi ionally, we calib a e he F/T senso o compensa e o he weigh o he 3-D p in ed pa and he came a. We show he expe imen al se up on he le o Figu e 1. A. Implemen a ion De ails The pipeline o ou ac ile e godic co e age me hod consis s o h ee modules: (i) su ace acquisi ion, (ii) su ace co e age and (iii) obo con ol. Figu e 3 summa izes he in o ma ion low be ween he componen s. 1) Su ace Acquisi ion: The su ace acquisi ion node is esponsible o collec ing he poin cloud and pe o ming p ep ocessing ope a ions desc ibed in Sec ion IV-B. We use scipy1 o he nea es neighbo que ies and o sol ing he eigenp oblem in (24). The ma ices Cand Mcomposing he disc e e Laplacian in (21) a e compu ed wi h he o- bus _laplacian package2[66]. 2) Su ace Co e age: The su ace co e age node pe - o ms he compu a ions based on he p ocedu e gi en in Sec ion IV-C. I uses he in o ma ion p o ided by he su ace acquisi ion node and p oduces he a ge line o he obo con ol node. 1h ps://scipy.o g 2h ps://gi hub.com/nmwsha p/ obus -laplacians-py 9 Su ace Acquisi ion Su ace Co e age Robo Con ol Inne loop Ou e loop Ta ge , K-D T ee, Laplacian eigenbasis Ac ual co e age Line objec Ac ual agen posi ion Fo ce a ge  Fig. 3: In o ma ion low be ween he h ee componen s. The pipeline is composed o an ou e loop esponsible o con olling he co e age p og ess wi h he eedback om he came a, whe eas he inne loop compensa es o he misma ch due o he obo dynamics. 3) Robo con ol: On a high le el, he obo con ol can be seen as a s a e machine wi h h ee disc e e s a es. The i s wo s a es a e essen ially wo p e- eco ded join posi ions in which he obo is wai ing o o he pa s o he pipeline o be comple ed. One o hese posi ions co esponds o he pic u e- aking posi ion, i.e., a join posi ion whe e he came a has he ull objec in i s ame and he poin cloud can be ob ained. The obo is wai ing in his posi ion un il he poin cloud has been ob ained, a e wa ds i changes i s posi ion o ho e sho ly o e he objec . In his second posi ion, i is wai ing o he compu a ion o he Laplacian eigen unc ions o be comple ed, such ha he co e age can s a . The swi ching be ween hose wo posi ions is achie ed using a simple join impedance con olle . The hi d and mos impo an s a e is when obo is ac ually con olled o be in con ac wi h he su ace and o ollow he a ge co esponding o he co e age agen . This beha iou is achie ed using he con olle ha we desc ibed in Sec ion IV-D. The ele an pa ame e s, ha we e chosen empi ically o he eal-wo ld expe imen s, a e he s i ness and damping o he line acking con olle , i.e. KLa,⊥=diag(30,30,30,750,750,300) and DV= diag(10,10,10,150,150,50), as well as he gains o he w ench PID con olle , i.e. Kp,W= 0.5,Ki,W= 5 and Kd,W= 0.5. The con olle has been implemen ed using ou open-sou ce geome ic algeb a o obo ics lib a y ga o3 ha we i s p esen ed in [74]. No e ha in some cases, ma ix- ec o p oduc s o geome ic algeb a quan i ies ha e been used o he implemen a ion, whe e he ma hema ical s uc u e o he geome ic p oduc ac ually simpli ies o his, which can be exploi ed o mo e e icien compu a ion. B. Simula ed Expe imen s 1) Compu a ion Pe o mance: In o de o assess he com- pu a ional pe o mance, we in es iga ed he wo main ope a- ions o ou me hod: (i) p ep ocessing by sol ing ei he he eigenp oblem (24) o ma ix in e sion in (23) (ii) in eg a ing he di usion a un ime using ei he he spec al (28) o implici (23) o mula ions. In his expe imen , we used he S an o d Bunny as he e e ence poin cloud and pe o med oxel il e ing o se he poin cloud esolu ion. We p esen he 3h ps://gi lab.com/ga o esul s o he p ep ocessing in Figu e 4 and o he un ime in Figu e 5. 3k 6k 9k 12k 15k 18k 5 0.1 2 5 1 2 5 10 2 5 25 50 100 200 implici Fig. 4: Compu a ional complexi y o he p ep ocessing s ep o di e en nPand nM. Legend shows nM alues. The ime axis is loga i hmic and he legend shows nM alues. 3k 6k 9k 12k 15k 18k 0.1 2 5 1 2 5 10 2 5 100 2 25 50 100 200 implici Fig. 5: Compu a ional complexi y o in eg a ing he di usion equa ion a un ime o di e en nPand nM. The ime axis is loga i hmic and he legend shows nM alues. 2) Co e age Pe o mance: We es ed he co e age pe o - mance in a se ies o kinema ic simula ions. As he co e age me ic, we used he no malized e godici y o e he a ge dis ibu ion, which compa es he ime-a e aged s a is ics o agen ajec o ies o he a ge dis ibu ion ε =∥max (p−c ,0) ∥2 PnP i=1 pi .(57)