gafro: Geometric Algebra for Robotics

ga o: Geome ic Algeb a o Robo ics Tobias Löw∗†, Philip Abbe ∗and Syl ain Calinon∗† ∗Idiap Resea ch Ins i u e, Ma igny, Swi ze land †EPFL, Lausanne, Swi ze land Abs ac —Geome y is a undamen al pa o obo ics and he e ha e been a ious amewo ks o ep esen a ion o e he yea s. Recen ly, geome ic algeb a has gained a en ion o i s p ope y o uni ying many o hose p e ious ideas in o one algeb a. While he e a e al eady e icien open-sou ce implemen- a ions o geome ic algeb a a ailable, none o hem is a ge ed a obo ics applica ions. We wan o add ess his sho coming wi h ou lib a y ga o. This a icle p esen s an o e iew o he imple- men a ion de ails as well as a u o ial o ga o, an e icien c++ lib a y a ge ing obo ics applica ions using geome ic algeb a. The lib a y ocuses on using con o mal geome ic algeb a. Hence, a ious geome ic p imi i es a e a ailable o compu a ion as well as igid body ans o ma ions. The modeling o obo ic sys ems is also an impo an aspec o he lib a y. I implemen s a ious algo i hms o calcula ing he kinema ics and dynamics o such sys ems as well as objec i es o op imisa ion p oblems. The so wa e s ack is comple ed by py hon bindings in pyga o and a ROS in e ace in ga o_ os. I. INTRODUCTION Geome ic algeb a can be conside ed a high-le el ma he- ma ical language o geome ic easoning. As such i is e y well sui ed o gene al p oblems in obo ics. Al hough geome ic algeb a has g ea po en ial o model- ing, lea ning and con ol in obo ics, i has no been widely adop ed in obo ics esea ch. One eason o his is he lack o easy- o-use lib a ies o obo ics applica ions, while a he same ime ools based on ma ix algeb a a e e y ma u e and eadily a ailable. We aim o change ha by p o iding a eady- o-use geome ic algeb a lib a y o obo ics ha can be used wi h he mos popula p og amming amewo ks, namely c++, py hon and ROS. Fo obo modeling and con ol i is necessa y o compu e he kinema ics and dynamics o obo s. These algo i hms a e well s udied and ha e been implemen ed in a ious so wa e amewo ks. In ou ga o lib a y we p o ide an implemen- a ion o he geome ic algeb a a ian o hese algo i hms. I is impo an o poin ou ha geome ic algeb a can be used o compu e hese impo an quan i ies ha a e classically compu ed using ma ix algeb a, while also o e ing a iche oolse , i.e. i also includes ools o geome ic easoning ha ma ix algeb a does no ha e. In his a icle we wan o explain he implemen a ion de ails o ou geome ic algeb a lib a y ga o and gi e a u o ial on 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 Commission’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). how o use i o common obo ics p oblems such as in e se kinema ics and op imal con ol. Ou aim is o make geome ic algeb a mo e accessible o obo ics esea ch by p o iding his eady- o-use lib a y. This should esul in a wide adop ion and acili a e he esea ch on using his powe ul amewo k o obo ics. This a icle is o ganized as ollows: in Sec ion II we gi e an o e iew o he a ailable p og amming in e aces, in Sec ion III we explain he implemen a ion de ails o he algeb a, in Sec ion IV we show how o model obo s using he lib a y, in Sec ion V we compa e ga o o o he GA and obo modeling lib a ies and inally in Sec ion VI we demons a e a ious applica ions and gi e a u o ial on how o use he lib a y. The documen a ion and he links o all eposi o ies can be ound on ou websi e h ps://geome ic-algeb a. obiloew.ch/ga o. II. PROGRAMMING INTERFACES In his sec ion we explain he p og amming in e aces ha ga o o e s. The main lib a y is w i en in c++ o which we p o ide py hon bindings called pyga o as well as he ROS package ga o_ os. All men ioned eposi o ies can be ound a h ps://gi lab.com/ga o. A. c++ Lib a y The co e implemen a ion o ga o is done in c++20 and elies hea ily on empla es. Apa om he s anda d lib a y he only manda o y dependency is he Eigen1lib a y. We use he he yaml-cpp2lib a y as an op ional dependency o loading o obo desc ip ions. Se e al obo desc ip ions a e al eady a ailable and can be used ia he espec i e classes. B. ROS Package The Robo Ope a ing Sys em (ROS) is he de ac o s anda d o building obo applica ions. Hence, we p o ide a ROS package called ga o_ os o use ou lib a y wi hin he ROS amewo k. I allows passing geome ic p imi i es be ween nodes ia messages. I also enables he isualiza ion o hese geome ic p imi i es in R iz ia cus om plugins. Fu he mo e, since mos obo desc ip ions a e a ailable in he Uni e sal Robo Desc ip ion Fo ma (URDF), his package suppo s loading his ile o ma . 1h ps://eigen. ux amily.o g 2h ps://gi hub.com/jbede /yaml-cpp a Xi :2310.19090 1 [cs.RO] 29 Oc 2023 C. Py hon Bindings Since he py hon p og amming language is a popula ool o apid p o o yping and is gene ally mo e accessible han he c++ language, we a e p o iding language bindings in py hon o ga o using he pybind113lib a y. III. IMPLEMENTATION OF CONFORMAL GEOMETRIC ALGEBRA In his sec ion we will explain in de ail ou implemen a ion o con o mal geome ic algeb a (CGA). The aspec s ha a e highligh ed a e he implemen a ion o a gene al mul i ec o and he exp essions ha a e ac ing on i . The lib a y co e s se e al poin s ha we e p oposed in [1] as a wishlis o geome ic algeb a implemen a ions. We ha e designed he lib a y in an objec -o ien ed way, so he classes also e- lec he ma hema ical inhe i ance ela ionships. Fu he mo e all classes, i.e. all specialized mul i ec o s a e ins an ia ed as di e en ypes, which allows hem o be dis inguished a compile- ime o ype-sa e y and ha e pe sis en s o age. These specialized classes, also enable he compu a ion wi h pa ial mul i ec o s, i.e. he lib a y exploi s he ac ha he mos commonly muli ec o s a e spa se and only use ce ain subspaces o he algeb a. We add ess nume ical imp ecisions by ensu ing ha only elemen s o he esul ing mul i ec o s o exp essions a e e alua ed ha a e known o be non-ze o. We handle he ype explosion o bina y ope a o s by au oma ically e alua ion pa ial exp essions when he ull exp essions ge oo complex. A. Gene al Mul i ec o The co e elemen o compu a ion in geome ic algeb a is he mul i ec o . Hence, i is e y impo an o hink abou he design choices when implemen ing i s s uc u e, as his will de e mine he memo y usage and compu a ional pe o mance. The gene al s uc u e o a mul i ec o in CGA can be seen in Figu e 1. I is composed o 32 basis blades, di ided in o g ades ze o o i e. A gene al mul i ec o would he e o e be qui e hea y in e ms o memo y and compu a ion. Howe e , g ade 0 1 g ade 1 e1e2e3e∞e0 g ade 2 e23 e13 e12 e1∞e2∞e3∞e01 e02 e03 e0∞ g ade 3 e123 e12∞e13∞e23∞e012 e013 e023 e01∞e02∞e03∞ g ade 4 e123∞e0123 e012∞e023∞e013∞ g ade 5 e0123∞ Fig. 1: S uc u e o con o mal geome ic algeb a wi h he 32 basis blades, di ided in o he di e en g ades. G ade 0 and 5 a e he scala and pseudo-scala , espec i ely. G ades 1 o 4 a e called bi-, i- and quad ec o s. 3h ps://gi hub.com/pybind/pybind11 Mo o Twis Poin Line Ci cle Plane Fig. 2: Non-ze o elemen s o a ious geome ic p imi i es in hei p imal ep esen a ions in con o mal geome ic algeb a. Boxes ep esen basis blades and colo ed boxes ep esen he non-ze o blades o he geome ic p imi i e wi h he ma ching colo . I can be seen ha o he 32 basis blades composing mul i ec o s only a spa se numbe is used o he ep esen a- ions. No e ha geome ic p imi i es a e single-g ade objec s, while ans o ma ions a e mixed-g ade. An impo an s uc u al aspec s o CGA ha acili a e he design p ocess he e a e he spa si y o i s ep esen a ions and he ac ha he s uc u e o mul i ec o exp essions is known a compile- ime. Bo h o hese p ope ies mean ha we can implemen he da a ec o o a mul i ec o by only s o ing i s known non-ze o elemen s. This is achie ed by using a empla e ha akes lis o blade indices as inpu : empla e <class T, in ... blades> class Mul i ec o { public: cons exp s a ic in size = sizeo ...(blades); {...} p i a e: Eigen::Ma ix<T, size, 1> da a_; }; The lis o indices is hen s o ed in e nally as a bi se ha acili a es he compa ison o he subspaces o wo muli ec o s. A bi se is simply a lis o 32 bi s, ha a e ei he 0 o 1, depending on whe he he co esponding blade is p esen in he mul i ec o o no . The memo y ha is alloca ed co esponds o he numbe o blade indices ha is gi en o he empla e. I uses an Eigen::Ma ix o s o e he da a, ha is exposed ia an accesso unc ion called ec o (). This makes i possible o di ec ly use he pa ame e ec o o any mul i ec o , which is use ul o e.g. op imisa ion sol e s. The unde lying da a ype Tis a empla e a gumen , which makes i possible o ei he use e.g. loa o double, depending on he sys em a chi ec u e. Fu he mo e, i allows he usage o gene al pu pose au oma ic di e en ia ion lib a ies such as au odi 4. This helps when o mulizing op imisa ion p oblems in geome ic algeb a using ga o since i acili a es he coding o complex objec i e unc ions and hus accele a es p o o yping. The Mul i ec o class and all i s de i a es (including he exp essions) ha e a me hod called ge ha is empla ed on he blade index and p o ec ed by he concep s lib a y o c++20. 4h ps://au odi .gi hub.io/ TABLE I: Una y exp essions ha a e implemen ed as membe unc ions o he Mul i ec o class. membe unc ion una y exp ession ma hema ical symbol e e se Re e se e X in e se In e se X−1 dual Dual X∗ B. Algeb aic Compu a ions using Exp ession Templa es When implemen ing geome ic algeb a, he e a e some co e bina y exp essions ha need o be a ailable o gene al algeb aic compu a ions. These ope a o s a e lis ed in Table II. TABLE II: Bina y ope a o s. ope a o symbol addi ion + + subs ac ion - − ou e p oduc ˆ∧ inne p oduc | · geome ic p oduc ∗ All exp essions a e implemen ed as exp ession empla es. The exp ession empla es de e mine he esul ype o he exp ession a compile ime. This is achie ed ia accompanying e alua ion classes ha a e hidden in he de ail namespace. The challenge in he implemen a ion he e is he ac ha he esul ing mul i ec o s only a ely ha e he same blades as he inpu ope ands. No e ha he exp essions a e e alua ed in a lazy ashion, which means ha he blades a e e alua ed on demand. This makes i possible o e.g. only e alua e a single blade o he esul ing mul i ec o , depending on he equi emen s. empla e <class De i ed, class Resul > class Exp ession { public: empla e <in blade> equi es(Resul ::has(blade)) ypename Resul ::V ype ge () cons { e u n s a ic_cas <cons De i ed &>(* his). empla e ge <blade>(); } }; A i s example o his can be seen in he Sum exp ession in Figu e 3. The co esponding ype e alua ion class cons uc s he ype o he esul ing mul i ec o a compile- ime. In he case o a summa ion his amoun s o a simple bi wise OR ope a ion compa ing he bi se s o he inpu mul i ec o s. The inne and ou e p oduc s wo k essen ially in he same way and hus he co esponding exp ession bo h in- he i om a base P oduc class, i.e. Inne P oduc and Ou e P oduc . The P oduc class akes a class s uc u e implemen ing he co esponding Cayley able as empla e a gumen . This Cayley able de ines he esul ing blades o a blade by blade mul iplica ion unde he inne and ou e p oduc , espec i ely. Thus, in he case o CGA, i de ines 1024 ope a ions. In o de o de e mine he ype o he esul ing mul i ec o , we employ old exp essions ha allow us o i e a e o e he blades o bo h inpu muli ec o s a compile- ime. In his loop, we ob ain he esul ing blade pe pai o blades P1+P2=P3 += (a) The summa ion o wo muli ec o s wi h he same blades esul s in ano he mul i ec o wi h he same blades. P1+P2=P3 += (b) The summa ion o wo muli ec o s wi h he di e en blades esul s in a mul i ec o wi h he blades o bo h inpu muli ec o s. Fig. 3: Summa ion using he espec i e Cayley able and hen assemble hem in o he esul ing mul i ec o again using OR ope a ions. Figu e 4 shows an example o each he inne and he ou e p oduc . C·S=X ·= (a) The inne p oduc is a g ade lowe ing ope a ion, i.e. he esul ing mul i ec o will be o lowe g ade han he inpu s. The example shows ha he inne p oduc o a ci cle Cwi h a sphe e S esul s in a poin P. P P ∧e∞=L ∧= (b) The ou e p oduc is a g ade aising ope a ion, i.e. he esul ing mul i ec o will be o highe g ade han he inpu s. The example shows ha he ou e p oduc o a poin pai P P and e∞ esul s in a line L Fig. 4: The esul ing mul i ec o o he inne and ou e p oduc ope a ions has a di e en g ade han he inpu s. The geome ic p oduc class Geome icP oduc also inhe i s om he base P oduc class and comes wi h i s own Cayley able. So implemen a ion wise i is he same as he inne and ou e p oduc s. The main di e ence is ha wo blades can esul om a blade mul iplica ion, which causes he esul ing mul i ec o o po en ially ha e bo h a lowe and a highe g ade han he inpu s, as can be seen in Figu e 5a. Fu he mo e, we ha e p oduc s ha a e based on he geome ic p oduc such as he sandwich p oduc , which is also ea ed as a bina y exp ession and shown in Figu e 5b. M P =X = (a) Using he geome ic p oduc esul s in bo h blades o lowe and highe g ade. M C M=C′ = (b) The sandwich p oduc is a g ade-p ese ing ope a ion. Nume ical issues migh lead o esiduals in o he blades, which we a oid by simply no e alua ing hem in he exp essions. Fig. 5: The geome ic p oduc is a combina ion o he inne and ou e p oduc . C. Geome ic P imi i es Since we know he subspaces o all he geome ic p imi- i es, we chose o implemen hem in an objec -o ien ed way by inhe i ing om he base Mul i ec o class. Hence, he a ailable classes a e Vec o ,Di ec ionVec o , Tangen Vec o ,Poin ,Poin Pai ,Line,Ci cle, Plane and Sphe e. Thei co esponding subspaces wi hin he geome ic algeb a can be seen in Figu e 2. Ha ing he geome ic p imi i es as explici classes allows he implemen- a ion o commonly used equa ions as membe s unc ions, which acili a es he usage. Fo example he cons uc o s o he geome ic p imi i e classes implemen he a ious ways hey can be de ined. The explici classes a e mean o acili a e he use and cons uc ion, bu o cou se, using compu a ion wi h base mul i ec o s is also possible. This p ese es he p ope y o co a ian compu a ion wi hin he algeb a. D. Rigid Body T ans o ma ions The igid body ans o ma ions ha a e cu en ly a ail- able a e implemen ed in he classes Ro o ,T ansla o , Mo o and Dila o . They all inhe i om he base class Ve so . Since all h ee classes a e exponen ial map- pings o bi ec o s hey a e accompanied by he exp essions *Loga i hm and *Exponen ial, espec i ely. The main me hod o he igid body ans o ma ions is apply, which implemen s he sandwich p oduc X′=M X Mand ensu es ype sa e y. While Xcan echnically be any mul i ec o , he in ended usage is wi h he geome ic p imi i es ha we e p esen ed in Sec ion III-C. Hence, in his con ex , ype sa e y means ha X′s ays he same geome ic p imi i e as X, e.g. a Poin s ays a Poin . This ensu es ha he exp ession only e alua es blades ha a e pa o he geome ic p imi i e, which no only educes he numbe o loa ing poin ope a ions, i also deals wi h nume ical imp ecisions in he compu a ion ha a e known o occu in geome ic algeb a implemen a ions. IV. ROBOT MODELING The p e ious sec ions in oduced he ea u es ela ed o he unde lying geome ic algeb a implemen a ion o ga o. This sec ion will now in oduce he highe le el ea u es o he lib a y ela ed o obo modeling, ha dis inguish i om o he geome ic algeb a lib a ies. The main aspec s o obo modeling a e he compu a ion o he kinema ics and dynamics o obo ic sys ems. While he o wa d kinema ics and o wa d/in e se dynamics can be compu ed using e icien ecu si e algo i hms, he in e se kinema ics p oblem o edundan manipula o s is an op imi- sa ion p oblem. Hence, he obo modeling sec ion also co e s he necessa y ools o sol ing such op imisa ion p oblems. The base class o he modeling o obo ic sys ems is called Sys em. I implemen s he main unc ionali y o he compu a ion o he kinema ics and dynamics o obo ic sys ems and s o es he join s and links. Those a e o ganized in he classes Join ,FixedJoin ,Re olu eJoin , P isma icJoin and Link. A cus om obo sys em can hen be c ea ed using he unc ions addJoin and addLink. No e ha hose unc ions only add he links and join s o he sys em, he p ope ela ionships be ween hem need o be speci ied indi idually by se ing he espec i e child/pa en links/join s. Usually, obo ic sys ems consis o one o mo e kinema ic chains o in e es s, e.g. he chain om he base link o he end-e ec o ame o se ial manipula o s. This concep is implemen ed in he Kinema icChain helpe class and can be added o a Sys em ia he addKinema icChain unc ion. The o wa d and in e se dynamics a e implemen ed in he membe unc ions compu eJoin Accele a ions and compu eJoin To ques, espec i ely. These unc ions a e using he con o mal geome ic algeb a e sions o he Recu - si e New on-Eule Algo i hm (RNEA) and A icula ed Body Algo i hm (ABA). The Manipula o class ha implemen s a se ial ma- nipula o wi h Ndeg ees o eedom is a specializa ion o he base Sys em class. The pa ame e Nis gi en as a empla e a gumen , which makes i possible o use ixed- size Eigen::Ma ix ypes o he implemen a ion o e.g. he join con igu a ion ec o s and Jacobian ma ices. The Manipula o class implemen s unc ions ha a e mos commonly o in e es when dealing wi h such ypes o sys- ems. Mo e speci ically, hese a e unc ions dealing wi h he end-e ec o o wa d kinema ics and Jacobian ma ices. Hence, hese unc ions a e •ge EEMo o •ge EEAnaly icJacobian •ge EEGeome icJacobian •ge EEF ameJacobian These unc ions use he a o emen ioned Kinema icChain class in o de o de ine he end-e ec o kinema ic chain and p ecompu e ce ain ela ed quan i ies. Hence, loading a Manipula o om a ile addionally equi es he name o he end-e ec o join . The ga o lib a y includes se e al p ede ined se ial manip- ula o s ha inhe i om he Manipula o class, hose a e implemen ed in he ollowing classes: •F ankaEmikaRobo •UR5 These classes a e loading he de ini ion om yaml iles and a e only a ailable i yaml-cpp is ins alled on he sys em. The ROS package con ains a p og am ha con e s URDF iles o he yaml o ma ha is being used by his lib a y. V. COMPARISON TO OTHER LIBRARIES The e ha e been a ious wo ks ha published implemen a- ions o geome ic algeb a. These lib a ies all ha e in common ha hey a e mean o be gene ic geome ic algeb a imple- men a ions ocusing on he compu a ional and ma hema ical aspec s o he algeb a i sel . In con as o ha , ou implemen- a ion is a ge ed speci ically a obo ics applica ions and hus no only implemen s he low-le el algeb aic compu a ions bu also ea u es he compu a ion o he kinema ics and dynamics o se ial manipula o s as well as gene ic cos unc ions o op imal con ol. A. Algeb aic Ope a ions Benchma ks In o de o compa e he pe o mance o ou ga o li- b a y o o he geome ic algeb a lib a ies we o ked he ga_benchma k5 eposi o y in o de o in eg a e ga o. Ou o k can be ound a h ps://gi hub.com/loew / ga-benchma k. Dualiza ion 012345 0 5.126 10.25 15.38 20.5 25.63 30.76 35.88 41.01 46.14 51.26 G ade Time [ns] Re e se 012345 0 10.97 21.94 32.91 43.87 54.84 65.81 76.78 87.75 98.72 109.7 G ade In e sion 012345 0 18.62 37.25 55.87 74.49 93.11 111.7 130.4 149 167.6 186.2 G ade GATL Ga amon Gaale Ve so ga o (a) Benchma ks o una y algeb aic ope a ions. Addi ion 0 6 12 18 24 30 36 0 10.65 21.31 31.96 42.62 53.27 63.93 74.58 85.24 95.89 106.5 G ade Time [ns] Inne P oduc 0 6 12 18 24 30 36 0 7.832 15.66 23.5 31.33 39.16 46.99 54.82 62.65 70.49 78.32 G ade Ou e P oduc 0 6 12 18 24 30 36 0 7.286 14.57 21.86 29.14 36.43 43.71 51 58.29 65.57 72.86 G ade Geome ic P oduc 0 6 12 18 24 30 36 0 43.55 87.09 130.6 174.2 217.7 261.3 304.8 348.4 391.9 435.5 G ade GATL Ga amon Gaale Ve so ga o (b) Benchma ks o bina y algeb aic ope a ions. Fig. 6: Benchma ks o di e en geome ic algeb a lib a ies. All ope a ions a e compu ed using con o mal geome ic algeb a. We omi ed TbGAL om he plo s o he benchma k esul s, since i is by a he slowes lib a y. The benchma ks show ha ga o can compe e in e ms o pe o mance wi h GATL and Ve so , which we e p e iously epo ed o be he as es GA lib a ies. We belie e, howe e , ha he API o ga o is a lo mo e app oachable and easy o use. Fu he mo e, i addi ionally implemen s specialized algo i hms o obo modeling and op imisa ion p oblems. 5h ps://gi hub.com/ga-de elope s/ga-benchma k TABLE III: Compa ison o di e en lib a ies. (a) O e iew o o he geome ic algeb a lib a ies. Ga amon [2] a gene a o o C++ lib a ies dedica ed o geome ic algeb a GATL [3] C++ lib a y o Euclidean, homogeneous/p ojec i e, Minkowski/space ime, con o mal, and a bi a y geo- me ic algeb as using empla e me a-p og amming Ve so [4] ( as ) gene ic C++ lib a y o geome ic algeb as GAL [5] C++17 exp ession compile and engine o compu ing wi h geome ic algeb a Gaigen [6] code gene a o o geome ic algeb a Gaale [7] C++ lib a y o e alua ion o geome ic algeb a ex- p essions o e ing com o able implemen a ion and easonable speed by using exp ession empla es and me ap og amming echniques Gaalop [8] so wa e o op imize geome ic algeb a iles TbGAL [9] C++/Py hon lib a y o Euclidean, homogeneous/p o- jec i e, Minkowski/space ime, con o mal, and a bi- a y geome ic algeb as ep esen ing blades (and e - so s) in hei decomposed s a e o scale o scale high dimensions (b) O e iew o o he lib a ies o obo modeling. DQ Robo ics [10] lib a y o obo modeling and con ol based on dual qua e nion algeb a Pinocchio [11] s a e-o - he-a igid body algo i hms o poly- a icula ed sys ems Raisim [12] mul i-body physics engine o obo ics and AI KDL [13] applica ion independen amewo k o modeling and compu a ion o kinema ic chains Mujoco [14] physics engine o model-based op imisa ion RBDL [15] highly e icien code o bo h o wa d and in e se dynamics o kinema ic chains and b anched models B. Robo ics Algo i hms Benchma ks Since his lib a y implemen s obo kinema ics and dynam- ics algo i hms, we a e compa ing and benchma king ga o agains se e al lib a ies ha a e commonly used in obo ics applica ions. This eposi o y ac ually uses a gi pipeline in o de o con inuously pull he la es changes o he lib a ies and upda e he benchma k esul s. The cu en benchma king esul s on ou sys em can be ound in Figu e 7. As can be seen, ga o is e y compe i i e when i comes o he compu a ion o he kinema ics o a obo ic sys em. The compu a ion o he dynamics, howe e , especially he o wa d dynamics, is s ill slowe a his poin . The eason o his is an ou da ed implemen a ion choice in he lowe le els o he lib a y. This issue will be add essed and ixed in a u u e ealease o ga o, which should make he compu a ion o he dynamics also compe i i e compa ed o he es ablished lib a ies. VI. APPLICATIONS AND TUTORIAL In his sec ion we p o ide some example applica ions o how he lib a y can be used. Fo ha pu pose, we p o ide 0 200 400 600 800 1000 1200 1400 1600 1800 2000 2200 ime [ns] ga o aisim pinocchio KDL Mujoco RBDL DQ Robo ics (a) Fo wa d Kinema ics. 0 200 400 600 800 1000 1200 1400 1600 1800 2000 2200 2400 2600 2800 3000 3200 3400 3600 ime [ns] ga o aisim pinocchio KDL Mujoco RBDL DQ Robo ics (b) Jacobian. 0 5000 10000 15000 20000 ime [ns] ga o aisim pinocchio KDL Mujoco RBDL (c) In e se Dynamics. 0 5000 10000 15000 20000 25000 30000 35000 40000 45000 50000 55000 60000 65000 ime [ns] ga o aisim pinocchio KDL Mujoco RBDL (d) Fo wa d Dynamics. Fig. 7: Benchma ks o obo ics algo i hms. The benchma k was un on an AMD Ryzen 7 4800U CPU. All lib a ies we e compiled using gcc 13.1.1 wi h he compile lags -O3 -ma ch=na i e. The e e ence sys em is he F anka Emika Robo . an accompanying eposi o y ga o_examples ha explains he usage o ga o,pyga o and ga o_ os. No e ha in he ex we a e always e e encing he c++ iles, bu he same examples can also be ound in py hon in he co esponding olde . These examples a e using he same naming scheme. A. Geome ic Algeb a Since many po en ial use s o ga o a e likely o be un a- milia wi h he concep o geome ic algeb a we a e p o iding some examples on how o do compu a ions using his algeb a. 1) Mul i ec o Ope a ions: In a i s example we a e show- ing how o c ea e di e en gene al mul i ec o s and use hem o algeb aic compu a ions. This example can be ound in mul i ec o .cpp. In pa icula , he e we show he usage o he di e en p oduc s in geome ic algeb a and how hey a e used wi h hei co esponding ope a o s ha we e lis ed in Table II. 2) Geome ic P imi i es: An example on how o use he lib a y o cons uc geome ic p imi i es is shown in geo- me ic_p imi i es.cpp. Since hese geome ic p imi i es a e essen ially specializa ions o he base Mul i ec o class all ope a ions ha we e p esen ed in he example in Sec ion VI-A1 a e also alid o he geome ic p imi i es. Hence, we a e showing he cons uc ion o he geome ic p imi i es in di e en ways. Fi s , using hei algeb aic de ini ion based on he ou e p oduc and second, using hei cons uc o s. Essen ially, he cons uc o s implemen he algeb aic de ini- ion, and se e o simpli y he usage o he lib a y. Bu he example should show he equi alence o he cons uc ion. Since geome ic algeb a acili a es geome ic easoning, we a e also p o iding an example o how o p ojec ions, e lec ions and in e sec ions using he geome ic p imi i es, which can be ound in geome ic_p imi i es_incidence.cpp. 3) Con o mal T ans o ma ions: This pa o he u o ial is dedica ed o showing he usage o he di e en Ve so classes, i.e. i shows how o apply con o mal ans o ma- ions o geome ic p imi i es. The e a e ac ually wo pa s o his example. The i s one, which is implemen ed in e so _log_exp_map.cpp, shows how de ine he di e en e - so s using he exponen ial mappings and eco e he gen- e a o bi ec o s using he co esponding loga i hmic map- pings. In he second example, which is implemen ed in e - so s_ ans o ming_geome ic_p imi i es.cpp, we hen show how o apply he e so s o he geome ic p imi i es. B. Robo Modeling One o he a ge ed use cases o he ga o lib a y is he modeling o obo ic sys ems. In his pa o he u o ial we will show how o ha in p ac ice. 1) Gene ic Sys em: A gene ic sys em can be c ea ed using he Sys em class and i s addJoin and addLink unc ions as men ioned in Sec ion IV. No e ha in his case he pa en- /child ela ionships be ween he join s/links need o handled as well. An easie way is o load he sys em desc ip ion om a yaml ile, whe e he lib a y will d op an e o message i no all necessa y ela ionships a e de ined. These wo ways o c ea ing a sys em a e shown in he examples c e- a e_sys em_ om_code.cpp and c ea e_sys em_ om_ ile.cpp. 2) Manipula o Sys em: Manipul o sys ems a e specialized obo ic sys ems, which can be seen om he Manipula o class inhe i ing om he Sys em class. So in o de o de ined a manipula o , a no mal sys em can be c ea ed and hen mo ed o he manipula o sys em wi h he addi ional in o ma ion abou he end-e ec o join in o de o c e- a e he kinema ic chain. We a e again p o iding he wo examples o c ea ing a manipula o om code o om a ile in examples c ea e_manipula o _ om_code.cpp and c e- a e_manipula o _ om_ ile.cpp, espec i ely. 3) Robo Kinema ics/Dynamics: A e c ea ing he model o he sys em and in o de o use he lib a y in p ac ical applica ions, he compu a ion o he kinema ics and dynamics o he sys ems a e necessa y. We show how he compu e all ele an quan i ies using he F ankaEmikaRobo class in he example anka_emika_kinema ics_and_dynamics.cpp. 4) Robo Kinema ics and Geome ic P imi i es: In con- as o o he lib a ies, ga o uses geome ic algeb a o he modeling o obo ic sys ems, which allows he usage o a ious geome ic p imi i es a a kinema ic le el. In anka_emika_geome ic_p im i es.cpp we show an example using he F ankaEmikaRobo o how o mo e hose ge- ome ic p imi i es o he end-e ec o , which will la e be exploi ed in he o mula ion o he op imisa ion p oblems. C. Op imisa ion P oblems Many p oblems in impo an domains o obo ics, such as lea ning and con ol, can be cas as op imiza ion p oblems. Hence, in his sec ion we a e p o iding some examples on how ga o can be used o simpli y he modeling o op imisa ion p oblems using geome ic algeb a. 1) In e se Kinema ics: A common example in obo ics is he compu a ion o he in e se kinema ics. Since we a e o en dealing wi h edundan manipula o s, his becomes an op imisa ion p oblem. We a e modeling his p oblem using he Mo o class o ga o, which ep esen s poses in Euclidean space. An in e se kinema ics p oblem in geome ic algeb a, o mula ed as an op imiza ion p oblem, hus minimizes he e o be ween wo mo o s, which is exp essed ia he log- a i hmic map. The co esponding class ha implemen s his cos unc ion is called SingleManipula o Mo o Cos . I compu es he alue o he cos unc ion, as well as i s g adien and hessian, o allow o i s - o second-o de op imisa ion. The example o compu ing he in e se kine- ma ics using he Gauss-New on algo i hm can be ound in in e se_kinema ics.cpp. 2) Reaching Geome ic P imi i es: Geome ic algeb a ex- ends he cos unc ion o be uni o mly applicable ac oss he di e en geome ic p imi i es. I is implemen ed in he class SingleManipula o Ta ge . The empla e a gu- men s Tool and Ta ge can be di e en combina ions o geome ic p imi i es. He e, we cas he op imisa ion p oblem again as an in e se kinema ics p oblem o simplici y, so we a e op imizing o he join angle con igu a ion in which he end-e ec o eaches a ce ain geome ic p imi i e. In p e ious wo k, howe e , we ha e shown he applica ion o CGA o modeling manipula ion asks in an op imal con ol amewo k o model p edic i e con ol [16], which can o cou se be achie ed using he same cos unc ion. The di e en examples a e lis ed in Table IV. P imi i es File Poin &Poin in e se_kinema ics_poin _poin .cpp Poin &Line in e se_kinema ics_poin _line.cpp Poin &Poin Pai in e se_kinema ics_poin _poin pai .cpp Poin &Ci cle in e se_kinema ics_poin _ci cle.cpp Poin &Plane in e se_kinema ics_poin _plane.cpp Poin &Sphe e in e se_kinema ics_poin _sphe e.cpp Line &Poin in e se_kinema ics_line_poin .cpp Line &Line in e se_kinema ics_line_line.cpp TABLE IV: Example o op imisa ion p oblems using di e en geome ic p imi i es. These examples a e loca ed in he olde ga o_examples/s c/cpp/. No e ha wi h he excep ion o he de ini ion o he cos unc ion, all hese iles a e iden ical. D. ROS Visualiza ion The main pu pose o he ga o_ os package is isualiza ion o he geome ic p imi i es in R iz. Hence, we a e p o iding an example ha isualizes he F anka Emika obo eaching a ious geome ic p imi es. The example is implemen ed isualizing_geome ic_p imi i es.cpp and equi es o be com- piled in a ROS wo kspace. The R iz ou pu o he example is shown in Figu e 8. VII. CONCLUSION In his a icle we p esen ed he implemen a ion de ails as well as a u o ial o ou so wa e s ack a ound ga o, which is a c++ lib a y ha implemen s con o mal geome ic algeb a Poin acking Plane acking Op ion Poin 1 Poin ing Line acking Ci cle acking Op ion Poin 2 Cons ain s Fig. 8: R iz isualiza ions o he F anka Emika eaching a ious geome ic p imi es. o obo ics. The so wa e s ack also includes py hon bindings in pyga o as well as a ROS package in ga o_ os. Tu o ial ma e ial and oy examples can be ound in ga o_examples. While showing compa able pe o mance o he obo mod- eling, geome ic algeb a also o e s an easy and in ui i e way o model a ious geome ic ela ionships. Hence i p o ides a iche oolse han s anda d ma ix algeb a wi hou loosing any o he exis ing ools. Ou lib a y ga o p o ides hese s anda d ools o obo modeling and augmen s hem wi h concep s ha a e exclusi e o geome ic algeb a. P o iding his lib a y ha makes geome ic algeb a easily accessible o obo ics esea ch should allow o a wide adop ion and acili a e he esea ch on using his powe ul amewo k o obo ics. REFERENCES [1] W. Benge and W. Doble , “Massi e Geome ic Algeb a: Visions o C++ Implemen a ions o Geome ic Algeb a o Scale in o he Big Da a E a,” Ad . Appl. Cli o d Algeb as, ol. 27, no. 3, pp. 2153–2174, Sep. 2017. DOI: 10.1007/s00006-017-0780-4. [2] S. B euils, V. Nozick, and L. Fuchs, “Ga amon: A Geome ic Algeb a Lib a y Gene a o ,” Ad . Appl. Cli o d Algeb as, ol. 29, no. 4, p. 69, Jul. 22, 2019. DOI: 10.1007/s00006-019-0987-7. [3] L. A. F. Fe nandes, “Explo ing Lazy E alua ion and Compile-Time Simpli ica ions o E icien Geome ic Algeb a Compu a ions,” in Sys ems, Pa e ns and Da a Enginee ing wi h Geome ic Calculi, S. Xambó-Descamps, Ed., ol. 13, Cham: Sp inge In e na ional Publishing, 2021, pp. 111–131. DOI: 10.1007/978-3-030-74486-1_6. [4] P. Colapin o, “Ve so : Spa ial compu ing wi h con o mal geome ic algeb a,” Uni e si y o Cali o nia a San a Ba ba a, 2011. [5] J. Ong, GAL, h ps://gi hub.com/je emyong/gal: Gi Hub, 2019. [6] D. Fon ijne, “Gaigen 2: A geome ic algeb a implemen a ion gene a- o ,” in P oceedings o he 5 h In e na ional Con e ence on Gene a i e P og amming and Componen Enginee ing - GPCE ’06, Po land, O egon, USA: ACM P ess, 2006, p. 141. DOI: 10.1145/1173706. 1173728. [7] F. Seybold and U. W{ "o}ssne , Gaale - a C++ exp ession empla e lib a y o implemen ing geome ic algeb a, 2010. [8] D. Hildenb and, J. Pi , and A. Koch, “Gaalop—High pe o mance pa allel compu ing based on con o mal geome ic algeb a,” in Geo- me ic Algeb a Compu ing: In Enginee ing and Compu e Science, E. Bay o-Co ochano and G. Scheue mann, Eds., London: Sp inge London, 2010, pp. 477–494. DOI: 10.1007/978-1-84996-108-0_22. [9] E. V. Sousa and L. A. F. Fe nandes, “TbGAL: A Tenso -Based Lib a y o Geome ic Algeb a,” Ad . Appl. Cli o d Algeb as, ol. 30, no. 2, p. 27, Ap . 2020. DOI: 10.1007/s00006-020-1053-1. [10] B. V. Ado no and M. Ma ques Ma inho, “DQ Robo ics: A Lib a y o Robo Modeling and Con ol,” IEEE Robo ics Au oma ion Magazine, ol. 28, no. 3, pp. 102–116, Sep. 2021. DOI: 10.1109/MRA.2020. 2997920. [11] J. Ca pen ie , G. Sau el, G. Buondonno, e al., “The Pinocchio C++ lib a y : A as and lexible implemen a ion o igid body dynamics algo i hms and hei analy ical de i a i es,” in 2019 IEEE/SICE In e na ional Symposium on Sys em In eg a ion (SII), Pa is, F ance: IEEE, Jan. 2019, pp. 614–619. DOI: 10.1109/SII.2019.8700380. [12] J. Hwangbo, J. Lee, and M. Hu e , “Pe -Con ac I e a ion Me hod o Sol ing Con ac Dynamics,” IEEE Robo . Au om. Le ., ol. 3, no. 2, pp. 895–902, Ap . 2018. DOI: 10.1109/LRA.2018.2792536. [13] R. Smi s, KDL: Kinema ics and Dynamics Lib a y, h p://www.o ocos.o g/kdl. [14] E. Todo o , T. E ez, and Y. Tassa, “MuJoCo: A physics engine o model-based con ol,” in 2012 IEEE/RSJ In e na ional Con e ence on In elligen Robo s and Sys ems, Oc . 2012, pp. 5026–5033. DOI: 10.1109/IROS.2012.6386109. [15] M. L. Felis, “RBDL: An e icien igid-body dynamics lib a y using ecu si e algo i hms,” Au on Robo , ol. 41, no. 2, pp. 495–511, Feb. 1, 2017. DOI: 10.1007/s10514-016-9574-0. [16] T. Löw and S. Calinon, “Geome ic Algeb a o Op imal Con ol Wi h Applica ions in Manipula ion Tasks,” IEEE T ansac ions on Robo ics, pp. 1–15, 2023. DOI: 10.1109/TRO.2023.3277282.