Recu si e Fo wa d Dynamics o Se ial Kinema ic
Chains using Con o mal Geome ic Algeb a
Tobias Löw1,2[0000−0003−4001−2770] and Syl ain Calinon1,2[0000−0002−9036−6799]
1Idiap Resea ch Ins i u e, Ma igny Swi ze land.
{ obias.loew, syl ain.calinon}@idiap.ch
2EPFL, Lausanne, Swi ze land
Abs ac . The compu a ion o he o wa d dynamics plays an impo -
an ole in simula ing he mo ion o in e connec ed igid bodies while
conside ing he physical p ope ies and cons ain s o each pa . The
applica ions in g aphics, anima ion, and obo ics usually equi e as
compu a ion, which leads o he usage o as ecu si e algo i hms. In
his pape , we p esen a o mula ion o he ecu si e o wa d dynamics
o se ial kinema ic chains ha ha is oo ed in geome y, which al-
lows coo dina e- ee iew and geome ically meaning ul in e p e a ions
o he in ol ed quan i ies. The ma hema ical amewo k is called con o -
mal geome ic algeb a (CGA) and i ex ends classical ec o algeb a by
in oduc ing a uni ied ep esen a ion o a la ge a ay o geome ic ope -
a ions, ans o ma ions and ma hema ical objec s, such as poin s, lines,
and planes, in a igo ous ye in ui i e manne . Hence, using CGA o he
compu a ion o he o wa d dynamcis p o ides a uni ied ma hema ical
amewo k ha seamlessly in eg a es bo h he geome ic and dynamic
aspec s o he sys em. We alida e he compu a ion nume ically and p o-
ide an implemen a ion o he esul s in an open-sou ce lib a y, making
i immedia ely a ailable in p ac ice.
Keywo ds: Geome ic Algeb a, Dynamics
1 In oduc ion
Simula ion is an in eg al ool o obo ic enginee ing since i allows o he sa e
es ing and e alua ion o algo i hms. Simula ing he mo ion o obo s om o ces
using hei dynamics is u ilized in many di e en ields in obo ics and i is an
impo an pa in he de elopmen o new algo i hms o obo ics. The p oblem
o mapping o ces o mo ion is sol ed by compu ing he o wa d dynamics o
he obo , i.e. mapping he applied join o ques o join accele a ions o a
gi en join posi ion and eloci y. Consequen ly, he e a e many o - he-shel obo
dynamics simula o s a ailable, such as Raisim [1], Isaac Sim [2], Gazebo [3],
Mujoco [4], Bulle [5] o Coppelia Sim [6]. Fu he mo e, he e a e se e al so wa e
lib a ies ha , while no being ull simula ion engines, allow o he e icien
compu a ion o he obo dynamics, e.g. Pinocchio [7], KDL [8] and RBDL [9].
2 Tobias Löw and Syl ain Calinon
The e a e many applica ions whe e he o wa d simula ion o obo ic sys ems
has become an in eg al pa o algo i hm. O en hese o wa d simula ions a e un
in pa allel o ob ain ajec o y ollou s, which equi es e y e icien algo i hms
o he o wa d dynamics, such as in s ochas ic [10] and di e en ial dynamic
p og amming [11] op imal con ol. Sampling-based model p edic i e con ol, ha
is buil upon g adien - ee op imize s such as model p edic i e pa h in eg al
con ol [12], also hea ily le e ages pa allel ollou s p o ided by simula o s [13].
Rolling ou he sys em dynamics is also necessa y o dynamic p og amming
app oaches, including model p edic i e con ol [14], [15]. The same applies o
mo ion planning whe e he dynamics a e simula ed o wa d in ime o de e mine
he e ec s o he planned con ol command [16]. Apa om obo ics, he o wa d
dynamics also play an impo an ole in physics-based anima ion whe e hey a e
used o high-quali y cha ac e anima ions [17].
Ano he aspec when de eloping obo ic algo i hms, apa om he model-
ing o he obo i sel , is he modeling o he asks and he en i onmen . He e,
an inc eased ocus has been pu on exploi ing he unde lying geome y o he
p oblems. The di e en app oaches include powe ul ma hema ical echniques
such as Riemannian mani olds and Lie g oups. They ha e been used especially
a he in e sec ion o lea ning and con ol. Va ious wo ks p esen ed app oaches
o ep esen ing obo skills using Riemannian mani olds and i was shown ha
di e en mani olds can be used o ha pu pose [18]. The mani old can be p ede-
ined, such as lea ning o ien a ions [19], and le e aged in a dic iona y o di e en
mani olds [20]. In [21], he au ho s p esen ed a amewo k o geome y awa e
manipulabili y lea ning, and hen acking he lea ned ajec o y o manipula-
bili y ellipsoids and ans e ing i o di e en sys ems. These wo ks used he
mani old o symme ic posi i e-de ini e ma ices o he skill ep esen a ion. The
unde lying Riemannian mani old can, howe e , also be lea ned o ob ain mo ion
skills and hen la e be used o eac i e mo ion gene a ion [22]. Riemannian
mo ion policies a e ano he geome ic amewo k ha eac i e con ol [23]. In
addi ion o hese geome ic lea ning and con ol me hods, Riemannian op imiza-
ion is also used o exploi geome ic s uc u es o sol ing he in e se kinema ics
p oblem based on dis ance geome y [24].
Apa om hese geome ic echniques, a ma hema ical amewo k called
geome ic algeb a is also gaining ac ion in obo ics esea ch. The s o y o
geome ic algeb a in enginee ing is he s o y o an algeb aic amewo k ha
g ea ly simpli ies well-known equa ions, he mos popula example being he
Maxwell equa ions, which educe o a single equa ion in geome ic algeb a [25].
In obo ics, geome ic algeb a has been used o di e en ial kinema ics [26]. In-
e se kinema ics also se es as a good example o he geome ic signi icance o
geome ic algeb a objec s, since he p oblem can be sol ed i e a i ely by using
he in e sec ion ope a ions o he geome ic p imi i es in he algeb a [27]. O he
applica ions o hese geome ic p imi i es a e ools o medical obo ics o p o-
ide geome ic in ui i e echniques o planning su gical pa hs [28]. The e also
al eady exis s a link o he p e iously men ioned use-cases o he o wa d dy-
namics simula ion and geome ic algeb a in he o m o modeling op imal con ol
Fo wa d Dynamics o Kinema ic Chains using Geome ic Algeb a 3
p oblems using geome ic p imi i es [29]. Albei , in ha wo k he dynamics we e
included a e sol ing he op imal con ol p oblem in he o m o Lag angian in-
e se dynamics in o de o ob ain o que commands. Ano he in e es ing way o
use he geome ic p imi i es is o objec manipula ion om s e eoscopic images
[30]. Ma hema ically, geome ic algeb a p esen s esh insigh s on sc ew heo y
[31], which is o en used in obo ics o exp ess kinema ic ela ionships.
Despi e his g owing popula i y o geome ic algeb a in he ield o obo ics,
he e a e s ill many algo i hms ha need o be de i ed in hei geome ic algeb a
e sion in o de o i o become a single ool o compu a ion in obo ics. One
o hem is he e icien compu a ion o he o wa d dynamics, which we a e
add essing in his pape . Hence, ou con ibu ions a e as ollows:
–we de ine an ine ia enso using elemen s om con o mal geome ic algeb a,
–we o mula e he a icula ed body ine ia in con o mal geome ic algeb a,
–we o mula e he ecu si e o wa d dynamics in con o mal geome ic algeb a.
–we implemen he p esen ed algo i hms in an open-sou ce lib a y ga o3 ha
we i s p esen ed in [29]
The es o he pape is o ganized as ollows: Sec ion 2 p esen s ela ed wo k,
Sec ion 3 in oduces he ma hema ical backg ound o geome ic algeb a and
obo dynamics, in Sec ion 4 we show ou o mula ion o he ine ia enso in
CGA, in Sec ion 5 we hen in oduce he ecu si e dynamics algo i hms in CGA,
inally in Sec ion 6 we discuss some ma hema ical implica ions.
2 Rela ed Wo k
This pape desc ibes he ecu si e compu a ion o he o wa d dynamics in con-
o mal geome ic algeb a. Since his is an impo an concep in obo ics, na u-
ally, he e ha e been been many algo i hms and implemen a ions ackling his
p oblem. Ma hema ically i can be sol ed by de i ing he Lag angian o mula-
ion o he obo dynamics and sol ing o he join accele a ions. This app oach,
howe e equi es he in e sion o he gene alized mass ma ix, which is usually
no he mos e icien way o compu a ion [32]. Since hese dynamics simula ions
a e equen ly used in applica ions ha equi e a high compu a ional e iciency,
such as con ol, i is impo an ha hese accele a ions a e compu ed as as
as possible. The ecu si e o mula ion o sol ing he o wa d dynamics p oblem
called he A icula ed Body Algo i hm (ABA) was i s p esen ed in Fea he -
s one’s seminal wo k [33] and has been shown o ha e complexi y O(n). Ou
algo i hms build on his wo k and we will explain in he ma hema ical discus-
sions how he app oaches di e , and whe e he ad an ages o CGA lie.
Resea che s ha e ealized be o e ha ma hema ical ools om geome y can
lead o simpli ied and uni ied ea men s o he obo dynamics. By ea ing
SE(3) as a Lie g oup and adop ing basic ideas om Riemannian geome y, a
geome ic a ian o he obo dynamics was de i ed in [34]. Tha wo k showed
3h ps://gi lab.com/ga o
4 Tobias Löw and Syl ain Calinon
ha by looking a he p oblem h ough he lens o geome y, a ious imp ac ical
no a ional con en ions can be a oided and ha he connec ions o di e en ial
geome y lead o easily ac o izable and di e en iable equa ions, making i e y
a ac i e o op imiza ion and op imal con ol. These ideas o exploi ing Lie
g oups and Lie algeb as we e ex ended u he o show he na u al eme gence o
a ma ix ac o iza ion o he mass ma ix and i s in e se [35], whe e a ecu si e
algo i hm is embedded wi hin i s s uc u e. Fu he mo e, he inhe en in a iance
w. . he e e ence ame allows o a bi a y ames in he kinema ic modeling
[36]. Ou o mula ion in CGA no only e ains his coo dina e in a iance, bu
also, as we will explain la e , ac ually uses a double co e ing g oup o SE(3) o
i s compu a ions and ha he algeb a con ains mo e classes o ans o ma ions
including non- igid ones.
The heo y o dual qua e nion algeb a p esen s ano he pa adigm o ap-
p oaching p oblems in obo ics om a geome ic pe spec i e. The e a e wo ks
ha a e desc ibing he obo dynamics using dual qua e nion algeb a, cu en ly
in he o m o New on-Eule ype dynamics, which a e less e icien o he com-
pu a ion o he accele a ions [37]. A no able insigh om ha wo k is, ha by
combining he geome ic pe spec i e om sc ew heo y, he ho ougness o Lie
algeb a and a simple algeb aic amewo k in he o m o dual qua e nion algeb a
leads o simpli ied and mo e gene al exp essions. This insigh seamlessly ans-
la es o geome ic algeb a due o hei hei common oo s in Cli o d algeb a.
Hence, he e a e also wo ks ha u ilize geome ic algeb a o de i e a New on-
Eule ype algo i hm o obo in e se dynamics and con ol o manipula o s [38]
and mul icop e s [39]. In con as o hose wo ks, we a e p esen ing a ecu si e
o wa d dynamics algo i hm ollowing Fea he s one’s o malism o he a icu-
la ed body algo i hm. In o de o gi e a comple e o e iew and o in eg a e ou
ine ia enso o mula ion in o he New on-Eule algo i hm, we a e also showing
he in e se dynamics. P e ious wo k in geome ic algeb a also looked a he in-
e ia enso and showed ha i was ully con ained wi hin he geome ic algeb a
G0,6,2[40]. Since his algeb a is compa a i ely la ge, ha wo k asked he ques-
ion ega ding he minimal algeb a con aining he adjoin ope a ion o SE(3).
While he p esen ed ine ia enso and ans o ma ions o i equi e mul iple
ope a ions, he p oposed app oach is ully con ained wi hin he algeb a.
3 Backg ound
In his sec ion we a e b ie ly in oducing obo dynamics and geome ic algeb a.
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.
3.1 Robo Dynamics
The manipula o equa ion o compu ing he dynamics o he sys em is
M(q)¨q+C(q,˙q)˙q+g(q) = τ−τex ,(1)
Fo wa d Dynamics o Kinema ic Chains using Geome ic Algeb a 5
whe e M(q)is known as he ine ia o gene alized mass ma ix, C(q,˙q)is ep-
esen ing Co iolis/cen i ugal o ces, g(q)s ands o he g a i a ional o ces, τis
he ec o o join o ques and τex a e he ex e nal o ques. The p oblem o o -
wa d dynamics hen a ises when sol ing Equa ion (1) o he join accele a ions
¨q, i.e.
¨q=M−1(q)τ−τex −C(q,˙q)˙q−g(q).(2)
This ma ix algeb a app oach o sol ing he o wa d dynamics he e o e equi es
inding he in e se o he gene alized mass ma ix M−1(q), which can be compu-
a ionally demanding. Hence, he p e ious wo k on de i ing ecu si e algo i hms
o he o wa d dynamics showed how he in e se mass ma ix could be e icien ly
ac o ized, alle ia ing he need o ma ix in e sion. This concep ansla es o
he o mula ion o he o wa d dynamics in CGA, since we also don’ compu e
he mass ma ix o i s in e se explici ly.
3.2 Geome ic Algeb a
Geome ic algeb as, also known as Cli o d algeb as, a e ac ually a amily o alge-
b as ha can be ound as he quo ien algeb as o enso algeb as o e quad a ic
spaces. His o ically, geome ic algeb a is a uni ica ion o Hamil on’s qua e nion
algeb a and G assmann’s ex e io algeb a. I is a single algeb a o geome ic
easoning, alle ia ing he need o u ilizing mul iple algeb as o exp ess geome ic
ela ions. Geome ic algeb as a e de ined o ec o spaces Rp,q, o dimension
n=p+q+ , whe e p, q and a e he numbe o basis ec o s ha squa e o
1,-1 and 0, espec i ely.
The undamen al mul iplica ion ope a ion is called he geome ic p oduc
ab =a·b+a∧b,(3)
and is he sum o an inne ·and an ou e ∧p oduc . The ou e p oduc is
equi alen o he ex e io p oduc o G assman algeb a and is an ope a ion ha
spans subspaces. The e o e, unlike he ec o algeb a o Rp,q, , he associa ed
geome ic algeb a Gp,q, uses subspaces o he associa ed ec o space as ele-
men s o compu a ions. The basic elemen s, called blades, a e ound as he ou e
p oduc o klinea ly independen ec o s, which leads o 2n= 2p+q+ blades
o a geome ic algeb a. The linea combina ion o basis blades is hen called a
mul i ec o . The numbe kis hen called g ade o he mul i ec o .
The compu a ion wi h subspaces leads o nullspace ep esen a ions o ge-
ome ic p imi es w. . ei he he inne o he ou e p oduc . In CGA, hese
p imi i es include poin s, lines, planes, ci cles and sphe es. Since hey a e no
wi hin he scope o his pape , we e e o [41] o hei de in ion and cons uc ion
and o [29] o hei usage in op imal con ol applica ions in obo manipula ion.
In his pape , we a e using CGA o o mula e he o wa d dynamics, which
is G4,1. As i has ou basis ec o s ha squa e o 1 and one ha squa es o -1
i is clea ly a pseudo-Euclidean space, in ac i has a Minkowski signa u e. I
can be unde s ood as ex ending he Euclidean space R3wi h a Minkowski plane

6 Tobias Löw and Syl ain Calinon
R1,1, i.e. R4,1=R3⊕R1,1. This leads o se e al ans o ma ions beyond he igid
body ans o ma ions p esen ed in he ollowing sec ion. We will discuss hose
ans o ma ions la e in Sec ion 6.4.
Rigid Body T ans o ma ions Rigid body ans o ma ions in CGA a e achie ed
using a ep esen a ion o he Lie g oup Spin(3)⋉R3called mo o s, which is a dou-
ble co e o he ma ix Lie g oup o igid body ans o ma ions in 3-dimensional
Euclidean space SE(3), i.e. he e exis s a su jec i e wo- o-one homomo phism
om Spin(3) ⋉ R3 o SE(3). The associa ed Lie algeb a o he mo o g oup is
a bi ec o algeb a and is connec ed o he mo o mani old by he exponen ial
map and i s in e se, he loga i hmic map. The bi ec o s o he Lie algeb a a e
essen ially (dual) lines in CGA, i.e. hey de ine he sc ew axis o he mo o .
Mo o s a e also isomo phic o dual qua e nions. The di e ence be ween hem is
ha CGA is a la ge algeb a which in oduces mo e geome ic p imi i es and
alle ia es he need o special adjoin ope a ions o ans o m hem.
Mo o s can be applied o a bi a y mul i ec o s in he algeb a by using a
sandwich p oduc ope a ion (simila o how qua e nions o a e ec o s)
Y=MX
M, (4)
whe e
Ms ands o he e e se o a mo o , which can hough o as being
simila o a conjuga e qua e nion. This ope a ion is g ade p ese ing, i.e. i
lea es he numbe o kbasis ec o s in ou e p oduc ep eseen a ion unchanged
and hus does no change wha he mul i ec o s ep esen . He ein lies one o he
ad an ages o CGA, as i ex ends linea ans o ma ions na u ally om ec o s
o he en i e algeb a, i.e. mo o s can be applied o all mul i ec o s in a uni o m
manne , hey a e a mo e gene al ep esen a ion o igid body ans o ma ions
and can also be used o ans o m all geome ic p imi i es ha a e pa o he
algeb a and no jus ec o s. This ex ension is called ou e mo phism.
Mo o s a e used o ep esen he o wa d kinema ics o a se ial kinema ic
chain, i.e. he mo o M(q), gi en he con igu a ion q, can be compu ed wi h
M(q) =
N
Y
j=1
Mj(qj).(5)
The mo o s Mj(qj)a e he cu en join mo o s o he j- h join gi en he join
posi ion. They a e ound as
Mj(qj) = exp (qjBj),(6)
whe e Bjis he bi ec o desc ibing he sc ew axis o he j- h join . In he ol-
lowing we will be using Mjins ead o Mj(qj) o a sho e no a ion.
Twis s and W enches U ilizing he e minology o sc ew heo y, he sc ews
ha ca y eloci y and o ce in o ma ion a e called wis s and w enches, espec-
i ely. In CGA, hey a e ep esen ed as bi ec o s. The wis s a e iden i ied wi h
Fo wa d Dynamics o Kinema ic Chains using Geome ic Algeb a 7
he bi ec o s ha gene a e igid body mo ions, i.e. he bi ec o ha esul om
he loga i hmic mapping o mo o s. Hence, hei space is ound as
V ∈ span{e23,e13,e12,e1∞,e2∞,e3∞},(7)
whe e he six objec s o ming a wis a e bi ec o s. The space o wis s alge-
b aically co esponds o he bi ec o dual lines ha o m he sc ew axes o
mo o s.
W enches on he o he hand, a e usually called cosc ews, meaning ha he e
is a ce ain duali y ela ionship be ween wis s and w enches. In ma ix Lie alge-
b a, howe e , his duali y is no di ec ly isible, since bo h wis s and w enches
a e simply 6-dimensional ec o s. In con o mal geome ic algeb a on he o he
hand, his duali y is explici ly ound ia mul iplica ion wi h he conjuga e pseu-
doscala Ic=Ie0[42], whe e Iis he s anda d pseudoscala o CGA, i.e.
I=e0123∞. Mul iplica ion o Icwi h a wis yields he space o w enches as
W ∈ span{e23,e13,e12,e01,e02,e03}.(8)
Using hese de ini ions, he inne p oduc o a wis Vand a w ench W
educes o he scala p oduc and calcula es he powe o he mo ion, i.e.
p=−V · W (9)
In Lie heo y, wis s a e elemen s o he Lie algeb a and w enches a e elemen s
o he dual Lie algeb a. As pe he abo e de ini ions his duali y ela ionship can
be di ec ly seen om he di e en bi ec o blades in he espec i e spaces. The
elemen s also allow o a di ec geome ical in e p e a ion: he linea eloci y
pa o wis s (i.e. e1∞,e2∞,e3∞) co esponds o a di ec ion bi ec o , whe eas
he o ce pa o w enches (i.e. e01,e02,e03) co esponds o a angen bi ec o .
This geome ic in e p e a ion u he cla i ies why wis s and w enches ans o m
di e en ly unde igid body ans o ma ions, i.e. why in ma ix Lie algeb a he
adjoin ma ix Ad ans o ms wis s and he dual adjoin ma ix Ad∗ ans o ms
w enches. Using con o mal geome ic algeb a, his dis inc ion is no necessa y,
since, by de ini ion o he algeb a, di ec ion and angen bi ec o s (i.e. linea
eloci ies and o ces) mul iply di e en ly using he geome ic p oduc . Hence, a
mo o can be used o ans o m bo h wis s and w enches acco ding o Equa ion
(4) which means i simul aneously ep esen s he adjoin and he dual adjoin
ope a ion.
Simila ly, a uni ied exp ession o he Lie b acke can be ound in con o mal
geome ic algeb a. The Lie b acke is a linea mapping be ween elemen s o he
Lie algeb a. Fo wis s ac ing on wis s o w enches his mapping can be ound
as
V′=V1× V2and W′=V × W,(10)
whe e ×is he commu a o p oduc ha is de ined as
X×Y=1
2(XY −Y X).(11)
8 Tobias Löw and Syl ain Calinon
The implica ions o hese de ini ions a e e y in e es ing, since CGA simul-
aneously cla i ies he duali y ela ionship o wis s and w enches, by emo ing
he ambigui y o wha a 6-dimensional ec o ep esen s h ough an algeb aically
de e mined di e ence. Fu he mo e, i also uni ies hei ea men by ha ing he
same adjoin ope a ions.
4 Ine ia Tenso
In his sec ion, we a e p esen ing ou o mula ion o he gene al ine ia enso in
CGA. This o mula ion is a di ec ex ension o he de ini ion o he o a ional
ine ia enso in dual qua e nion algeb a [37]. The o a ional pa can be iden-
i ied in con o mal geome ic algeb a wi h he bi ec o blades e23,e13,e12. We
add he bi ec o blades e01,e02,e03, such ha a gene al ine ia elemen becomes
I ∈ span{e23,e13,e12,e01,e02,e03}.(12)
No e ha an ine ia elemen uses he same basis blades as a w ench ha was
de ined in Equa ion (8). This ollows om he ac ha we wan he inne p oduc
o an ine ia elemen and wis s o be he scala p oduc in o de o compu e
he di e en componen s o he esul ing w enches.
Hence, he ine ia enso Iconsis s o six bi ec o - alued ine ia elemen s,
one o each componen o he esul ing w ench. We de ine he geome ic algeb a
ine ia enso as he union o all elemen s belonging o he blade index lis II,
i.e.
I={Iek}ek∈II,(13)
whe e he uppe blade index indica es which componen o he esul ing w ench
i is esponsible o . In acco dance wi h he de ini ion o a w ench, hese blade
indices he e o e a e
II={e23,e13,e12,e01,e02,e03}.(14)
4.1 Linea Mapping om Twis o W ench
The ine ia enso is a linea ope a o ha ac s on wis s and p oduces w enches.
This linea map can be ound as he sum o he inne p oduc s o each ine ia
elemen wi h he gi en wis , i.e.
W=I[V] = −X
ek∈II
(Iek· V)ek.(15)
A his poin we would like o men ion ha we gene ally call he ma he-
ma ical objec s wis s and w enches acco ding o he de inion o he spaces in
Equa ions (7) and (8), espec i ely. The physical in e p e a ion, howe e , can o
cou se be a mapping ei he om eloci y o momen um o accele a ion o o ce.
In bo h cases, he ma hema ical bi ec o spaces a e he same, hence we ea i
using a uni ied e minology.
Fo wa d Dynamics o Kinema ic Chains using Geome ic Algeb a 9
4.2 Rigid Body T ans o ma ion o Ine ia Tenso
O en, i is equi ed o iew he ine ia enso om a ame ha is di e en
o he ine ial ame. Hence, i is necessa y o ha e a o mula ion o how an
ine ia enso beha es unde igid body ans o ma ions. These a e exp essed
using mo o s in CGA, hence, gi en a mo o M, we de ine he ans o ma ion o
an ine ia enso and deno e his ope a ion using he symbol ∗, i.e.
M∗I=(M X
ek∈II
⟨MIek
M⟩ejek!
M)ej∈II
,(16)
whe e he ope a o ⟨·⟩ejex ac s he ejblade componen o he enclosed exp es-
sion.
Wi h equa ions (15) and (16), we can ind he equi a iance ela ionship
MI[V]
M= (M∗I)hMV
Mi.(17)
Hence, ans o ming a w ench p oduced by applying an ine ia enso o a wis ,
gi es he same w ench as i s ans o ming he ine ia and he wis indi idually
and hen applying he ans o med ine ia enso o he ans o med wis .
4.3 Spa ial Ine ia
T adi ionally, he spa ial ine ia is a 6×6symme ic ma ix ha con ains he
in o ma ion abou he o a ional ine ia, he mass and he cen e o mass. We
show in his sec ion how o ob ain i in ou CGA o malism, in o de o gi e a
mo e de ailed explana ion o he in ol ed ope a ions.
Gi en an a bi a y igid body, we ha e i s ine ial pa ame e s as he mass m,
he mo o desc ibing he loca ion o he cen e o mass MCoM and he ine ia
ma ix I, la e is a symme ic posi i e-de ini e ma ix wi h he six independen
componen s ixx, iyy, izz, ixy, ixz, iyz. Thus, we ind he he gene al ine ia enso
Ias a unc ion o mand I, i.e. he elemen s o Ia e
Ie23 =ixxe23 −ixye13 +ixze12,Ie01 =me01,
Ie13 =−ixye23 +iyye13 −iyze12,Ie02 =me02,(18)
Ie12 =ixze23 −iyze13 +izze12,Ie03 =me03.
No e he sign change in ce ain elemen s o he ine ia, ha is due o he me ic
enso o CGA, which led o ou choice o basis bi ec o s o closely ma ch and
acili a e he implemen a ion. Then, we ind he spa ial ine ia enso ICoM by
ans o ming he ine ia enso Iusing he mo o desc ibing he loca ion o he
cen e o mass MCoM
ICoM =MCoM ∗I.(19)
16 Tobias Löw and Syl ain Calinon
h ee bi ec o s ca ying he o ce in o ma ion om he w enches o m he gen-
e a o s o hype bolic o a ions, he e, an in e es ing u u e esea ch di ec ions
would be o unco e he implica ions o his connec ion. The las bi ec o e0∞
is esponsible o dila ions, which means ha CGA also con ains he g oup o
di ec simila i ies Sim(3) as a subg oup. The ac ha CGA con ains non- igid
ans o ma ions could mean ha he p esen ed dynamics algo i hms could be
ex ended o mo e complex dynamics such as so obo ics o o model de o mable
objec s.
7 Conclusion
In his pape , we p esen ed he o mula ion o he ecu si e o wa d dynamics
and he spa ial and a icula ed body ine ia in CGA. Al hough he p esen ed
esul s in hei cu en s age o esea ch a e o heo e ical na u e, we ha e shown
hei alidi y in simula ion expe imen s. Addi ionally, he algo i hms a e imple-
men ed in an open-sou ce lib a y, making hem eady o be used in p ac ical
applica ions. Fu u e wo k on his lib a y now includes add essing he p oblem
o con ac modeling in geome ic algb a, since his would be he nex s ep in o -
de o p o ide ull simula ion capabili ies wi h geome ic algeb a. Howe e , we
wan o poin ou ha by including he o wa d dynamics, geome ic algeb a, in
p inciple, is now capable o eplacing adi ional ec o /ma ix based lib a ies
o compu ing he kinema ics and dynamics o se ial kinema ic chains, since all
equi ed algo i hms a e now a ailable. I , howe e , o e s many addi ional ools
o geome ically modeling a ious p oblems in obo ics.
The o mula ion o CGA o e s a uni ied iew on sc ews, i allows o he
coo dina e in a ian o mula ions o Lie g oups and has compu a ional and ep-
esen a ional ad an ages. Thus i no only uni ies se e al amewo ks in o one
algeb a, bu also, ia he di ec connec ion o he pin g oup as a double co e
o he o hogonal g oup, i acili a es he geome ic in e p e a ion o con o mal
mappings compa ed o ma ix algeb a and makes hem compu a ionally mo e
e icien .
The cu en esea ch o cou se is limi ed by i s implemen a ion and he e-
sul ing lowe compu a ional pe o mance compa ed o o he lib a ies. Fu u e
esea ch on using CGA can he e o e ha e se e al di ec ions. Fi s , i should
be explo ed how he di e en possible implemen a ions o geome ic algeb a a -
ec he pe o mance o he algo i hms and o imp o e he implemen a ion in
gene al. And second, he ma hema ical backg ound o geome ic algeb a o e s
se e al di ec ions o possible ex ensions and applica ions o o he a eas such as
de o mable objec s and so obo ics. Fo his eason, he con ibu ions should
be seen om a ma hema ical pe spec i e, opening doo s o new po en ial e-
sea ch di ec ions and unco e ing ma hema ical and geome ic connec ions ha
a e hidden in o he amewo ks.

Fo wa d Dynamics o Kinema ic Chains using Geome ic Algeb a 17
Re e ences
1. Hwangbo, J., Lee, J. & Hu e , M. Pe -Con ac I e a ion Me hod o Sol ing
Con ac Dynamics. IEEE Robo . Au om. Le . 3, 895–902 (Ap . 2018).
2. Liang, J. e al. GPU-Accele a ed Robo ic Simula ion o Dis ibu ed Rein o ce-
men Lea ning h p://a xi .o g/abs/1810.05762 (2024). P e-published.
3. Koenig, N. & Howa d, A. Design and Use Pa adigms o Gazebo, an Open-Sou ce
Mul i-Robo Simula o in 2004 IEEE/RSJ In e na ional Con e ence on In elli-
gen Robo s and Sys ems (IROS) (IEEE Ca . No.04CH37566) 2004 IEEE/RSJ
In e na ional Con e ence on In elligen Robo s and Sys ems (IROS) (IEEE Ca .
No.04CH37566). 3(IEEE, Sendai, Japan, 2004), 2149–2154.
4. Todo o , E., E ez, T. & Tassa, Y. 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 2012 IEEE/RSJ In e na ional Con e ence on In elligen Robo s and Sys-
ems (Oc . 2012), 5026–5033.
5. Coumans, E. & Bai, Y. PyBulle , a Py hon Module o Physics Simula ion o
Games, Robo ics and Machine Lea ning 2016–2021.
6. V-REP: A Ve sa ile and Scalable Robo Simula ion F amewo k | IEEE Con e ence
Publica ion | IEEE Xplo e h ps://ieeexplo e.ieee.o g/documen /6696520
(2024).
7. Ca pen ie , J. e al. The Pinocchio C++ Lib a y : A Fas and Flexible Imple-
men a ion o Rigid Body Dynamics Algo i hms and Thei Analy ical De i a i es
in 2019 IEEE/SICE In e na ional Symposium on Sys em In eg a ion (SII) 2019
IEEE/SICE In e na ional Symposium on Sys em In eg a ion (SII) (IEEE, Pa is,
F ance, Jan. 2019), 614–619.
8. Smi s, R. KDL: Kinema ics and Dynamics Lib a y h p://www.o ocos.o g/kdl.
9. Felis, M. L. RBDL: An E icien Rigid-Body Dynamics Lib a y Using Recu si e
Algo i hms. Au on Robo 41, 495–511 (Feb. 1, 2017).
10. Ca ius, J., Ran l, R., Fa shidian, F. & Hu e , M. Cons ained S ochas ic Op i-
mal Con ol wi h Lea ned Impo ance Sampling: A Pa h In eg al App oach. The
In e na ional Jou nal o Robo ics Resea ch 41, 189–209 (Feb. 2022).
11. Mas alli, C. e al. C ocoddyl: An E icien and Ve sa ile F amewo k o Mul i-
Con ac Op imal Con ol h p : / / a xi . o g / abs / 1909 . 04947 (2022). P e-
published.
12. Williams, G., Ald ich, A. & Theodo ou, E. A. Model P edic i e Pa h In eg al
Con ol: F om Theo y o Pa allel Compu a ion. Jou nal o Guidance, Con ol,
and Dynamics 40, 344–357 (Feb. 2017).
13. Pezza o, C. e al. Sampling-Based Model P edic i e Con ol Le e aging Pa al-
lelizable Physics Simula ions h p://a xi .o g/abs/2307.09105 (2024). P e-
published.
14. Sleiman, J.-P., Fa shidian, F. & Hu e , M. Cons ain Handling in Con inuous-
Time DDP-Based Model P edic i e Con ol in 2021 IEEE In e na ional Con e -
ence on Robo ics and Au oma ion (ICRA) 2021 IEEE In e na ional Con e ence
on Robo ics and Au oma ion (ICRA) (IEEE, Xi’an, China, May 30, 2021), 8209–
8215.
15. Bjelonic, M. e al. O line Mo ion Lib a ies and Online MPC o Ad anced Mo-
bili y Skills. The In e na ional Jou nal o Robo ics Resea ch, 22.
16. Plaku, E., Ka aki, L. E. & Va di, M. Y. Mo ion Planning Wi h Dynamics by a
Syne gis ic Combina ion o Laye s o Planning. IEEE T ans. Robo . 26, 469–482
(June 2010).
18 Tobias Löw and Syl ain Calinon
17. Liu, L., Yin, K., an de Panne, M., Shao, T. & Xu, W. Sampling-Based Con ac -
ich Mo ion Con ol.
18. Calinon, S. Gaussians on Riemannian Mani olds: Applica ions o Robo Lea ning
and Adap i e Con ol. IEEE Robo ics Au oma ion Magazine 27, 33–45 (June
2020).
19. Sa e iano, M., Abu-Dakka, F. J. & Ky ki, V. Lea ning S able Robo ic Skills on
Riemannian Mani olds. Robo ics and Au onomous Sys ems 169, 104510 (No . 1,
2023).
20. Ti, B., Razmjoo, A., Gao, Y., Zhao, J. & Calinon, S. A Geome ic Op imal Con-
ol App oach o Imi a ion and Gene aliza ion o Manipula ion Skills. Robo ics
and Au onomous Sys ems 164, 104413 (June 1, 2023).
21. Jaquie , N., Rozo, L., Caldwell, D. G. & Calinon, S. Geome y-Awa e Manipu-
labili y Lea ning, T acking, and T ans e . The In e na ional Jou nal o Robo ics
Resea ch 40, 624–650 (Feb. 2021).
22. Beik-Mohammadi, H., Haube g, S., A ani idis, G., Neumann, G. & Rozo, L. Re-
ac i e Mo ion Gene a ion on Lea ned Riemannian Mani olds. The In e na ional
Jou nal o Robo ics Resea ch, 02783649231193046 (Aug. 28, 2023).
23. Cheng, C.-A. e al. RMP low: A Geome ic F amewo k o Gene a ion o Mul i-
ask Mo ion Policies. IEEE T ansac ions on Au oma ion Science and Enginee ing
18, 968–987 (July 2021).
24. Ma ić, F. e al. Riemannian Op imiza ion o Dis ance-Geome ic In e se Kine-
ma ics. IEEE T ans. Robo . 38, 1703–1722 (June 2022).
25. Baylis, W. E. in Ablamowicz, R. e al. Lec u es on Cli o d (Geome ic) Alge-
b as and Applica ions (eds Abłamowicz, R. & Sobczyk, G.) 91–133 (Bi khäuse
Bos on, Bos on, MA, 2004).
26. Bay o-Co ochano, E. & Zamo a-Esqui el, J. Di e en ial and In e se Kinema ics
o Robo De ices Using Con o mal Geome ic Algeb a. Robo ica 25, 43–61 (Jan.
2007).
27. A is idou, A. & Lasenby, J. FABRIK: A Fas , I e a i e Sol e o he In e se
Kinema ics P oblem. G aphical Models 73, 243–260 (Sep . 2011).
28. Bay o-Co ochano, E., Ga za-Bu gos, A. M. & Del-Valle-Padilla, J. L. Geome ic
In ui i e Techniques o Human Machine In e ac ion in Medical Robo ics. In J
o Soc Robo ics 12, 91–112 (Jan. 2020).
29. Löw, T. & Calinon, S. Geome ic Algeb a o Op imal Con ol Wi h Applica ions
in Manipula ion Tasks. IEEE T ansac ions on Robo ics 39, 3586–3600 (2023).
30. Zamo a-Esqui el, J. & Bay o-Co ochano, E. Robo Objec Manipula ion Us-
ing S e eoscopic Vision and Con o mal Geome ic Algeb a. Applied Bionics and
Biomechanics 8, 411–428 (2011).
31. Dela osse, L. A New App oach o Sc ew Theo y Using Geome ic Algeb a.
32. Fea he s one, R. & O in, D. Robo Dynamics: Equa ions and Algo i hms in P o-
ceedings 2000 ICRA. Millennium Con e ence. IEEE In e na ional Con e ence
on Robo ics and Au oma ion. Symposia P oceedings (Ca . No.00CH37065) 2000
ICRA. IEEE In e na ional Con e ence on Robo ics and Au oma ion. 1(IEEE,
San F ancisco, CA, USA, 2000), 826–834.
33. Fea he s one, R. Robo Dynamics Algo i hms (Sp inge US, Bos on, MA, 1987).
34. Pa k, F., Bob ow, J. & Ploen, S. A Lie G oup Fo mula ion o Robo Dynamics.
The In e na ional Jou nal o Robo ics Resea ch 14, 609–618 (Dec. 1995).
35. Ploen, S. & Bob ow, J. An O(n) Geome ic Algo i hm o Manipula o Fo wa d
Dynamics in 1997 8 h In e na ional Con e ence on Ad anced Robo ics. P oceed-
Fo wa d Dynamics o Kinema ic Chains using Geome ic Algeb a 19
ings. ICAR’97 1997 8 h In e na ional Con e ence on Ad anced Robo ics. P o-
ceedings. ICAR’97 (July 1997), 563–568.
36. Mülle , A. Sc ew and Lie G oup Theo y in Mul ibody Dynamics: Recu si e Algo-
i hms and Equa ions o Mo ion o T ee-Topology Sys ems. Mul ibody Sys Dyn
42, 219–248 (Feb. 2018).
37. A onso Sil a, F. F., José Qui oz-Omaña, J. & Vilhena Ado no, B. Dynamics o
Mobile Manipula o s Using Dual Qua e nion Algeb a. Jou nal o Mechanisms
and Robo ics 14 (Sep . 14, 2022).
38. Bay o-Co ochano, E., Med ano-He mosillo, J., Osuna-González, G. & U ios egui-
Lego e a, U. New on–Eule Modeling and Hamil onians o Robo Con ol in he
Geome ic Algeb a. Robo ica 40, 4031–4055 (No . 2022).
39. A ellano-Mu o, C. A., Osuna-González, G., Cas illo-Toledo, B. & Bay o-Co ochano,
E. New on–Eule Modeling and Con ol o a Mul i-cop e Using Mo o Algeb a.
Ad . Appl. Cli o d Algeb as 30, 19 (Ap . 2020).
40. Selig, J. M. & Bay o-Co ochano, E. Rigid Body Dynamics Using Cli o d Alge-
b a. AACA 20, 141–154 (Ma . 2010).
41. Pe wass, C. Geome ic Algeb a wi h Applica ions in Enginee ing Geome y and
Compu ing 4(Sp inge , Be lin, 2009).
42. Hes enes, D. in Geome ic Algeb a Compu ing: In Enginee ing and Compu e
Science (eds Bay o-Co ochano, E. & Scheue mann, G.) 3–33 (Sp inge , London,
2010).
43. Fea he s one, R. Rigid Body Dynamics Algo i hms (Sp inge US, Bos on, MA,
2008).
44. Bay o-Co ochano, E. Robo Modeling and Con ol Using he Mo o Algeb a F ame-
wo k in 2019 12 h In e na ional Wo kshop on Robo Mo ion and Con ol (Ro-
MoCo) 2019 12 h In e na ional Wo kshop on Robo Mo ion and Con ol (Ro-
MoCo) (July 2019), 1–8.
45. Paszke, A. e al. PyTo ch: An Impe a i e S yle, High-Pe o mance Deep Lea ning
Lib a y.
46. Dan am, N. T. Robus and E icien Fo wa d, Di e en ial, and In e se Kinema ics
Using Dual Qua e nions. The In e na ional Jou nal o Robo ics Resea ch 40,
1087–1105 (Sep . 2021).
47. Fea he s one, R. A Beginne ’s Guide o 6-D Vec o s (Pa 1). IEEE Robo . Au-
oma . Mag. 17, 83–94 (Sep . 2010).
48. Vaz J ., J. & Rocha J ., R. da. An In oduc ion o Cli o d Algeb as and Spino s
Fi s edi ion. 242 pp. (Ox o d Uni e si y P ess, Ox o d, 2016).