Mas e ’s Final Thesis
Mas e ’s Deg ee in Au oma ic Con ol and Robo ics
(MUAR)
Assessmen o an open-sou ce
so wa e (Mo eI ) o obo ics
eaching and i s applica ions o
he Mas e ’s deg ee in Au oma ic
Con ol and Robo ics
Au o : Lucas Fosco To nielli
Di ec o : C is ina Lampón Dies e
Call: July 2024
Escola Tècnica Supe io
d’Enginye ia Indus ial de Ba celona
2
3
Resum
El p esen eball p e én e una in es igació sob e la plani icació de u es i la
manipulació de obo s u ili zan el p og ama i Mo eI , pe al pos e io es udi de les se es
aplicacions en el Màs e de Con ol Au omà ic i Robò ica.
Es comença amb una e isió his ò ica de la plani icació de u es, des acan l’e olució
dels algo ismes des de mè odes simples ins a sis emes complexos capaços d’ope a
en emps eal i en en o ns dinàmics. També es comen en emes bàsics de p og amació
en ROS 2. Aques a base eò ica p e én explica el mínim i necessa i pe en end e les
explicacions que es a an en el següen apa a sob e l'eina.
Mo eI es des aca com a una pla a o ma uni icada que in eg a múl iples plani icado s de
mo imen i sis emes de pe cepció, pe adap a -se als di e en s ni ells de coneixemen
dels usua is. P ecisamen , el seu pun o és que, g àcies a la se a in e ície g à ica, no
és necessa i sabe p og ama pe pode isuali za la plani icació i execució d’un camí
en un b aç obo . Pe ò ambé es poden du a e me asques més complicades, sen el
majo limi ado els coneixemen s de p og amació de l’usua i.
Du an l’explicació es p e én ana pujan el ni ell amb cada apa a , ambé pe
aconsegui asques més complexes. Es comença amb la in eg ació d’un obo ,
exempli ica amb el model UR3 de Uni e sal Robo s, i la se a pos e io con igu ació en
Mo eI . Es p ossegueix amb l’execució de mo imen s pun a pun i l’e asió d’objec es
de col·lisió en l’en o n de simulació, i s’acaba amb un exemple de “pick and place”.
Una egada es udiada l’eina, es p ocedi à a l’explicació de les possibles in eg acions en
les assigna u es del Màs e que més pod ien bene icia -se. També es dona an possibles
millo es a l’aplicació enin en comp e els seus usos.
Finalmen , s'acaba l'es udi amb l'exposició de les conclusions a què s’ha a iba e e en
als objec ius plan eja s.
4
Resumen
El p esen e abajo p e ende hace una in es igación sob e la plani icación de u as y la
manipulación de obo s u ilizando el so wa e Mo eI , pa a el pos e io es udio de sus
aplicaciones en el Más e de Con ol Au omá ico i Robó ica.
Se comienza con una e isión his ó ica de la plani icación de u as, des acando la
e olución de los algo i mos desde mé odos simples has a sis emas complejos capaces
de ope a en iempo eal y en en o nos dinámicos. También se comen an emas básicos
de p og amación en ROS 2. Es a base eó ica p e ende explica lo mínimo y necesa io
pa a en ende las explicaciones que se ha án en el siguien e apa ado sob e la
he amien a.
Mo eI se des aca como una pla a o ma uni icada que in eg a múl iples plani icado es
de mo imien o y sis emas de pe cepción, pa a adap a se a los di e en es ni eles de
conocimien o de los usua ios. P ecisamen e, su pun o ue e es que, g acias a su
in e az g á ica, no es necesa io sabe p og ama pa a pode isualiza la plani icación
y ejecución de un plan en un b azo obo . Pe o ambién se pueden lle a a cabo labo es
más complicadas, siendo el mayo limi ado los conocimien os de p og amación del
usua io.
Du an e la explicación se p e ende i subiendo el ni el con cada apa ado, ambién pa a
consegui a eas más complejas. Se empieza con la in eg ación de un obo ,
ejempli icado con el modelo UR3 de Uni e sal Robo s, y su pos e io con igu ación en
Mo eI . Se p osigue con la ejecución de mo imien os pun o a pun o y la e asión de
obje os de colisión en el en o no de simulación, y se acaba con un ejemplo de “pick and
place”.
Una ez es udiada la he amien a, se p ocede á a la explicación de las posibles
in eg aciones en las asigna u as del Más e que más pod ían bene icia se. También se
da án posibles mejo as a la aplicación eniendo en cuen a sus usos.
Finalmen e, se e mina el es udio con la exposición de las conclusiones a las que se ha
llegado e e en e a los obje i os plan eados.
5
Abs ac
The p esen wo k aims o conduc esea ch on ou e planning and obo manipula ion
using he Mo eI so wa e, o subsequen s udy o i s applica ions in he Mas e 's
p og am in Au oma ic Con ol and Robo ics..
I begins wi h a his o ical e iew o ou e planning, highligh ing he e olu ion o algo i hms
om simple me hods o complex sys ems capable o ope a ing in eal- ime and dynamic
en i onmen s. Basic p og amming opics in ROS 2 a e also discussed. This heo e ical
ounda ion aims o explain he minimum necessa y o unde s and he explana ions ha
will be made in he nex sec ion abou he ool.
Mo eI s ands ou as a uni ied pla o m ha in eg a es mul iple mo ion planne s and
pe cep ion sys ems, o accommoda e di e en le els o use knowledge. P ecisely, i s
s ong poin is ha , hanks o i s g aphical in e ace, i is no necessa y o know how o
p og am in o de o isualize he planning and execu ion o a pa h in a obo ic a m.
Howe e , mo e complex asks can also be ca ied ou , wi h he main limi a ion being he
use ’s p og amming knowledge.
Du ing he explana ion, he in en ion is o g adually inc ease he le el wi h each sec ion,
also o achie e mo e complex asks. I s a s wi h he in eg a ion o a obo , exempli ied
by he UR3 model om Uni e sal Robo s, and i s subsequen con igu a ion in Mo eI . I
con inues wi h he execu ion o poin - o-poin mo emen s and he a oidance o collision
objec s in he simula ion en i onmen , ending wi h a pick and place example.
Once he ool has been s udied, he possible in eg a ions in o he Mas e ’s cou ses ha
could bene i he mos will be explained. Possible imp o emen s o he applica ion will
also be sugges ed, aking i s uses in o accoun .
Finally, he s udy concludes wi h he p esen a ion o he conclusions eached ega ding
he objec i es se .

6
INDEX
Resum ......................................................................................................................... 3
Resumen ..................................................................................................................... 4
Abs ac ....................................................................................................................... 5
1. In oduc ion .......................................................................................................... 8
2. S a e o he a ...................................................................................................... 9
2.1. Pa h planning so wa e’s ................................................................................. 9
2.2. Mo eI ........................................................................................................... 11
3. Theo e ical backg ound .................................................................................... 13
3.1. Pa h planning algo i hms .............................................................................. 13
3.1.1. Classical me hods: Cell decomposi ion .................................................. 13
3.1.2. Classical me hods: Po en ial Fields ....................................................... 14
3.1.3. Classical me hods: Road maps ............................................................. 16
3.1.4. Sampling-based algo i hms ................................................................... 17
3.1.5. Heu is ic-based algo i hms .................................................................... 18
3.2. Robo ic Ope a ing Sys em (ROS) ................................................................. 21
3.2.1. Topics .................................................................................................... 22
3.2.2. Se ices ................................................................................................. 24
3.2.3. Launch................................................................................................... 26
3.2.4. Uni ied Robo Desc ip ion Fo ma (URDF) ............................................. 27
4. Mo eI 2 .............................................................................................................. 29
4.1. In eg a ion o a obo model .......................................................................... 29
4.1.1. F om SolidWo ks o URDF .................................................................... 29
4.1.2. Mo eI Se up Assis an .......................................................................... 31
4.2. Launching a obo wi h Mo eI ...................................................................... 35
4.2.1. Mo e g oup node ................................................................................... 35
4.2.2. O he nodes ........................................................................................... 37
4.3. Mo eI GUI in RViz ....................................................................................... 38
4.4. Poin o poin mo emen ............................................................................... 41
4.4.1. Collision Objec s .................................................................................... 42
4.5. Pick and place .............................................................................................. 43
4.5.1. P imi i e s ages ..................................................................................... 44
4.5.2. Con aine s s ages .................................................................................. 44
4.5.3. Example o pick and place asks ............................................................ 45
4.5.4. Coding s ages ........................................................................................ 47
5. Mo eI applica ions o he Mas e ..................................................................... 50
5.1. Applica ions o Robo ics, Kinema ics & Dynamics Sys ems subjec .............. 50
7
5.2. Applica ions o In oduc ion o ROS subjec .................................................. 52
5.3. Applica ions o Planning and Implemen a ion o Robo ic Sys ems subjec .... 55
5.3.1. Benchma king using Mo eI ................................................................... 55
5.3.2. Visualiza ion o he esul s ..................................................................... 56
6. Possible imp o emen s ..................................................................................... 58
6.1. Documen a ion ............................................................................................. 58
6.2. Mobile obo s ................................................................................................ 58
6.3. Vi ual eali y (VR), Augmen ed Reali y (AR) and A i icial In elligence (AI) ... 59
7. Conclusions ....................................................................................................... 60
Acknowledgemen s .................................................................................................. 62
Annexes ..................................................................................................................... 63
Re e ences ................................................................................................................ 73
Bibliog aphy .............................................................................................................. 76
8
1. In oduc ion
Pa h planning, is a undamen al discipline in he ield o obo ics. I ocuses on de eloping
algo i hms and me hods o guide a obo om an ini ial poin o a goal poin , a oiding
obs acles and op imizing speci ic c i e ia such as ime, ene gy, o dis ance.
In he wo ld o obo ics, plan e icien and sa ely ou es is c ucial o execu e complex
asks and ensu ing sa e in e ac ion wi h humans and o he obo s. The e olu ion o pa h
planning has enabled obo s o ope a e in eal- ime and wi h dynamic en i onmen s,
adap ing and op imizing hei beha io based on en i onmen al condi ions.
Figu e 1. Example o pa h planning and obs acle a oidance in a dynamic
en i onmen . Sou ce: [1].
The de elopmen o applica ions ha can simula e hese p ocesses allows p o essionals
o es and e ine algo i hms in i ual en i onmen s be o e implemen ing hem in physical
obo s. Addi ionally, hey p o ide a pla o m o eaching and lea ning, acili a ing he
unde s anding o complex obo ics and p og amming concep s wi hou he need o
access o eal and expensi e obo s.
The objec i es o his wo k a e o in es iga e and analyze he speci ic capabili ies o he
Mo eI so wa e o pa h planning and obo manipula ion. Du ing he lea ning p ocess,
he goal is o implemen , con igu e, and con ol a UR3 obo . S a ing wi h execu ing
basic mo emen s, he aim is o g adually achie e mo e ad anced asks in a simula ion
en i onmen . The ul ima e pu pose is o explo e po en ial in eg a ions o Mo eI in o he
Mas e 's cou ses, iden i ying hose ha could mos bene i om i s applica ions, and e en
p oposing imp o emen s and adap a ions o he Mo eI so wa e, conside ing i s
possible uses in educa ional and p o essional con ex s.
Rega ding he scope, he wo k will mainly ocus on p o iding p edominan ly heo e ical
in o ma ion abou pa h and mo ion planning on he Mo eI pla o m, along wi h some
mo e gene al aspec s o obo ics. I will no del e in o mo e complex opics, such as he
con ol o manipula o s, and al hough many explana ions will be accompanied by code,
bu no all he necessa y code o all de elopmen s ha will be p esen ed.
To s a unde s anding he concep s o be discussed, i is impo an o explain he cu en
ou look o planne s and he a ailable so wa e.
9
2. S a e o he a
2.1. Pa h planning so wa e’s
The main objec i e o pa h planning is o de elop a ou e om a s a ing poin o he desi ed
des ina ion, a oiding obs acles and op imizing ce ain c i e ia such as ime, ene gy o
dis ance. His o ically, pa h planning has e ol ed om simple algo i hms o s a ic
en i onmen s o complex sys ems capable o ope a ing in dynamic and eal- ime
en i onmen s.
The ele ance o pa h planning inc eased signi ican ly wi h he Shakey mobile obo p ojec
in he 1960s, “ he i s mobile obo wi h he abili y o eason abou i s own ac ions” (Pe e
Ha , 1972, [2]). F om hen on, isibili y g aph me hods o na iga ion a ound obs acles
began o be in en ed O e he yea s, wi h ad ances in compu ing and obo ics, pa h
planning echniques ha e e ol ed o add ess mo e complex and dynamic applica ions in
di e se en i onmen s, om manu ac u ing o au onomous explo a ion [3].
This con inuous de elopmen has been d i en by he need o obo s and au oma ed
sys ems o ope a e a e e -inc easing speeds, wi hou comp omising accu acy and sa e y,
leading o he c ea ion o sophis ica ed algo i hms ha op imize sa e and e icien ou es o
a ious obo ic geome ies and compu a ional cons ain s.
The e a e a lo o me hodologies, howe e , he classical mo ion planning me hods a e
usually sepa a ed in o: oadmap echniques, cell decomposi ion and a i icial po en ial
me hods.
− Roadmap echniques: c ea e an abs ac "map" o he con igu a ion space using
nodes and edges o ep esen possible pa hs. Nodes ep esen collision- ee
con igu a ions, and edges ep esen iable pa hs be ween hem.
− Cell Decomposi ion me hods: his in ol es di iding he con igu a ion space in o
egions o cells, so each cell can be easily na iga ed, and con iguous cells a e
connec ed i i is possible o mo e be ween hem wi hou collisions.
− A i icial Po en ial me hods: he obo mo es depending a g adien calcula ed in
using he con igu a ion space de ined as a o ce ield, whe e a ge s a ac he
obo (a posi i e alue is assigned o his ield) and obs acles epel i (nega i e
alues assigned).
To help use s in selec ing he app op ia e pa h planning so wa e, Table 1 p o ides a
de ailed compa ison o se e al p incipal lib a ies and so wa e packages. This able
highligh s key ea u es, equi emen s, and ypical use cases o each ool, o e ing a
p ac ical guide o choosing he igh solu ion based on he speci ic needs o a p ojec .
16
beginning, and s a wi h he pa h wi h he highes g adien . I a local minimum is eached,
he second-bes sa ed ou e is aken, and so on un il he inal goal is eached.
The second me hod consis s on inding a subop imal pa h as he i s ask, wi hou aking
in o accoun he cons ain s ha he p oblem may need o accomplish. Th ough local
changes, he me hod ies o imp o e his solu ion by educing he numbe o b oken
cons ain s. When he algo i hm eaches a local minimum, i inc eases he ‘weigh ’ o he
iola ed cons ain s a ha poin . This modi ies he sea ch ‘landscape’, aising he cos
o s aying a he local minimum and allowing he algo i hm o con inue i s sea ch [25].
Po en ial ield me hods a e used o se e al easons, he simplici y in hei
implemen a ion means ha he compu a ional esou ces a e no high and he sea ch o
possible pa hs is as , some hing e y use ul in dynamic and eal- ime en i onmen s. As
men ioned, i is impo an o ake in o accoun hei limi a ions in ields wi h local minima.
Howe e , he combina ion o hese me hods wi h op imize s makes hem a e y popula
op ion, especially in en i onmen s wi h mobile obo s.
3.1.3. Classical me hods: Road maps
Roadmap me hods ocus on he cons uc ion o a map composed o pa hs connec ing
collision- ee a eas in he obo 's wo kspace. This map ac s as a ne wo k o possible
ou es ha he obo can ollow o each i s des ina ion.
The g aph is cons uc ed by aking in o accoun ha he nodes ep esen speci ic
con igu a ions in he space and he edges ep esen sa e pa hs be ween hese nodes.
Once he g aph is cons uc ed, a pa h- inding algo i hm is used o ind he sho es ou e
om he ini ial posi ion o he desi ed inal posi ion.
The mos commonly used algo i hms o his ype a e:
- Visibili y G aphs: Uses he e ices o obs acles and he s a and end poin s o
cons uc he g aph, whe e e ices a e di ec ly connec ed i hey a e isible o
each o he wi hou he in e e ence o any obs acle.
Figu e 4. Visibili y G aph example. Sou ce: [26].

17
- Vo onoi Diag ams: C ea es a g aph based on poin s ha a e equidis an om he
nea es obs acles, p o iding pa hs ha maximize he minimum dis ance o
obs acles, which is use ul in igh en i onmen s.
Road-mapping me hods ha e become e y popula in en i onmen s whe e he map is
known because o i s speed in inding iable solu ions using samples. As will be
explained in he nex sec ion, sample-based me hods sp ead samples ac oss he map
in a andom o o de ed manne o c ea e he nodes ha will be used o c ea e he
oadmap.
3.1.4. Sampling-based algo i hms
Sample-based me hods a e essen ial used o complex na iga ion and mo emen
p oblems in high-dimensional en i onmen s wi h a ying cons ain s.
As he name sugges s, a e ocused on c ea ing a se ies o samples ac oss he map,
which will be ca alogued and hen connec ed o build he pa h in a as and e icien way.
The mos p ominen algo i hms in his ield a e he P obabilis ic Roadmap Me hod (PRM)
and Rapidly-explo ing Random T ees (RRT).
The PRM is a ype o oadmap ha is sepa a ed in o wo phases: a cons uc ion phase
and a que y phase. In he i s phase, nodes/samples a e andomly gene a ed in he ee
con igu a ion space, hen an a emp is made o connec hese nodes by means o simple
segmen s, a oiding collisions, and inally o ming a g aph. Gi en he s a and end poin s,
he second pa aims o connec hem o he exis ing g aph and hen ind he sho es
pa h be ween hem using sea ch and op imiza ion algo i hms such as A* o Dijks a.
Figu e 5. PRM oadmap example. Solid g een line
ep esen ing he i s phase, and do ed line he
second one. Sou ce: [27].
I is impo an o ake in o accoun ha his me hod is only used in s a ic en i onmen s
whe e he con igu a ion space does no change, o he wise he second phase would
ne e be execu ed. I is especially e ec i e in high-dimensional spaces and can handle
di e se que ies e icien ly once he g aph is cons uc ed. E en he cons uc ion p ocess
can be cos ly, i s ad an age lies in he eusabili y o he ne wo k o mul iple pa h que ies.
18
In con as o PRM, RRT ocuses on pe o m a quick explo a ion o he con igu a ion
space and is pa icula ly use ul when a solu ion needs o be ound as wi hou he need
o answe s o mul iple planning que ies.
I s a s wi h an ini ial node (e.g., he ini ial con igu a ion o he obo ) and is expanded in
a ee om his ini ial poin in o unexplo ed egions o he con igu a ion space. A each
s ep, a andom poin in he space is selec ed and he ee is ex ended om he nea es
node in he di ec ion o his poin , usually wi h a ixed s ep leng h and ensu ing ha he
join does no encoun e any obs acles along he line.
In he ollowing igu e can be seen ha om he ini ial ee (wi h black nodes) a new
sample is gi en [𝑞𝑟𝑎𝑛𝑑]. Then he nea es node o he ee is ound [𝑞𝑛𝑒𝑎𝑟] and a line will
be c ea ed connec ing bo h nodes. Using a p ede ined s ep size, he new node is
es ablished, and hen he collision on he node o in he connec ion line is checked.
Figu e 6. I e a ion example using RRT algo i hm. Sou ce: [28].
This second me hod also has a ia ions ha seek o imp o e i s quali ies. The mos
widely used is he RRT-Connec , which aims o un he explo a ion in a bidi ec ional way,
i.e. making s eps om he s a and end poin s, ying o connec bo h ees using he
minimum possible s eps [29].
Sample-based me hods a e he mos used algo i hms in obo ics due o hei abili y o
handle e icien ly in high-dimensional en i onmen s, such as obo s wi h many deg ees
o eedom.
3.1.5. Heu is ic-based algo i hms
Heu is ic me hods ha e p o ided a di e en app oach o pa h planning, o e ing solu ions
o complex p oblems using echniques inspi ed by biological and/o neu al p ocesses.
Among hese, Neu al Ne wo ks (NN), Gene ic Algo i hms (GA) and Pa icle Swa m
Op imiza ion (PSO) s and ou .
Neu al Ne wo ks a e compu a ional models ha simula e he way he human b ain
p ocesses in o ma ion. They use laye s o in e connec ed nodes o ‘neu ons’ o lea n, in
a i s aining s age, and hen o make p edic ions abou he inpu da a.
19
Focusing on pa h planning, NN allows obo s o lea n om pas plannings and imp o e
hei pe o mance o e ime. Al hough hey a e able o p ocess da a om he obo 's
senso s, o example, o a oid obs acles. This algo i hm is ypically used in conjunc ion
wi h o he planne s o ind he co ec pa h, especially he i s ew imes.
The neu al ne wo k is able o lea n mo e quickly o achie e esul s ha a e in line wi h
he global op imum. Then he g ea es ad an age is he abili y o handle complex and
‘noisy’ in o ma ion, bu in e u n hey equi e la ge amoun s o da a o aining.
In con as wi h neu al ne wo ks, Gene ic Algo i hms a e inspi ed by na u al selec ion
laws o pe o m sea ches and op imiza ions. They ope a e on a ‘popula ion’ o candida e
solu ions and apply gene ic ope a o s such as selec ion, c osso e and mu a ion o
e ol e be e solu ions.
Like NNs, GAs a e ideal o inding op imal ajec o ies in complex and dynamic
en i onmen s, as hey can adjus o changes in he obo 's goals. Fo his eason, hey
a e o en used o mobile obo explo a ion o la ge-complex spaces. Like mos i e a i e
me hods, he majo isk is ha hey con e ge o sub-op imal local solu ions.
Finally, PSO is an op imiza ion echnique based on simula ing he social beha io o
swa ms, such as bi ds o ish. I uses a se o indi iduals o ‘pa icles’, each ep esen ing
a momen a y solu ion, which adjus hei posi ion in he solu ion space based on hei
neighbou s o hei own expe ience.
Basically, a de ined numbe o pa icles a e ini ialized in space and andomly sca e ed
a ound he obo . Each solu ion is e alua ed agains an objec i e unc ion (which is
wan ed o op imize), and he bes solu ion is chosen. I a pa icle's cu en posi ion is
be e han i s bes posi ion [𝑝𝑏𝑒𝑠𝑡], hen i s 𝑝𝑏𝑒𝑠𝑡 is upda ed o his new posi ion. I , in
addi ion, he pa icle's cu en posi ion is be e han he cu en global bes one [𝑔𝑏𝑒𝑠𝑡],
hen he 𝑔𝑏𝑒𝑠𝑡 is upda ed.
The speed upda e o mula can be desc ibed as ollows:
𝑣𝑛𝑒𝑤 = 𝑤 · 𝑣𝑜𝑙𝑑 + 𝑐1· 𝑟1·(𝑝𝑏𝑒𝑠𝑡 − 𝑥𝑐𝑢𝑟𝑟𝑒𝑛𝑡)+ 𝑐2· 𝑟2· (𝑔𝑏𝑒𝑠𝑡 − 𝑥𝑐𝑢𝑟𝑟𝑒𝑛𝑡)
[1]
Whe e 𝑤 is he ine ia ac o , 𝑐1 and 𝑐2 a e lea ning coe icien s, 𝑟1 and 𝑟2 a e andom
numbe s be ween 0 and 1, and 𝑥𝑐𝑢𝑟𝑟𝑒𝑛𝑡 is he cu en posi ion o he pa icle. Then he
posi ion is upda ed by simply adding he new eloci y o he cu en posi ion:
𝑥𝑛𝑒𝑤 = 𝑥𝑐𝑢𝑟𝑟𝑒𝑛𝑡 + 𝑣𝑛𝑒𝑤
[2]
This p ocess is epea ed un il a e mina ion c i e ion is me , such as a maximum numbe
o i e a ions o su icien con e gence o an op imal solu ion.
20
PSO is a widely used me hod in mobile obo s because i can quickly ind accep able
solu ions, which is use ul in p oblems ha need eal- ime solu ions. Bu i can con e ge
oo quickly o subop imal solu ions, especially in p oblems wi h many dimensions o
complex sea ch spaces.
Apa om he me hods explained abo e, he e a e many mo e, as well as a ia ions o
mixes o hose al eady men ioned. These include F on ie -based Explo a ion, Gene ic
Algo i hms, algo i hms based on Ma ko decision p ocesses, and Deep Rein o cemen
Lea ning me hods. [30]
21
3.2. Robo ic Ope a ing Sys em (ROS)
Robo Ope a ing Sys em (ROS)
1
is an open-sou ce middlewa e o obo ic sys ems ha
p o ides a collec ion o ools, lib a ies and con en ions ha aim o simpli y he ask o
simula ing and con olling a obo .
I was c ea ed in 2007 in a collabo a ion be ween S and o d A i icial In elligence
Labo a o y and Willow Ga age. Since hen, i has e ol ed in o a s anda d o use in
indus y and academic en i onmen s o de elop obo ic applica ions, and is now
main ained by Open Robo ics.
I wo ks as a modula so wa e amewo k based on nodes ha pe o m speci ic asks
and communica e wi h each o he o ansmi in o ma ion and coo dina e o e en ually
achie e mo e complex ac ions.
In he i s e sion o ROS, nodes we e managed h ough a cen al node known as he
mas e , which o ganizes and acili a es he in e ac ion be ween he o he nodes by
egis e ing names and es ablishing he necessa y connec ions. I also ac ed as a
pa ame e se e o cen alize con igu a ion and da a managemen , allowing use s o
adjus he beha iou o he nodes du ing execu ion.
In he second e sion o ROS (ROS 2, which will be used du ing his p ojec ) his
changes. The e is no longe a mas e node, so each node can connec di ec ly o any
node. This way he da a exchange is mo e di ec and allows o c ea e a ully dis ibu ed
sys em. Howe e , i should be no ed ha , wi h his change, pa ame e s ha could
p e iously be global, now only exis i he node ha c ea es hem is ac i e.
Communica ion be ween nodes is achie ed mainly h ough wo me hods: opics and
se ices. Topics allows he publica ion and subsc ip ion o messages, acili a ing he
exchange o in o ma ion in a con inuous and decen alized way. Each message
published in a opic is accessible o all subsc ibed nodes, allowing o e icien , eal- ime
communica ion.
On he o he hand, he e a e also se ices, which use a eques - esponse model, whe e
a node can eques da a o an ac ion om ano he node and wai o a esponse. This is
c ucial o ope a ions ha equi e con i ma ion o he execu ion o a speci ic ac ion o
da a deli e y.
The modula and decen alized design is a g ea ad an age as i makes i ex emely
e sa ile. Added o i s exis ing adap abili y o in eg a e a ious ypes o ha dwa e and
1
h ps://docs. os.o g

22
so wa e, i is one o he mos widely used open-sou ces o plan, con ol and simula ion
in he ield o obo ics.
3.2.1. Topics
As in oduced abo e, opics a e one o he keys ones o communica ion be ween he
di e en nodes in a obo ic sys em. A opic ac s as a channel ha allows nodes o
exchange messages, making use o publishe and subsc ibe nodes, which acili a es
decoupled, unidi ec ional and e icien communica ion.
Each opic has a message wi h a con en o be ansmi ed, and whe e he ype o
message mus be de ined o se he s uc u e o i s da a. The opic is published by one
o mo e nodes o he publishe ype, and is ecei ed by one o mo e nodes o he
subsc ibe ype, as shown in he ollowing igu e:
Figu e 7. ROS publishe -subsc ibe communica ion scheme. Sou ce: [31]
To illus a e how o wo k wi h opics in ROS 2, a simple example has been made whe e
he i s node publishes in o ma ion abou he speed o a obo and he second node
subsc ibes o his in o ma ion in o de o egis e i and ac acco dingly:
Code 1. Node wi h a publishe called Veloci yPublishe ha publish s ings- ype messages.
// Include he lib ai es needed
#include " clcpp/ clcpp.hpp"
#include "s d_msgs/msg/s ing.hpp"
#include <s ing>
//C ea ion o a class called Veloci yPublishe ha is inhe i ed om clcpp::Node
class Veloci yPublishe : public clcpp::Node {
// C ea e a Publishe called “publishe _” ha will publish S ing messages.
clcpp::Publishe <s d_msgs::msg::S ing>::Sha edP publishe _;
public:
// Ini ialize he publishe wi h he opic name " obo _ eloci y" and a queue size
o 10.
Veloci yPublishe ():Node(" eloci y_publishe "){
publishe _ = c ea e_publishe <s d_msgs::msg::S ing>(" obo _ eloci y", 10);
}
oid publish_message(){
clcpp::Ra e a e(5); // C ea e a Ra e objec o keep he loop a 5 Hz
while ( clcpp::ok()) { // Loop as long as ROS is unning.
au o message = s d_msgs::msg::S ing(); // C ea e a S ing message.
message.da a = "Veloci y: 1.0 m/s"; // W i e he message con en .
publishe _->publish(message); // Publish he message
23
a e.sleep(); // Sleep o main ain he loop a e
}
}
};
// Main unc ion ha ini ializes he node and keeps i unning.
in main(in a gc, cha * a g []){
clcpp::ini (a gc, a g ); // Ini ialize ROS 2.
clcpp::spin(s d::make_sha ed<Veloci yPublishe >()); // C ea e and spin he
node.
clcpp::shu down(); // Shu down ROS 2 when he node s ops spinning.
e u n 0;
}
As can be seen, i s ly is needed o c ea e a class o con ain all he code. Then as many
publishe s as necessa y mus be c ea ed and ini ialized, which will con ain he message
as long as he ype o message con igu a ion is espec ed. I should be no ed ha he
publica ion biased by a a e has been used as an example, bu i is no he only way o
pe iodize he publica ion o messages.
On he o he side o he connec ion, i is he subsc ibe node will c ea e a subsc ibe
objec o ead hese messages and ca y ou he necessa y ac ions.
Code 2. Node wi h a subsc ibe ha eads and isualizes in he e minal he messages published by he
publishe Veloci yPublishe o Code 1.
// Include he lib ai es needed
#include " clcpp/ clcpp.hpp"
#include "s d_msgs/msg/s ing.hpp"
#include <s ing>
// C ea e a class called Veloci ySubsc ibe ha is inhe i ed om clcpp::Node
class Veloci ySubsc ibe : public clcpp::Node {
// C ea e a Subsc ibe called “subsc ip ion_” ha ecei es S ing messages.
clcpp::Subsc ip ion<s d_msgs::msg::S ing>::Sha edP subsc ip ion_;
// Callback unc ion ha will be called when a message is ecei ed on he
" obo _ eloci y" opic.
oid opic_callback(cons s d_msgs::msg::S ing::Sha edP msg) {
RCLCPP_INFO(ge _logge (), "I hea d: '%s'", msg.da a.c_s ()); //P in he
message on he e minal.
}
public:
// Ini ialize he subsc ibe wi h he opic name " obo _ eloci y".
Veloci ySubsc ibe () : Node(" eloci y_subsc ibe ") {
subsc ip ion_ = c ea e_subsc ip ion<s d_msgs::msg::S ing>(
" obo _ eloci y", 10,
s d::bind(&Veloci ySubsc ibe :: opic_callback, his,
s d::placeholde s::_1));
}
};
// Main unc ion ha ini ializes he node and keeps i unning.
in main(in a gc, cha * a g []) {
clcpp::ini (a gc, a g ); // Ini ialize ROS 2.
clcpp::spin(s d::make_sha ed<Veloci ySubsc ibe >()); // C ea e and spin he
node.
clcpp::shu down(); // Shu down ROS 2 when he node s ops spinning.
e u n 0;
}
24
Fo he subsc ibe , i is con enien o c ea e a callback unc ion ha will be execu ed
e e y ime he publishe sends a new message. In his case, he ac ion ha he
subsc ibe pe o ms when i ecei es a new message is o p in i on he e minal sc een.
In his way, i has been exempli ied how a unidi ec ional connec ion can be achie ed
be ween a publishe node and a subsc ibe node, o send messages.
3.2.2. Se ices
Ano he way o es ablish communica ion be ween wo nodes is h ough he use o
se ices, which o e bidi ec ional communica ion be ween hem, based on a eques -
esponse model. As opposed o opics, se ices a e ideal o in e ac ing when an
immedia e esponse is equi ed om ano he node. This makes hem sui able o
ope a ions ha canno ole a e he asynch onous na u e o opics.
Se ices a e de ined by wo pa ies: he se e and he clien . The se e p o ides he
se ice, i.e. i is p epa ed o p ocess eques s and send esponses. Clien s, on he o he
hand, a e he ones who makes he eques o he se ice and expec s o ecei e a
esponse. As wi h opics, each se ice has a message ype ha de ines he s uc u e o
he eques and he esponse.
Figu e 8. ROS se e -clien communica ion scheme. Sou ce: [31].
Below a e wo pieces o code, one om he se e and one om he clien , o exempli y
he eques - esponse communica ion. Fo simplici y, le 's assume ha a se ice called
Ge Poin .s ha e been c ea ed o indica e he ype o Reques and esponse message
mus be used:
Code 3. Se ice eques and esponse de ini ion.
# Reques
loa 64 x
loa 64 y
loa 64 z
---
# Response
loa 64 x
loa 64 y
loa 64 z
25
Wi h his ype o message, he ollowing se e , a e ecei ing he eques , akes he
cu en posi ion alue and adds andom alues o he X and Y, and hen sends he
esponse:
Code 4. Node wi h a se ice se e called ge _posi ion ha ecei es a eques and gene a es a esponse.
// Include he lib a ies needed
#include " clcpp/ clcpp.hpp"
#include "geome y_msgs/msg/Poin .hpp"
#include "example_ m/s /ge _poin .hpp" // se ice de ini ion
#include <cs dlib> // Fo and() and s and()
// Se ice callback unc ion o handle incoming eques s and send esponses
oid handle_posi ion(cons
s d::sha ed_p <example_in e aces::s ::Ge Poin ::Reques > eques ,
s d::sha ed_p <example_in e aces::s ::Ge Poin ::Response> esponse){
in and_addi ion = and() % 10
esponse->x = eques ->x + and_addi ion; // Add andom numbe o x
esponse->y = eques ->y + and_addi ion; // Add andom numbe o y
esponse->z = eques ->z; // z emains unchanged
}
// Main unc ion ha ini ializes he node and keeps i unning.
in main(in a gc, cha * a g []) {
clcpp::ini (a gc, a g );
// C ea e a ROS node
s d::sha ed_p < clcpp::Node> node = clcpp::Node::make_sha ed("posi ion_se ");
// C ea e a se ice named 'ge _posi ion' and de ine he callback handle
clcpp::Se ice<example_in e aces::s ::Ge Poin >::Sha edP se ice = node->
c ea e_se ice<example_in e aces::s ::Ge Poin >("ge _posi ion", handle_posi ion);
clcpp::spin(node);
clcpp::shu down();
e u n 0;
}
On he o he hand, he clien only sends a eques and wai s o he esponse which, in
his case, will be displayed on he e minal sc een. I i does no ecei e a esponse, an
e o message will be displayed:
Code 5. Node wi h a se ice clien ha sends a eques and wai s o he esponse o execu e he p ope
ac ion.
// Include he lib a ies needed
#include " clcpp/ clcpp.hpp"
#include " example_ m/s /ge _poin .hpp " // se ice de ini ion
#include <ch ono>
#include <cs dlib>
#include <memo y>
using namespace s d::ch ono_li e als;
in main(in a gc, cha * a g []) {
clcpp::ini (a gc, a g ); // Ini ialize ROS 2
// C ea e a ROS node
s d::sha ed_p < clcpp::Node> node = clcpp::Node::make_sha ed("Ge Pos_clien ");
32
Figu e 14. Collision ma ix o he UR3 wi h a 64000 o sampling densi y.
C ea ing his ma ix helps o educe he mo ion planning ime by disabling collision
checking o pai s o links known o be sa e. This is pa icula ly use ul in obo s wi h
mul iple links and join s. In his case, he only con igu a ion ha can be adjus ed in his
panel is he sampling densi y, which de e mines how many andom posi ions o he obo
a e checked o sel -collision.
b) Vi ual Join s
Vi ual join s a e p ima ily used o connec obo s o he wo ld, which is necessa y o
clea ly de ine he ela ionship be ween he obo and i s en i onmen .
In he case o he UR3, he i ual join 's name mus be de ined, he child link, which
would be he base link de ined a he beginning o his chap e , i s pa en ame, in his
case, he p e-de ined wo ld in he en i onmen , and inally, he ype o join , which is
ixed.
This would be he s anda d connec ion be ween a obo ic a m and he en i onmen . I
wo king wi h mobile obo s, he ype o join would need o be plana , allowing i o mo e
ac oss he a ailable a ea.
c) Planning g oups
A his poin , i is impo an o be clea abou how many con olle s will be de ined and,
he e o e, how many independen pa s he obo has.
I wo king wi h a ixed manipula o obo , i is common o sepa a e he join s ha de ine
he a m om hose used o he end e ec o . I i is a mobile obo , only he mo o ized

33
wheels would be de ined, and i i is a mobile manipula o , i would be he sum o bo h
cases sepa a ely.
S a ing wi h he a m g oup, i s de ine he name, he kinema ic sol e and i s
cha ac e is ics, and he planne ha wan o be used by de aul . In his case, he chosen
kinema ic sol e is KDL, which e icien ly handles complex kinema ic chains, and he
planne is RRT Connec , a e y eliable and as sampling planne .
Finally, selec he links ha will belong o ha g oup, choosing he en i e chain om he
p e iously de ined i ual join o he las link o he a m (link 6), excluding he end
e ec o . The p ocedu e o he end e ec o is iden ical, bu his ime he kinema ic sol e
can be despised since he mo emen s o he g ippe a e mechanically cons ained and
only mo e along one axis.
d) Robo poses
As he name sugges s, his sec ion will es ablish p ede ined poses o he obo . Any
numbe o poses can be se , bu he mos common ones a e o de ine he ini ial posi ion
o he a m g oup and he open and closed s a es o he g ippe .
To do his, selec he g oup being wo ked on (a m o g ippe ) and he posi ions o each
join in ha g oup, ei he by using he slide s o by di ec ly en e ing he alue:
Figu e 15. Ejemplo de como de ini la posicione inicial del b azo del obo UR3.
34
e) End e ec o
This pa is impo an because i de ines he ela ionship be ween he g ippe g oup and
he link o he a m g oup o which i is connec ed. Simply gi e a name o he new join ,
choose he g oup ela ed o he g ippe , and selec he pa en link o pa en g oup i i
has one. Ob iously, his sec ion is only necessa y o obo s wi h end e ec o s and is no
a manda o y ield o o he cases.
) ROS2_con ol URDF modi ica ions
This is a sec ion in eg a ed in o he second e sion o Mo eI , o adap o ROS 2. I
essen ially helps o modi y he URDF; in ac , i c ea es ano he one om he o iginal o
wo k wi h he os2_con ol package.
By de aul , he Mo eI Se up Assis an assumes a posi ion command in e ace and
posi ion and eloci y s a e in e aces, which ha e been used o he UR3. This adap a ion
is bene icial o enabling eal- ime con ol o he join s. In his way, i ensu es ha he
obo can espond quickly o commands and p o ide p ecise eedback on i s s a e.
g) ROS 2 Con olle s and Mo eI Con olle s
These wo sec ions will be explained oge he since he p ocedu e is iden ical o adding
bo h ROS 2 con ols and Mo eI con ols, al hough hey a e con igu ed sepa a ely in he
assis an . The di e ence is ha ROS 2 con ols a e used o he middlewa e's own ools,
such as eal- ime obo con ol, while Mo eI con ols a e used o gene a e he obo 's
mo emen con ols and send hem o he ROS con olle o execu ion.
Figu e 16. ROS 2 con olle s’ con igu a ion o he UR3.
35
In bo h cases, i is common o c ea e a con olle o each g oup, speci ying he name,
he ype o con olle (Follow Join T ajec o y o he a m link chains and G ippe
Command o he g ippe s), and he g oup i will con ol.
h) Pe cep ion
The pe cep ion sec ion is dedica ed o he con igu a ion o 3D senso s used by he obo .
I is only necessa y o choose he ype o senso , he opic olde whe e i s con igu a ion
is s o ed, he speci ica ions o be used ( ange, o se , a e, e c.), and i will c ea e he
adap ed con igu a ion o use in Mo eI in a sepa a e YAML ile. Fo he UR3, such
senso s we e no needed.
i) Launch iles, Au ho and Con igu a ion Files
The las sec ions a e mo e gene ic. Fi s , choose he launch iles o be c ea ed in he
launch olde (by de aul , all a e c ea ed). In he nex sec ion, speci y he au ho 's name
and email, and in he las sec ion, selec he olde whe e he assis an will sa e he
gene a ed olde .
Once all con igu a ions ha e been de ined as desi ed, simply click he Gene a e Package
bu on o execu e he Se up Assis an .
The esul is a package wi h a launch olde con aining all he iles de ined in he
p e iously men ioned sec ion, a Con ig sub olde wi h all he necessa y YAML iles o
he obo 's de ini ion and con igu a ion, he CMakeLis s ile, and he package.xml ile o
compile he package.
4.2. Launching a obo wi h Mo eI
Wi h he obo model al eady in eg a ed in o he sys em, i is ime o launch i o isualize
i in RViz and in e ac wi h Mo eI 's GUI. Howe e , se e al nodes need o be launched
o e e y hing o wo k p ope ly and o use all he unc ionali ies he p og am o e s.
Fo his, i is necessa y o unde s and wha Mo eI equi es and how i s nodes
communica e wi h each o he , wi h RViz, and wi h ROS.
4.2.1. Mo e g oup node
The mo e_g oup node is he cen al componen o Mo eI , ac ing as an in eg a o o all
he necessa y subsys ems o mo ion planning and execu ion. This node p o ides ROS
se ices and ac ions ha allow use s o in e ac wi h he obo o manipula ion and
na iga ion asks.
36
I mainly handles g ouping se e al essen ial componen s such as mo ion planning,
collision de ec ion, and ajec o y execu ion. Use s can in e ac wi h i h ough C++ code
(using he mo e_g oup_in e ace node), Py hon (wi h he Mo eI _commande node), o
ia he GUI using he mo ion planning plugin in RViz.
Figu e 17. Gene al scheme o he connec ion be ween Mo eI nodes. Sou ce: [35].
Once use eques s a e ecei ed, he mo e_g oup node communica es wi h he es o
he nodes o execu e he necessa y ac ions. Among he mos impo an nodes a e:
- Planning Scene: Main ains a ep esen a ion o he en i onmen and he obo 's
s a e, including objec s ha may be a ached o i . This is managed h ough he
Planning Scene Moni o .
- Planning Pipeline: P o ides he in as uc u e o mo ion planning. I i s eads
he cu en s a e o he obo h ough he /join _s a es opic o ROS and
communica es wi h he Planning In e ace sub-node.
37
- Planning In e ace: Uses ex e nal planne packages o c ea e pa hs.
- Collision De ec ion: Ensu es ha he ajec o ies do no esul in collisions.
- T ajec o y Execu ion Manage : Responsible o execu ing he planned
ajec o ies using he obo con olle s c ea ed ea lie .
Howe e , al hough all hese nodes a e c ucial o he obo 's mo emen , hey need o
communica e wi h he obo _desc ip ion pa ame e o unde s and i s p ope ies. These
p ope ies a e de ined in he URDF and he Seman ic Robo Desc ip ion Fo ma (SRDF).
The URDF, as men ioned ea lie , desc ibes he obo 's physical model, while he SRDF
speci ies he join g oups (a m and hand), p ede ined con igu a ions o he obo (ini ial,
hand open, and hand close), addi ional collision checking in o ma ion (collision ma ix),
and p ope ies such as he end e ec o .
Wi h his unde s anding, he con igu a ion and launch o he main node can be de ined
as ollows:
Code 9. Con igu a ion o he mo e_g oup node.
Mo eI _con ig = (
Mo eI Con igsBuilde ( obo _name="u 3_ obo ", package_name="u 3_de ")
. obo _desc ip ion( ile_pa h="con ig/u 3_ obo .u d .xac o")
. obo _desc ip ion_seman ic( ile_pa h="con ig/u 3_ obo .s d ")
.planning_scene_moni o (publish_ obo _desc ip ion=T ue,
publish_ obo _desc ip ion_seman ic=T ue)
. ajec o y_execu ion( ile_pa h="con ig/Mo eI _con olle s.yaml")
.planning_pipelines(pipelines=["ompl", "chomp",
"pilz_indus ial_mo ion_planne ", "s omp"])
. o_Mo eI _con igs()
)
Code 10. C ea ion and ini ializa ion o he mo e_g oup node.
mo e_g oup_node = Node(
package="Mo eI _ os_mo e_g oup",
execu able="mo e_g oup",
ou pu ="sc een",
pa ame e s=[Mo eI _con ig. o_dic ()],
a gumen s=["-- os-a gs", "--log-le el", "in o"],
)
4.2.2. O he nodes
As men ioned ea lie , he mo e_g oup is no he only node ha needs o be launched,
as he e a e o he essen ial nodes o he p ope unc ioning o he sys em. The mos
impo an ones a e:
- RViz node: Launches RViz wi h he desi ed con igu a ion o isualize he obo
and i s planning en i onmen s.

38
- S a ic node: Publishes a s a ic ans o ma ion be ween he wo ld and base_link
e e ence ames. This is necessa y o p o ide a e e ence o he obo 's posi ion
in he global space.
- Robo S a e Publishe : Publishes he obo 's s a e, such as he posi ion o he
join s and he end e ec o , o he ROS nodes. As a node, i allows he es o he
sys em's nodes o know he obo 's cu en con igu a ion a any ime.
- Join S a e B oadcas e : Reads all s a e in e aces and publishes hem on he
/join _s a es opic. I is used o send he s a e o speci ic join s be ween nodes.
- ROS 2 Con ol Node: Runs he ROS 2 con olle manage . I essen ially uses he
con olle s c ea ed wi h he Se up Assis an o con ol he obo in eal ime. Recall
ha Mo eI 's con olle s a e used o c ea e ajec o ies, bu ROS con olle s a e
necessa y o execu e hem.
- UR3 A m/Hand Con olle Spawne : These wo nodes a e used o launch he
spawne s o he a m and hand con olle s o he obo , which a e go e ned by
he YAML iles c ea ed in he Con ig olde .
- TF Node: This is no launched di ec ly in his code because i is a ool in eg a ed
in o ROS, bu i is used du ing he applica ion’s ope a ion. I moni o s
ans o ma ion in o ma ion using he TF (T ans o m) lib a y o ROS, which
calcula es he ans o ma ions be ween join s o ob ain he obo 's global pose a
all imes. Wi hou his node, he o he nodes could no calcula e he obo 's pose
ei he .
Once all hese nodes a e c ea ed in a launch ile (see sec ion 4 o he annexes), i should
be compiled and hen execu ed.
4.3. Mo eI GUI in RViz
As soon as he nodes s a o launch, many logs wi h in o ma ion will appea in he
e minal whe e he command was execu ed. I is impo an o wa ch o any Wa ning o
E o logs. I e e y hing launches co ec ly, a message om he Mo e G oup will appea
s a ing, "You can s a planning now."
In he RViz applica ion, he obo may no be isible ye . This is because he Mo ion
Planning ab has no been added o he panel. To add i , i is needed o click he Add
bu on in he Displays window and sea ch o he Mo ion Planning display in he
Mo eI _ os_ isualiza ion olde .
Once added, he obo will become isible in he main window. I i is s ill no isible, he
Robo Desc ip ion and Planning Scene Topic sec ions should be upda ed o he
obo _desc ip ion pa ame e and he /moni o ed_planning_scene opic, espec i ely.
Ano he impo an ini ial con igu a ion is o se he Fixed F ame o wo ld, as i has been
con igu ed his way. I hese con igu a ions a e co ec , i is ad isable o sa e hem so
hey do no ha e o be edone each ime he launch ile is execu ed.
39
When he obo and he Mo ion Planning panel a e isible, ajec o ies can be c ea ed.
To do his, selec he planning g oup o he a m in he planning sec ion o he Mo ion
Planning panel and choose he andom op ion in Goal S a e. Nex , click he Plan bu on,
and i he Execu e bu on ac i a es, click i . This way, he obo will c ea e a ajec o y
om he ini ial s a e o a andom one and show he esul .
The same can be done wi h he hand by using he "hand" planning g oup. In his case, i
can only mo e om open o close.
This GUI p o ides se e al o e lay isualiza ions o acili a e in e ac ion and mo ion
planning. These isualiza ions include he cu en s a e o he obo in he planning
en i onmen , he planned ajec o y, he ini ial s a e, and he goal s a e o mo ion
planning.
I a mo e in e ac i e me hod o planning is p e e ed, he Que y Goal S a e op ion in he
Planning Reques sec ion o he Displays can be enabled. A copy o he obo will appea
in o ange wi h a ligh blue sphe e a he end e ec o . By mo ing his sphe e o using he
colou ed axes a ound i , his shadow can be posi ioned in he desi ed loca ion. Then,
planning and execu ion can be done as be o e, o by clicking he Plan & Execu e bu on
o do bo h a once.
This allows he a ge posi ion o he obo o be selec ed, and he ound ajec o y can
be execu ed wi h jus one click.
Figu e 18. Mo eI GUI wi h he UR3 Que y Goal S a e in o ange, and he Cu en S a e in g ey.
In he Display window, many mo e con igu a ions appea . Fo example, jus as he Goal
S a e has been isualized, he S a S a e can also be isualized, which appea s igh
abo e i and is displayed as a g een shadow.
Ano he in e es ing op ion is o isualize he obo a di e en momen s o he calcula ed
ajec o y:
40
Figu e 19. Example o he ail o he planed pa h wi h he UR3 in he Mo eI GUI.
To achie e his, deselec he Show T ail op ion in he Planned Pa h sec ion o he
Displays window. Nex , mo e he obo o he desi ed posi ion, click he Plan bu on, and
hen check he Show T ail box. Changing he alues o he S ep Size will allow mo e o
ewe momen s o be isualized in he main window.
Focusing solely on he Mo ion Planning Display, commands and Que ies can be
con igu ed, bu he e a e also mo e op ions speci ic o he pa h. The mos impo an ones
a e:
- Planning Time: Maximum ime o sea ch o a solu ion.
- Planning A emp s: Numbe o a emp s o ind a pa h.
- Use Ca esian Pa h: The esul ing pa h will use only s aigh mo emen s be ween
he h ee axes.
- Collision-awa e IK: Enables o disables collision de ec ion.
- Replanning: Sa es samples om one plan o ano he , use ul i a solu ion is no
ound on he i s a emp .
- The lib a y wi h he di e en planne s is loca ed in he Con ex ab o he Mo ion
Planning Display. The e, di e en planne s con igu ed in he launch can be
selec ed.
Addi ionally, i he join s need o be mo ed one by one manually, i can be done in he
Join s ab. To c ea e objec s in he en i onmen , use he Scene Objec s ab.
These a e jus some o he unc ionali ies o e ed by his ool. The ad an age in ended
wi h his g aphical in e ace is o p o ide use s wi h a ool ha allows hem o isualize,
plan, and execu e obo mo emen s in a i ual en i onmen wi hou he need o pe o m
all hese ac ions h ough code.
41
4.4. Poin o poin mo emen
The ad an age o e ed by he GUI is no ewo hy, bu o pe o ming complex mo emen s
o mo emen s wi h mul iple s ages, i is essen ial o unde s and how o execu e simple
mo emen s.
As explained in chap e 3.2, he node ha con ols he obo is he Mo e G oup, bu he
node esponsible o managing mo ion planning is he Planning In e ace. This is he
node o which join posi ion commands mus be sen in o de o c ea e he pa h o each
he a ge posi ion.
The essen ial commands o his ask would be:
- C ea e an ins ance o he Mo e G oup In e ace ype:
Code 11. C ea ion o a Mo e G oup In e ace in a node o a mo emen wi h he UR3 obo .
using Mo eI ::planning_in e ace::Mo eG oupIn e ace;
au o mo e_g oup_in e ace = Mo eG oupIn e ace(node, "u 3_a m");
- Ge he goal pose: The use is p omp ed o en e he a ge posi ion and
o ien a ion o he obo . The en e ed alues a e s o ed in a Pose message, which
is hen se as he a ge in he Mo e G oup In e ace. The a ge pose is en e ed
in he g oup in e ace, and he planne and ole ance o be used a e con igu ed.
Code 12. Ask he use o a goal pose o he obo h ough he e minal.
au o cons a ge _pose = []{
geome y_msgs::msg::Pose msg;
s d::s ing inpu ;
s d::cou << "En e posi ion and o ien a ion (x y z qx qy qz qw): ";
s d::ge line(s d::cin, inpu );
s d::is ings eam ss(inpu );
ss >> msg.posi ion.x >> msg.posi ion.y >> msg.posi ion.z
>> msg.o ien a ion.x >> msg.o ien a ion.y >> msg.o ien a ion.z >>
msg.o ien a ion.w;
e u n msg;
}();
mo e_g oup_in e ace.se PoseTa ge ( a ge _pose);
// Con igu a ion o he planne
mo e_g oup_in e ace.se Planne Id("RRTConnec kCon igDe aul ");
mo e_g oup_in e ace.se GoalTole ance(0.05);
- De ini ion and execu ion o he plan: A unc ion is de ined ha c ea es a
Mo eG oupIn e ace::Plan message and adds he p ope ies o he c ea ed
mo e_g oup_in e ace. The unc ion e u ns a esul , which is execu ed i i is a
success o p in s an e o message o he e minal sc een o he wise.
48
Code 17. C ea ion o a s age named Cu en S a e.
au o s age_s a e_cu en = s d::make_unique<m c::s ages::Cu en S a e>("cu en ");
cu en _s a e_p = s age_s a e_cu en .ge ();
ask.add(s d::mo e(s age_s a e_cu en ));
Nex , se e al planne s a e de ined ha will be used in di e en s ages o he ask. The
Pipeline Planne is used o gene al mo ion planning, he Join In e pola ion Planne o
simple join in e pola ion mo emen s, and he Ca esian Pa h o linea mo emen s in
Ca esian space. Each planne has i s speci ic use and is selec ed acco ding o he ype
o mo emen equi ed in each s age. These planne s allow he obo o execu e complex
mo emen s in a mo e pe sonalized and p ecise manne .
Code 18. De ini ion o he planne s and i s p ope ies.
au o sampling_planne = s d::make_sha ed<m c::sol e s::PipelinePlanne >(node_);
au o in e pola ion_planne =
s d::make_sha ed<m c::sol e s::Join In e pola ionPlanne >();
au o ca esian_planne = s d::make_sha ed<m c::sol e s::Ca esianPa h>();
ca esian_planne -> se MaxVeloci yScalingFac o (1.0);
ca esian_planne -> se MaxAccele a ionScalingFac o (1.0);
ca esian_planne -> se S epSize(.01);
S a ing wi h he ac ions o mo ing he obo 's a m, he i s s ep is o mo e i o he p e-
posi ion whe e he objec is o be picked up. Fo his, he "Mo e o pick" s age is c ea ed,
which will use he Pipeline planne o a ee mo emen .
This is he s anda d way o c ea ing a s age, whe e pa ame e s such as wai ime and
o he necessa y p ope ies o his mo emen mus be con igu ed.
Code 19. C ea ion o a connec s age.
//Mo e o pick
au o s age_mo e_ o_pick = s d::make_unique<m c::s ages::Connec >(
"mo e o pick",
m c::s ages::Connec ::G oupPlanne Vec o {{a m_g oup_name, sampling_planne }
});
s age_mo e_ o_pick->se Timeou (10.0);
s age_mo e_ o_pick->p ope ies().con igu eIni F om(m c::S age::PARENT);
ask.add(s d::mo e(s age_mo e_ o_pick));
Ano he impo an elemen is he Se ial Con aine , which is c ea ed o g oup all he
s ages ela ed o he picking ac ion. A e de ining i , he impo an p ope ies o he main
ask objec a e exposed o he con aine , allowing he in e nal s ages o access and use
hese pa ame e s cohe en ly.
Finally, he pe inen s ages mus be c ea ed wi hin he con aine , which g oups hem
wi h he keyboa d keys "{}". Fo example, in he ollowing code snippe , he "app oach o
objec " s age o he sequence is de ined. Once he s age is de ined, he b aces a e

49
closed, and when he con aine is comple e, i should be added o he ask wi h he .add
command.
Code 20. De ini ion o a se ial con aine in MTC.
//Se ial con aine o c ea e he pick mo emen
{
au o g asp = s d::make_unique<m c::Se ialCon aine >("pick objec ");
ask.p ope ies().exposeTo(g asp->p ope ies(), {"ee ", "g oup", "ik_ ame"});
g asp->p ope ies().con igu eIni F om(m c::S age::PARENT,
{ "ee ", "g oup", "ik_ ame" });
//App oach o he objec
{
au o s age =
s d::make_unique<m c::s ages::Mo eRela i e>("app oach objec ",
ca esian_planne );
s age -> p ope ies().se ("ma ke _ns", "app oach_objec ");
s age -> p ope ies().se ("link", hand_ ame);
s age -> p ope ies().con igu eIni F om(m c::S age::PARENT, {"g oup"});
s age -> se MinMaxDis ance(0.0, 0.25);
// Se hand o wa d di ec ion
geome y_msgs::msg::Vec o 3S amped ec;
ec.heade . ame_id = hand_ ame;
ec. ec o .z = 1.0; // Dis ance o he objec ame
s age->se Di ec ion( ec);
g asp->inse (s d::mo e(s age));
}
}
ask.add(s d::mo e(g asp)); //Add he con aine o he ask
}
In his way, he ask, con aine s, and s ages a e c ea ed. Fo a mo e comple e example,
see Sec ion 5 o he annexes.
50
5. Mo eI applica ions o he Mas e
Conside ing he u ili ies p esen ed by using Mo eI , he ollowing sec ion will explo e i s
possible applica ions in he a ious subjec s o he comple ed mas e 's p og am.
The i s s ep would be o iden i y which subjec s include obo ics con en , whe he in
heo y o p ac ice, and ha a e based on he use o obo ic a ms. This is impo an
because planning he mo emen pa h o a mobile obo using Mo eI is a e y uncommon
ask due o he exis ence o o he lib a ies specialized in hese applica ions.
So, he subjec s whe e i makes he mos sense o use Mo eI would be:
- Robo ics, Kinema ics & Dynamics sys ems.
- In oduc ion o ROS.
- Planning and Implemen a ion o Robo ic Sys ems.
5.1. Applica ions o Robo ics, Kinema ics & Dynamics
Sys ems subjec
This subjec se es o lay he ounda ions o he necessa y knowledge o unde s and
how a obo wo ks a a gene al le el and wha s ages exis o gene a e mo emen s wi h
i . I co e s how o model (in b oad e ms) a obo , how o calcula e kinema ics and
in e se kinema ics, he s udy o singula i ies, he dynamics in ol ed in he obo 's
mo emen s, e c.
Among all his, he mos in e es ing pa om he pe spec i e o his wo k would be he
s a emen o he second p ac ical assignmen o he subjec . Basically, he objec i e o
his assignmen is o ensu e ha he obo is capable o execu ing a se ies o mo emen s
by sending commands h ough a joys ick. Wi h he joys ick, mo emen s in X and Y a e
con olled, and wi h he ea bu ons, mo emen s in he Z-axis ( e e enced o he end-
e ec o ) a e con olled.
Figu e 20. Joys ick mo emen s: in g een he X-axis, in ed he Y-axis
and he bu ons he Z-axis.
51
To achie e his in Mo eI , se e al p e equisi es need o be me . Apa om ha ing he
ROS 2 and Mo eI 2 packages ins alled, he ROS 2 Joy package is essen ial.
Once he package is compiled and unc ioning, i is necessa y o connec he joys ick
and e i y ha ROS 2 de ec s i co ec ly. To do his, he joy_node mus be execu ed,
which cap u es he joys ick inpu s and publishes hem as pa ame e s, o see he esul s
in he e minal he ollowing command has o be execu ed:
Code 21. Commands o un he joy_node and de ec mo emen s om he joys ick.
os2 un joy joy_node
os2 opic echo /joy
The o he impo an node is he JoyToSe oPub, which is esponsible o ansla ing
joys ick inpu s in o Mo eI mo ion commands. Below is an example implemen a ion o
his node in C++:
Code 22. Example o a JoyToSe o node.
#include <geome y_msgs/msg/ wis _s amped.hpp>
#include <con ol_msgs/msg/join _jog.hpp>
#include <senso _msgs/msg/joy.hpp>
#include < clcpp/ clcpp.hpp>
class JoyToSe oPub : public clcpp::Node {
public:
JoyToSe oPub() : Node("joy_ o_se o_pub") {
joy_sub_ = his->c ea e_subsc ip ion<senso _msgs::msg::Joy>(
"/joy", 10, s d::bind(&JoyToSe oPub::joy_callback, his,
s d::placeholde s::_1));
wis _pub_ = his->c ea e_publishe <geome y_msgs::msg::Twis S amped>
("/se o_se e /del a_ wis _cmds", 10);
}
p i a e:
oid joy_callback(cons senso _msgs::msg::Joy::Sha edP msg) {
au o wis = geome y_msgs::msg::Twis S amped();
wis .heade .s amp = his->now();
wis .heade . ame_id = "base_link";
wis . wis .linea .x = msg->axes[1];//Assigns he axis o he mo emen in X
wis . wis .linea .y = msg->axes[0]; //Assigns he axis o he mo emen in Y
wis _pub_->publish( wis );
}
clcpp::Subsc ip ion<senso _msgs::msg::Joy>::Sha edP joy_sub_;
clcpp::Publishe <geome y_msgs::msg::Twis S amped>::Sha edP wis _pub_;
};
in main(in a gc, cha **a g ) {
clcpp::ini (a gc, a g );
au o node = s d::make_sha ed<JoyToSe oPub>();
clcpp::spin(node);
clcpp::shu down();
e u n 0;}
52
A e e i ying ha he /joy opic exis s and ROS 2 is de ec ing i , and ha he mo ion
eading node is co ec , i is ime o launch he “se o” node o Mo eI . This launch s a s
he wo explained nodes and ini ia es he d i e s ha communica e wi h Mo eI 's node
sys em. Fo an example o his sc ip wi h he joy and JoyToSe oPub nodes, see
Sec ion 6 o he annexes.
Finally, he obo model mus also be launched as explained in p e ious sec ions. Once
e e y hing is up and unning, mo ing he joys ick will demons a e how he UR3 esponds
o he commands.
5.2. Applica ions o In oduc ion o ROS subjec
The second subjec wi h con en compa ible wi h he Mo eI lib a y is In oduc ion o
ROS. This subjec aims o each he basics o p og amming in he ROS middlewa e.
I p ima ily ocuses on lea ning communica ion be ween nodes, ei he h ough opics
(subsc ibe s and publishe s) o h ough se ices (se ices and clien s). Addi ionally, i
co e s simula ing senso s using Gazebo
4
, which is a powe ul obo ic simula o , and also
communica ion using ac ions o obo con ol is explained.
To pu all his in o p ac ice, a inal p ojec is assigned wi h he goal o making he obo
capable o playing chess agains he use . Fo his, he labo a o y obo s (UR3 models)
and a boa d wi h A Ucos
5
a e used.
A Uco ma ke s a e a ype o ma ke (simila o he well-known QR codes) used in
compu e ision and obo ics o objec
de ec ion and acking. These ma ke s a e
squa e pa e ns wi h a unique iden i ie ha can
be accu a ely ecognized by came as and
specialized image p ocessing so wa e.
To make e e y hing wo k, a dep h came a is
placed on one side o he boa d, and bo h he
boa d and he pieces ha e an A Uco on op o
loca e hem in he en i onmen .
To simula e all his, he subjec p o ides he
en i e se up al eady in eg a ed in o RViz ( o
isualiza ion) and Gazebo ( o simula ing
came a images and he physical p ope ies o
he sys em).
4
h ps://gazebosim.o g/home
5
h ps://index. os.o g/p/A Uco/
Figu e 21. Chesslab package ini ial se up.
53
Addi ionally, a se ies o p e-p og ammed se ices a e p o ided o make he ask easie
o s uden s, such as:
- Rese Se ice: o ese he scene.
- Robo Con ig Se ice: o mo e he obo o a gi en con igu a ion.
- In e se Kinema ics (IK) Se ice: o calcula e he IK o he obo ’s end-e ec o
wi h espec o (w. . .) he base and mo e i o one o he esul ing solu ions.
- A ach Se ice: o link a piece wi h he obo ’s end-e ec o .
Wi h all his ma e ial, he main asks o he p ojec a e h ee:
- P ocessing he came a in o ma ion o loca e he A Ucos: he came a mus be
con igu ed o posi ion i ela i e o he main coo dina e axis o he sys em ( he
wo ld). The inpu pa ame e s (which come in as opics) mus be con e ed o
ans o ms ( ), and he necessa y s mus hen be calcula ed o loca e each
A Uco de ec ed by he came a w. . . he wo ld.
- Designing he necessa y ac ions: conside ing ha he obo mus make simple
mo emen s o he pieces and also pe o m "kill" mo emen s.
- P og amming he allowed mo emen s: in addi ion o p og amming an in e ac ion
wi h he use , he obo mus be able o know which mo emen s a e allowed o
each piece ( o example, he bishop can only mo e diagonally).
The e o e, he inal esul can be summa ized in ou main nodes: he Chess Game
Node, which commands all he game; he Sensing Node, ha p o ides he posi ion o
he A Ucos w. . he wo ld; he Task and Mo ion Planning Node, which c ea es he
ajec o ies and he Execu ion Node, which execu es hem. So, he communica ion
be ween all hese nodes can be seen in he ollowing scheme:
Figu e 22. Node diag am o he chesslab en i onmen o he inal p ojec o In oduc ion o ROS.

54
Conside ing all his, he in eg a ion o Mo eI in his p ojec would ha e o be ela ed o
e e y hing conce ning he obo 's mo emen s. This includes pa h planning, collision
de ec ion, IK calcula ion, he displacemen o he a m, he union wi h he objec and i s
posi ioning in ano he place. Taking he abo e scheme in o accoun , e e y hing ela ed
o he ed and g ey nodes would ha e o be eplaced.
To achie e his, i would be necessa y o use he example om chap e 4.5 o his p ojec ,
which explains how o pe o m a pick and place.
The signi ican di e ence in his case is ha a se ice would need o be c ea ed o que y
he posi ion o he A Uco o he piece o be picked up and whe e i will be placed. Wi h
his in o ma ion, he pick and place launch would be execu ed wi h he wo posi ions and
o ien a ions (pick and place), so he obo pe o ms all he sub asks in ol ed.
Taking he model o he Figu e 22, he se ice o communica e he Sensing Node and
he Mo e G oup o Mo eI can be di ec ly done in he Chess Game Node, which o ganize
all he p ocess.
So, he inal esul should like he ollowing:
Figu e 23. Node diag am p oposal o he in eg a ion o Mo ei in he chesslab en i onmen .
55
5.3. Applica ions o Planning and Implemen a ion o
Robo ic Sys ems subjec
Planning and Implemen a ion o Robo ic Sys ems is he subjec whe e he use o Mo eI
could be mos bene icial. As i s name sugges s, he main objec i e lies in pa h planning
and s udying how o implemen hese sys ems in obo s.
Ini ially, classical and i e a i e me hods o pa h planning a e s udied o unde s and how
he obo 's mo emen pa hs a e c ea ed. Then, he s eps o a ask ( ask planning) a e
di e en ia ed o iden i y he necessa y s a es o he obo and c ea e he mo ion planning,
which ollowing he s eps lis ed in he ask planning and using he p e iously c ea ed
pa hs o pe o m he mo emen s ha lead he obo o achie e he desi ed asks.
Mos o hese concep s and hei applica ion in Mo eI ha e al eady been co e ed in his
wo k. In he pa h planning pa (chap e 4.4), he Mo eI GUI can be con igu ed o see
he end e ec o 's pa h and choose which planne o use in each case, al hough i lacks
a iew o see he i e a ions o he me hods (simila o wha was shown in Figu es 5 and
6).
In he ask planning pa (chap e 4.5), i has been demons a ed wi h he pick and place
example how s ages should be o ganized o achie e mo e complex asks. Finally, he
obo 's mo ion planning can be di ec ly e i ied in simula ions, also using he ee
mo emen op ion ha o e s he in e ace o he ool. I is wo h men ioning ha in his
pa , he obo 's collision iew in he GUI can help unde s and he mo emen cons ain s
in he esul ing pa hs.
The e o e, in o de o dis inguish his exe cise om wha has al eady been done, and
because i was an impo an pa o he inal p ojec done by he s uden s o he subjec ,
whe e a s udy and compa ison o esul s h ough benchma kings has o be done, he
nex exe cise will consis o s udying how o pe o m benchma king, bu wi h da a
collec ed om Mo eI .
5.3.1. Benchma king using Mo eI
Benchma king is a p ocess o e alua ing and compa ing he pe o mance o di e en
me hods, sys ems, o p oduc s. I uses a s anda dized se o da a and condi ions o
each es o iden i y he bes p ac ices. In his way, he e iciency o he p oduc s can be
imp o ed.
To pe o m benchma king using da a om Mo eI nodes, i is essen ial o ollow a se ies
o s eps ha acili a e he con igu a ion, execu ion, con e sion, and isualiza ion o
benchma k esul s.
56
Fi s , he s _mo eI _planne _benchma king package mus be ins alled. Then, he
scenes and que ies mus be de ined. The scenes, desc ibed in "scene" o ma iles,
ep esen he en i onmen in which he obo will ope a e. These iles can be c ea ed,
modi ied, and impo ed using he mo ion planning plugin in RViz.
The que ies, consis ing o he ini ial and inal s a es o he obo o each scene, a e also
de ined and sa ed using RViz, allowing o di e en le els o complexi y in he es s.
The e a e ools o quickly load and expo hese iles, e en expo ing hem o ex iles
o mo e pe sonalized analysis.
Speci ic launch iles a e used o execu e he benchma ks. Addi ionally, i is possible o
speci y o he pa ame e s, wi h he mos impo an being:
- To al ime (s): Time aken by he whole p ocess.
- Sol ed (%): Pe cen age o que ies ha he planne was able o ind a solu ion o .
- Co ec ness (%): Pe cen age o sol ed plans gene a ed by he planne ha a e
co ec . Co ec ness means ha he pa h ajec o y a oids collisions and all
waypoin s sa is y gi en bounds.
- Leng h ( ad): Calcula ed by summing he angles a eled by each o he join s.
- Smoo hness: Looks a h ee consecu i e waypoin s and he angle o med
be ween hem. The alue is calcula ed as a squa e o he sum o all he angles
calcula ed ha way.
- Clea ance (m): Calcula ed by he a e age dis ance o he nea es in alid s a e
(obs acle) h oughou he planned pa h.
A e execu ing he benchma ks, log iles con aining he es esul s a e gene a ed.
5.3.2. Visualiza ion o he esul s
Visualizing he esul s is a c ucial s ep in he benchma king p ocess. The e a e wo
di e en me hods o his, s a ing om he log iles gene a ed in he benchma ks. In bo h
cases, he sc ip mo eI _benchma k_s a is ics.py om he ins alled package mus be un
o gene a e da abases om he log iles.
Wi h hese da abases, he ollowing line can be execu ed in a e minal o gene a e g aphs
om he da a and sa e hem in PDF o ma :
Code 23. Command o make a benchma king wi h Mo eI da a.
py hon -p <plo _ ilename> mo eI _benchma k_s a is ics.py <pa h_o _log ile>
57
Figu e 24. Plo s o he esul ing benchma king using he Mo eI Visualize . Sou ce: [36].
On he o he hand, i a mo e in e ac i e and pe sonalized isualiza ion is p e e ed, da a
isualiza ion pla o ms like Benchma ks Visualize
6
o Planne A ena
7
can be used. In
hese ools, he ile wi h he benchma k da abases is loaded, and summa y g aphs a e
au oma ically displayed o each planne o case s udy. In he case o Planne A ena,
il e ing by speci ic ea u es can be chosen, while in Benchma ks Visualize , he scene is
also shown in RViz.
6
h ps://benchma ks isualize .h ml
7
h ps://planne a ena.o g
64
< isual>
<o igin
xyz="0 0 0"
py="0 0 0" />
<geome y>
<mesh
ilename="package://u 3_model/meshes/base_link.STL" />
</geome y>
<ma e ial
name="">
<colo
gba="0.8980392156862 0.9176470588235 0.929411764705882" />
</ma e ial>
</ isual>
<collision>
<o igin
xyz="0 0 0"
py="0 0 0" />
<geome y>
<mesh
ilename="package://u 3_model/meshes/base_link.STL" />
</geome y>
</collision>
</link>
<link
name="link1">
<ine ial>
<o igin
xyz="-0.00273743686641284 3.63740413567015E-07 0.00510797908322314"
py="0 0 0" />
<mass
alue="0.906535714461713" />
<ine ia
ixx="0.00108616243564948"
ixy="-2.49003190422072E-09"
ixz="-1.93446158304905E-05"
iyy="0.00159690740350841"
iyz="1.5606262521433E-08"
izz="0.00148404114379818" />
</ine ial>
< isual>
<o igin
xyz="0 0 0"
py="0 0 0" />
<geome y>
<mesh
ilename="package://u 3_model/meshes/link1.STL" />
</geome y>
<ma e ial
name="">
<colo
gba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</ma e ial>
</ isual>
<collision>
<o igin
xyz="0 0 0"
py="0 0 0" />
<geome y>
<mesh
ilename="package://u 3_model/meshes/link1.STL" />
</geome y>
</collision>
</link>
<join
name="join 1"

65
ype=" e olu e">
<o igin
xyz="0 0 0.1708"
py="3.14159265358979 -1.5707963267949 0" />
<pa en
link="base_link" />
<child
link="link1" />
<axis
xyz="1 0 0" />
<limi
lowe ="-2.5"
uppe ="2.5"
e o ="300.0"
eloci y="3.0" />
</join >
</ obo >
Sec ion 4. Launch used o execu e he UR3 obo in ROS wi h Mo eI 2:
Code 27. Launch o execu e all necessa y nodes o use he UR3 obo in Mo eI 2.
impo os
om launch impo LaunchDesc ip ion
om launch.ac ions impo Decla eLaunchA gumen
om launch.subs i u ions impo LaunchCon igu a ion, Pa hJoinSubs i u ion
om launch_ os.ac ions impo Node
om launch_ os.subs i u ions impo FindPackageSha e
om amen _index_py hon.packages impo ge _package_sha e_di ec o y
om Mo eI _con igs_u ils impo Mo eI Con igsBuilde
de gene a e_launch_desc ip ion():
# Command-line a gumen s
iz_con ig_a g = Decla eLaunchA gumen (
" iz_con ig",
de aul _ alue="Mo eI . iz",
desc ip ion="RViz con igu a ion ile",
)
# ROS2 con ol launch a gumen s
os2_con ol_ha dwa e_ ype = Decla eLaunchA gumen (
" os2_con ol_ha dwa e_ ype",
de aul _ alue="mock_componen s",
desc ip ion="ROS 2 con ol ha dwa e in e ace",
)
# Mo e G oup node con igu a ion
Mo eI _con ig = (
Mo eI Con igsBuilde ( obo _name="u 3_ obo _sldasm", package_name="u 3_de ")
. obo _desc ip ion( ile_pa h="con ig/u 3_ obo _sldasm.u d .xac o", )
. obo _desc ip ion_seman ic( ile_pa h="con ig/u 3_ obo _sldasm.s d ")
. obo _desc ip ion_kinema ics( ile_pa h="con ig/kinema ics.yaml")
.planning_scene_moni o (publish_ obo _desc ip ion=T ue,
publish_ obo _desc ip ion_seman ic=T ue)
. ajec o y_execu ion( ile_pa h="con ig/Mo eI _con olle s.yaml")
.planning_pipelines(pipelines=["ompl", "chomp",
"pilz_indus ial_mo ion_planne ", "s omp"])
. o_Mo eI _con igs()
)
66
# S a he mo e_g oup node
mo e_g oup_node = Node(
package="Mo eI _ os_mo e_g oup",
execu able="mo e_g oup",
ou pu ="sc een",
pa ame e s=[Mo eI _con ig. o_dic ()], # Con e o a dic iona y
a gumen s=["-- os-a gs", "--log-le el", "in o"],
)
# R iz node
iz_base = LaunchCon igu a ion(" iz_con ig")
iz_con ig = Pa hJoinSubs i u ion([FindPackageSha e("u 3_de "), "launch",
iz_base])
iz_node = Node(
execu able=" iz2",
name=" iz2",
ou pu ="log",
a gumen s=["-d", iz_con ig],
pa ame e s=[Mo eI _con ig. obo _desc ip ion,
Mo eI _con ig. obo _desc ip ion_seman ic,
],
)
# S a ic TF node
s a ic_ _node = Node(
package=" 2_ os",
execu able="s a ic_ ans o m_publishe ",
name="s a ic_ ans o m_publishe ",
ou pu ="log",
a gumen s=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "wo ld", "base_link"],
)
# Publish TF node
obo _s a e_publishe = Node(
package=" obo _s a e_publishe ",
execu able=" obo _s a e_publishe ",
name=" obo _s a e_publishe ",
ou pu ="bo h",
pa ame e s=[Mo eI _con ig. obo _desc ip ion,
Mo eI _con ig. obo _desc ip ion_seman ic],
)
# os2_con olle node
os2_con olle s_pa h = os.pa h.join(ge _package_sha e_di ec o y("u 3_de "),
"con ig", " os2_con olle s.yaml")
os2_con ol_node = Node(
package="con olle _manage ",
execu able=" os2_con ol_node",
pa ame e s=[ os2_con olle s_pa h],
emappings=[("/con olle _manage / obo _desc ip ion", obo _desc ip ion")],
ou pu ="sc een",
)
# Publica los es ados de los join s
join _s a e_b oadcas e _spawne = Node(
package="con olle _manage ",
execu able="spawne ",
a gumen s=["join _s a e_b oadcas e ","/con olle _manage ",],
)
# De ine he con olle s o he Mo eI _con olle s.yaml
u 3_a m_con olle _spawne = Node(
package="con olle _manage ",
execu able="spawne ",
a gumen s=["u 3_a m_con olle ", "-c", "/con olle _manage "],
67
)
u 3_hand_con olle _spawne = Node(
package="con olle _manage ",
execu able="spawne ",
a gumen s=["u 3_hand_con olle ", "-c", "/con olle _manage "],
)
# Launch all he nodes de ined
e u n LaunchDesc ip ion( [
iz_con ig_a g,
os2_con ol_ha dwa e_ ype,
iz_node,
s a ic_ _node,
obo _s a e_publishe ,
mo e_g oup_node,
os2_con ol_node,
join _s a e_b oadcas e _spawne ,
u 3_a m_con olle _spawne ,
u 3_hand_con olle _spawne ,
]
)
Sec ion 5. Launch used o execu e a pick and place mo emen wi h he UR3 obo
in ROS wi h Mo eI 2:
Code 28. Launch o execu e a pick and place in Mo eI 2 wi h he UR3 obo model.
// C ea ion o a new logge and namespace o he code
s a ic cons clcpp::Logge LOGGER = clcpp::ge _logge ("m c_ u o ial_u 3");
namespace m c = Mo eI :: ask_cons uc o ;
// Class wi h he Task Cons uc o unc ionali y
class MTCTaskNode{
public:
MTCTaskNode(cons clcpp::NodeOp ions& op ions);
clcpp::node_in e aces::NodeBaseIn e ace::Sha edP ge NodeBaseIn e ace();
oid doTask();
// Call he unc ion ha c ea es he scene
oid se upPlanningScene();
p i a e:
// Compose an MTC ask om a se ies o s ages.
(↕) cu en s a e
(II) mo e o pick
(↕) pick he objec
(↑) app oach
(↕) g asp pose IK
(↕) gene a e g asp pose
(↓) allow con ac
(↓) close g ippe
(↓) a ach objec
(↓) li objec
68
m c::Task c ea eTask();
m c::Task ask_;
clcpp::Node::Sha edP node_;
};
// Ini ialize he node wi h speci ic op ions
MTCTaskNode::MTCTaskNode(cons clcpp::NodeOp ions& op ions)
: node_{ s d::make_sha ed< clcpp::Node>("m c_node_u 3", op ions) }{}
// De ine a ge e unc ion o ge he node base in e ace,
clcpp::node_in e aces::NodeBaseIn e ace::Sha edP
MTCTaskNode::ge NodeBaseIn e ace(){
e u n node_->ge _node_base_in e ace();
}
// Func ion o se he planning scene
oid MTCTaskNode::se upPlanningScene(){
// C ea e a colision objec
Mo eI _msgs::msg::CollisionObjec objec ;
objec .id = "objec ";
objec .heade . ame_id = "wo ld";
objec .p imi i es. esize(1);
objec .p imi i es[0]. ype = shape_msgs::msg::SolidP imi i e::CYLINDER;
objec .p imi i es[0].dimensions = { 0.3, 0.01 };
// Posi ion he objec
geome y_msgs::msg::Pose pose;
pose.posi ion.x = -0.5;
pose.posi ion.y = -0.33;
pose.posi ion.z = 0.15;
pose.o ien a ion.x = 0.0;
pose.o ien a ion.y = 0.0;
pose.o ien a ion.z = 0.0;
pose.o ien a ion.w = 1.0;
objec .pose = pose;
// Si e pa a ins e a el obje o en la escena como obs aculo y ac ualiza los
alo es en cada momen o
Mo eI ::planning_in e ace::PlanningSceneIn e ace psi;
psi.applyCollisionObjec (objec );
}
oid MTCTaskNode::doTask(){
// C ea e a ask
ask_ = c ea eTask();
// T y o ini ialize he ask
y{
ask_.ini ();
}
ca ch (m c::Ini S ageExcep ion& e){
RCLCPP_ERROR_STREAM(LOGGER, e);
e u n;
}
// Gene a es a plan, s opping a e 5 success ul plans a e ound
i (! ask_.plan(5)){
RCLCPP_ERROR_STREAM(LOGGER, "Task planning ailed");
e u n;
}
// Publish he solu ion o he plan
ask_.in ospec ion().publishSolu ion(* ask_.solu ions(). on ());
69
// Execu es he plan
au o esul = ask_.execu e(* ask_.solu ions(). on ());
i ( esul . al != Mo eI _msgs::msg::Mo eI E o Codes::SUCCESS){
RCLCPP_ERROR_STREAM(LOGGER, "Task execu ion ailed");
e u n;
}
e u n;
}
// C ea ion o a Task Cons uc o objec
m c::Task MTCTaskNode::c ea eTask(){
m c::Task ask;
ask.s ages()->se Name("demo ask"); // Task name
ask.loadRobo Model(node_); // Load he obo
// De ine he names o he impo an ames
cons au o& a m_g oup_name = "u 3_a m";
cons au o& hand_g oup_name = "hand";
cons au o& hand_ ame = "link6";
// Se ask p ope ies
ask.se P ope y("g oup", a m_g oup_name);
ask.se P ope y("ee ", hand_g oup_name);
ask.se P ope y("ik_ ame", hand_ ame);
// Disable wa nings o his line, as i 's a a iable ha 's se bu no used in
his example
#p agma GCC diagnos ic push
#p agma GCC diagnos ic igno ed "-Wunused-bu -se - a iable"
m c::S age* cu en _s a e_p = nullp ; // Fo wa d cu en _s a e on o g asp
pose gene a o
#p agma GCC diagnos ic pop
/****************************************************
* Cu en S a e *
***************************************************/
au o s age_s a e_cu en =
s d::make_unique<m c::s ages::Cu en S a e>("cu en ");
cu en _s a e_p = s age_s a e_cu en .ge ();
ask.add(s d::mo e(s age_s a e_cu en ));
au o sampling_planne = s d::make_sha ed<m c::sol e s::PipelinePlanne >(node_);
au o in e pola ion_planne =
s d::make_sha ed<m c::sol e s::Join In e pola ionPlanne >();
au o ca esian_planne = s d::make_sha ed<m c::sol e s::Ca esianPa h>();
ca esian_planne ->se MaxVeloci yScalingFac o (1.0);
ca esian_planne ->se MaxAccele a ionScalingFac o (1.0);
ca esian_planne ->se S epSize(.01);
//Mo e o pick
au o s age_mo e_ o_pick = s d::make_unique<m c::s ages::Connec >(
"mo e o pick",
m c::s ages::Connec ::G oupPlanne Vec o { { a m_g oup_name, sampling_planne
} });
s age_mo e_ o_pick->se Timeou (10.0);
s age_mo e_ o_pick->p ope ies().con igu eIni F om(m c::S age::PARENT);
ask.add(s d::mo e(s age_mo e_ o_pick));
//C ea ion o a poin e o a Mo eI Task Cons uc o s age objec
m c::S age* a ach_objec _s age =
nullp ; // Fo wa d a ach_objec _s age o place pose gene a o
//Se ial con aine o c ea e he pick mo emen
{
au o g asp = s d::make_unique<m c::Se ialCon aine >("pick objec ");

70
ask.p ope ies().exposeTo(g asp->p ope ies(), { "ee ", "g oup", "ik_ ame"
});
g asp->p ope ies().con igu eIni F om(m c::S age::PARENT,
{ "ee ", "g oup", "ik_ ame" });
//App oach o he objec
{
au o s age =
s d::make_unique<m c::s ages::Mo eRela i e>("app oach objec ",
ca esian_planne );
s age->p ope ies().se ("ma ke _ns", "app oach_objec ");
s age->p ope ies().se ("link", hand_ ame);
s age->p ope ies().con igu eIni F om(m c::S age::PARENT, { "g oup" });
s age->se MinMaxDis ance(0.0, 0.25);
// Se hand o wa d di ec ion
geome y_msgs::msg::Vec o 3S amped ec;
ec.heade . ame_id = hand_ ame;
ec. ec o .z = 1.0;
s age->se Di ec ion( ec);
g asp->inse (s d::mo e(s age));
}
{
// Sample g asp pose
au o s age = s d::make_unique<m c::s ages::Gene a eG aspPose>("gene a e g asp
pose");
s age->p ope ies().con igu eIni F om(m c::S age::PARENT);
s age->p ope ies().se ("ma ke _ns", "g asp_pose");
s age->se P eG aspPose("open");
s age->se Objec ("objec ");
s age->se AngleDel a(M_PI / 12);
s age->se Moni o edS age(cu en _s a e_p ); // Hook in o cu en s a e
Eigen::Isome y3d g asp_ ame_ ans o m;
Eigen::Qua e niond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vec o 3d::Uni X())*
Eigen::AngleAxisd(M_PI / 2, Eigen::Vec o 3d::Uni Y())*
Eigen::AngleAxisd(M_PI / 2, Eigen::Vec o 3d::Uni Z());
g asp_ ame_ ans o m.linea () = q.ma ix();
g asp_ ame_ ans o m. ansla ion().z() = 0.075;
// Compu e IK
au o w appe =
s d::make_unique<m c::s ages::Compu eIK>("g asp pose IK",
s d::mo e(s age));
w appe ->se MaxIKSolu ions(50);
w appe ->se MinSolu ionDis ance(2.0);
w appe ->se IKF ame(g asp_ ame_ ans o m, hand_ ame);
w appe ->p ope ies().con igu eIni F om(m c::S age::PARENT, { "ee ", "g oup"
});
w appe ->p ope ies().con igu eIni F om(m c::S age::INTERFACE, {
" a ge _pose" });
g asp->inse (s d::mo e(w appe ));
}
// Allow collisions
{
au o s age =
s d::make_unique<m c::s ages::Modi yPlanningScene>("allow collision
(hand,objec )");
s age->allowCollisions("objec ",
ask.ge Robo Model()
->ge Join ModelG oup(hand_g oup_name)
->ge LinkModelNamesWi hCollisionGeome y(),
ue);
g asp->inse (s d::mo e(s age));
71
}
// Close hand
{
au o s age = s d::make_unique<m c::s ages::Mo eTo>("close hand",
in e pola ion_planne );
s age->se G oup(hand_g oup_name);
s age->se Goal("close");
g asp->inse (s d::mo e(s age));
}
// A ach he objec
{
au o s age = s d::make_unique<m c::s ages::Modi yPlanningScene>("a ach
objec ");
s age->a achObjec ("objec ", hand_ ame);
a ach_objec _s age = s age.ge ();
g asp->inse (s d::mo e(s age));
}
// li he objec in a linea mo emen
{
au o s age =
s d::make_unique<m c::s ages::Mo eRela i e>("li objec ",
ca esian_planne );
s age->p ope ies().con igu eIni F om(m c::S age::PARENT, { "g oup" });
s age->se MinMaxDis ance(0.1, 0.15);
s age->se IKF ame(hand_ ame);
s age->p ope ies().se ("ma ke _ns", "li _objec ");
// Se upwa d di ec ion
geome y_msgs::msg::Vec o 3S amped ec;
ec.heade . ame_id = "wo ld";
ec. ec o .z = 1.0;
s age->se Di ec ion( ec);
g asp->inse (s d::mo e(s age));
}
ask.add(s d::mo e(g asp)); //End o Pick mo emen
}
Sec ion 6. Launch used o execu e he UR3 obo in ROS wi h Mo eI 2:
Code 29. Example o he launch ha ini ialises he wo nodes needed o con ol he obo om he joys ick.
om launch impo LaunchDesc ip ion
om launch_ os.ac ions impo Node
de gene a e_launch_desc ip ion():
e u n LaunchDesc ip ion([
Node(
package='joy',
execu able='joy_node',
name='joy_node',
ou pu ='sc een',
),
Node(
package='Mo eI _se o',
execu able='JoyToSe oPub',
72
name='con olle _ o_se o_node',
ou pu ='sc een',
pa ame e s=[{
'pa ame e _namespace': 'Mo eI _se o',
'mo e_g oup_name': 'u 3_a m',
'planning_ ame': 'wo ld',
'command_in_ ype': 'uni less',
'publish_join _posi ions': T ue,
'publish_join _ eloci ies': False,
'publish_join _accele a ions': False,
'command_ou _ opic': '/se o_se e /del a_ wis _cmds',
'command_ou _ ype': 'geome y_msgs/Twis S amped',
'join _ opic': '/join _s a es',
'low_la ency_mode': T ue
}],
)
])
73
Re e ences
[1] H. Yu, C. Hi ayama, C. Yu, S. He be , and S. Gao, “Sequen ial Neu al Ba ie s
o Scalable Dynamic Obs acle A oidance,” IEEE In . Con . In ell. Robo . Sys .,
pp. 11241–11248, 2023, doi: 10.1109/IROS55552.2023.10341605.
[2] “Shakey he obo .” h ps://en.wikipedia.o g/wiki/Shakey_ he_ obo (accessed
Ap . 21, 2024).
[3] K. Ka u , N. Sha ma, C. Dha ma i, and J. E. Siegel, “A Su ey o Pa h Planning
Algo i hms o Mobile Robo s,” Vehicles, ol. 3, no. 3, pp. 448–468, 2021, doi:
10.3390/ ehicles3030027.
[4] “The Open Mo ion Planning Lib a y.” h ps://ompl.ka akilab.o g/ (accessed Ap .
21, 2024).
[5] “Home | Pa hPlanne Docs.” h ps://pa hplanne .de /home.h ml#usage (accessed
Ap . 21, 2024).
[6] B. Li e al., “Pa h planning based on i e ly algo i hm and Bezie cu e,” 2014 IEEE
In . Con . In . Au om. ICIA 2014, no. July, pp. 630–633, 2014, doi:
10.1109/ICIn A.2014.6932730.
[7] P. I. Co ke, Robo ics, ision and con ol : undamen al algo i hms in MATLAB®. .
[8] “Mo ei 2 web page.” h ps://mo ei . os.o g (accessed Feb. 22, 2024).
[9] “RoboDK web page.” h ps:// obodk.com/es/ (accessed Feb. 22, 2024).
[10] “Na 2 web page.” h ps://na 2.o g (accessed Feb. 22, 2024).
[11] J. H. Pa k, W. J. Pa k, C. Lee, M. Kim, S. Kim, and H. J. Kim, “Endoscopic Came a
Manipula ion planning o a su gical obo using Rapidly-Explo ing Random T ee
algo i hm,” ICCAS 2015 - 2015 15 h In . Con . Con ol. Au om. Sys . P oc., no.
Iccas, pp. 1516–1519, 2015, doi: 10.1109/ICCAS.2015.7364594.
[12] “Mo eI 2 - His o y.” h ps://mo ei . os.o g/abou / (accessed Feb. 24, 2024).
[13] J. Meye , The Essen ial Guide o HTML5. 2023.
[14] D. Coleman, I. Sucan, S. Chi a, and N. Co ell, “Reducing he Ba ie o En y o
Complex Robo ic So wa e: a Mo eI ! Case S udy,” pp. 1–14, 2014, [Online].
A ailable: h p://a xi .o g/abs/1404.3785.
[15] J. Badge , D. Gooding, K. Ensley, K. Hambuchen, and A. Thacks on, “ROS in
Space: A Case S udy on Robonau 2,” Sp inge , Cham, 2016, pp. 343–373.
[16] “Mo eI 2 - Robo s.” h ps://mo ei . os.o g/ obo s/ (accessed Feb. 24, 2024).
[17] H. Deng, J. Xiong, and Z. Xia, “Mobile manipula ion ask simula ion using ROS
wi h Mo eI ,” 2017 IEEE In . Con . Real-Time Compu . Robo . RCAR 2017, ol.
2017-July, no. Augus 2018, pp. 612–616, 2017, doi:
10.1109/RCAR.2017.8311930.
[18] Z. Kings on and L. E. Ka aki, “Robow lex: Robo Mo ion Planning wi h Mo eI