1
Equa ion Chap e 1 Sec ion 1
T abajo Fin de G ado
G ado en Ingenie ía de las Tecnologías
Indus iales
Ope ación de emachado median e obo manipulado
indus ial basado en ROS
Au o : Víc o Hugo Gómez Tejada
Tu o : Guille mo He edia Beno
Dep. Ingenie ía de Sis emas y Au omá ica
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2015
T abajo Fin de G ado
G ado en Ingenie ía de las Tecnología Indus iales
Ope ación de emachado median e obo
manipulado indus ial basado en ROS
Au o :
Víc o Hugo Gómez Tejada
Tu o :
Guille mo He edia Beno
P o eso i ula
Dep. de Ingenie ía de Sis emas y Au omá ica
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2015
P oyec o Fin de G ado: Ope ación de emachado median e obo manipulado indus ial basado en ROS
Au o :
Víc o Hugo Gómez Tejada
Tu o :
Guille mo He edia Beno
El ibunal nomb ado pa a juzga el P oyec o a iba indicado, compues o po los siguien es miemb os:
P esiden e:
Vocales:
Sec e a io:
Acue dan o o ga le la cali icación de:
Se illa, 2015
El Sec e a io del T ibunal

i
Resumen
El diseño e implemen ación de p ocesos cada ez más au oma izados en la indus ia ha empujado el desa ollo
ecnológico del campo de la obó ica en los úl imos años. Es o ha p opiciado la inicia i a de p omo e e
in e i en el campo de in es igación de la obó ica indus ial.
EuRoC, una o ganización de ca ác e eu opeo, ha p omulgado la inicia i a en la in es igación en es e campo
median e el lanzamien o de un desa ío es uc u ado en dis in os ni eles, aquí es donde se encuen a el p oyec o
en cues ión.
La o ganización nos plan ea un e o de ca ác e indus ial, donde nos si ua en un en o no de simulación donde
debemos ealiza una a ea de emachado a una pieza de abajo median e un obo manipulado indus ial. En
es e desa ío se nos alo a á la capacidad pa a p og ama nues o obo median e el amewo k obó ico ROS
pa a ealiza la a ea de emachado, e aluando su apidez y e iciencia.
En las siguien es páginas se p esen a á el p oyec o, así como su solución p opues a con su cali icación inal
po pa e del colec i o EuRoC.
ii
Índice
Resumen i
Índice ii
Índice de Tablas i
Índice de Figu as
1 In oducción 1
1.1. EuRoC 1
1.1.1. Resumen 2
1.1.2. Obje i os 3
1.1.3. Challenge 1 3
2 Es ado del a e de la Robó ica 5
2.1. La Robó ica 5
2.2. His o ia de la Robó ica 5
2.3.Clasi icación de Robo s 6
2.4 Robo manipulado o indus ial 7
2.4.1 Es uc u a mecánica 8
2.4.2 Aplicaciones 8
2.4.2.1 Remachados median e Robo s 9
2.4.3 Nue as es uc u as 9
2.5. La simulación Robó ica 10
3 Modelado, con ol y simulación de obo s 11
3.1. Rep esen ación de posición y o ien ación 11
3.2. Modelo cinemá ico del obo manipulado 14
3.2.1 Cinemá ica di ec a 14
3.2.1.1. Rep esen ación Dena i -Ha enbe g 15
3.2.2 Cinemá ica in e sa 17
3.3 Velocidades: Jacobiano de un obo manipulado 17
3.4. Modelo dinámico de un obo manipulado 17
3.5 Con ol de a iculaciones de un obo manipulado 18
4 ROS y he amien as de desa ollo 19
4.1. ROS 19
4.2. Diseño modula y dis ibuido 20
4.3. His o ia 20
4.4. Comunidad ac i a y colabo a i a 21
4.5. Concep os básicos 21
4.6. He amien as ROS 23
4.6.1. Gazebo 23
4.6.2. RVIZ 23
4.6.3. TF 24
4.6.4. URDF 25
iii
4.6.5. UR_KIN_PY 26
4.6.6. Me cu ial 26
5 En o no de Simulación 28
5.1. En o no de simulación 28
5.1.1. Sis ema obó ico 29
5.1.2. Humano simulado 30
5.1.3. Pieza de abajo 30
5.2.Obje i o de la a ea 31
5.3. In e acción con el sis ema 32
5.4 E aluación 33
6 Algo i mo de plani icación y con ol de p oceso de emachado 34
6.1. Máquina de es ado 36
6.2. Mo e el obo 38
6.3 De ec a aguje o de emachado 34
6.4 Ejemplo de ejecución 40
7 Resul ados y conclusiones 41
Anexa A: A anque del Sis ema 42
Anexo B: S a e Machine Code 43
Bibliog a ía 47
In oducción
4
4
 Mé odos de con ol de sis emas obó icos de coope ación mul i- unción en un en o no indus ial
ele an e.
En la p ime a ase de es e challenge iene luga mi p oyec o. Cons a de 4 a eas in e ac i as obo -ope a io
obo - obo , que se ealiza án en dos escena ios de simulación dis in os, la a ea 1 y 2 en el p ime escena io, y
la 3 y la 4 en el segundo.
La a ea 1 no se u iliza el obo , se a a de una a ea de ca ác e de pe cepción de las cáma as del en o no. Hay
que iden i ica un ges o señalado del ope a io, así como econoce el pun o de la pieza que señala, y es ima la
posición de la pieza. Todo es o u ilizando las cáma as kinec .
La a ea 2 consis e en la p og amación de nues o obo pa a que ealice una secuencia de emachado.
Las a eas 3 y 4 consis en en con ol de coo dinación de dos b azos manipulado es obó icos, en la a ea 3 se
a a de in oduci un obje o que po a un b azo en o o obje o que po a el o o, y la a ea 4 se a a de mo e
un obje o de o ma coo dinada en e ambos manipulado es esqui ando un obs áculo del escena io.
Figu a 3: EuRoC pa icipan es.

5
5
Ope ación de emachado median e obo manipulado indus ial basado en ROS
2 ESTADO DEL ARTE DE LA ROBÓTICA
n es de comenza con el plan eamien o de la a ea encomendada y su pos e io desa ollo de la
solución, da emos un b e e epaso sob e la obó ica en gene al, sus inicios, su his o ia… has a
cen a nos en lo ele an e en nues o p oyec o, el obo indus ial y la simulación obó ica.
2.1 La Robó ica
El es udio de la obó ica equie e de un amplio conocimien o de la misma, y aunque es un á ea ela i amen e
nue a, los a ances ealizados en sus pocos años de his o ia han sido muy impo an es.
El é mino obó ica p ocede de la palab a obo . El é mino obo ue in oducido po el checo Ka el Capek en
1921, y iene de la combinación de las palab as checas “ obo a”, que signi ica “ abajo obliga o io” y
“ obo nik”, que signi ica cie o. La obó ica es, po an o, la ciencia o ama de la ciencia que se ocupa del
es udio, desa ollo y aplicaciones de los obo s.
O a de inición de obó ica es el diseño, ab icación y u ilización de máquinas au omá icas p og amables con
el in de ealiza a eas epe i i as como el ensamble de au omó iles, apa a os, e c. y o as ac i idades.
Básicamen e, la obó ica se ocupa de odo lo conce nien e a los obo s, lo cual incluye el con ol de mo o es,
mecanismos au omá icos neumá icos, senso es, sis emas de cómpu os, e c.
La obó ica [2] es una disciplina con sus p opios p oblemas, sus undamen os y sus leyes, Tiene dos e ien es:
eó ica y p ác ica. En el aspec o eó ico de aúnan las apo aciones de la au omá ica, in o má ica e in eligencia
a i icial. Po el lado p ác ico o ecnológico hay aspec os de cons ucción (mecánica, elec ónica), y de ges ión
(con ol, p og amación). La obó ica p esen a po lo an o un ma cado ca ác e in e disciplina io.
En la obó ica se aúnan pa a un mismo in a ias disciplinas a ines, pe o di e en es, como la Mecánica, la
Elec ónica, la Au omá ica, la In o má ica, e c. Los es p incipios o leyes de la obó ica según Asimo son:
 Un obo no puede daña ni pe mi i que sea dañado ningún se humano.
 El obo debe obedece a odas las ó denes de los humanos, excep o las que con aigan la p ime a ley.
 El obo debe au op o ege se, sal o que pa a hace lo en e en con lic o con la p ime a o segunda ley.
Los obo s son disposi i os compues os de senso es que eciben da os de en ada y que pueden es a
conec ados a la compu ado a. Es a, al ecibi la in o mación de en ada, o dena al obo que e ec úe una
de e minada acción. Puede se que los p opios obo s dispongan de mic op ocesado es que eciben el inpu de
los senso es y que es os mic op ocesado es o denen al obo la ejecución de las acciones pa a las cuales es á
concebido. En es e úl imo caso, el p opio obo es a su ez una compu ado a.
2.2 His o ia de la Robó ica
Po siglos, el se humano ha cons uido máquinas que imi an pa es del cue po humano. Los an iguos egipcios
unie on b azos mecánicos a las es a uas de sus dioses; los g iegos cons uye on es a uas que ope aban con
sis emas hid áulicos, los cuales e an u ilizados pa a ascina a los ado ado es de los emplos.
El inicio de la obó ica ac ual puede ija se en la indus ia ex il del siglo XVIII, cuando Joseph Jacqua d
in en a en 1801 una máquina ex il p og amable median e a je as pe o adas. Luego, la Re olución Indus ial
impulsó el desa ollo de es os agen es mecánicos. Además de es o, du an e los siglos XVII y XVIII en Eu opa
ue on cons uidos muñecos mecánicos muy ingeniosos que enían algunas ca ac e ís icas de obo s. En 1805,
Hen i Mailla de cons uyó una muñeca mecánica que e a capaz de hace dibujos.
La palab a obo , como bien se desa olló an es comenzó a u iliza se en 1921 en la ob a del d ama u go checo
Ka el Capek “Los Robo s Uni e sales de Rossum”, donde un homb e ab icó un obo que luego és e ma a ía a
A
Es ado del A e de la Robó ica
6
6
su c eado .
Son a ios ac o es que in e ienen pa a que se desa olla an los p ime os obo s en la década de los 50’s. La
in es igación en in eligencia a i icial desa olló mane as de emula el p ocesamien o de in o mación humana
con compu ado as elec ónicas e in en ó una a iedad de mecanismos pa a p oba sus eo ías. Las p ime as
pa en es apa ecie on en 1946 con los muy p imi i os obo s pa a aslado de maquina ia de De ol. En 1954,
con la apa ición de las p ime as compu ado as, De ol diseña el p ime obo p og amable
En los 60’s se ins aló en la Fo d Mo o s Company el obo Unima e, basado en la ans e encia de a ículos.
Ya en los 70’s, la S and o d Uni e si y desa olló un pequeño b azo obó ico de accionamien o eléc ico,
bau izado como “S and o d A m”.
Ac ualmen e, el concep o de obó ica ha e olucionado has a los sis emas mó iles au ónomos, que son aquellos
que son capaces de desen ol e se po sí mismos en en o nos desconocidos y pa cialmen e cambian es sin
necesidad de supe isión.
En los se en a, la NASA inició un p og ama de coope ación con el Je P opulsión Labo a o y pa a desa olla
pla a o mas capaces de explo a e enos hos iles.
En la ac ualidad, la obó ica se deba e en e modelos sumamen e ambiciosos, como es el caso del COG “ obo
de cua o sen idos”, desa ollado en el Ins i u o Tecnológico de Massachuse s (MIT), con p oceso de
ap endizaje cogni i o p opio, ehículos con con ol emo o como SOUJOURNER O LUNA ROVER, o los
obo s masco as de Sony.
En gene al la his o ia de la obó ica la podemos clasi ica en cinco gene aciones: las dos p ime as, ya
alcanzadas en los ochen a, incluían la ges ión de a eas epe i i as con au onomía muy limi ada. La e ce a
gene ación inclui ía isión a i icial, en lo cual se ha a anzado mucho en los ochen a y no en as. La cua a
incluye mo ilidad a anzada en ex e io es e in e io es y la quin a en a ía en el dominio de la in eligencia
a i icial en lo cual se es á abajando ac ualmen e.
2.3 Clasi icación de Robo s
De mane a gene al, y basándose en su mo ología, los obo s se suelen di idi en los siguien es ipos:
 Robo indus ial o manipulado
Son a ilugios mecánicos y elec ónicos des inados a ealiza de o ma au omá ica de e minados p ocesos de
ab icación o manipulación. Se u ilizan p incipalmen e en la ab icación indus ial.
Los obo s indus iales son, con di e encia, el ipo de obo más u ilizado, siendo Es ados Unidos y Japón los
líde es an o en su ab icación como en su consumo.
Figu a 4: Robo indus ial o manipulado .
7
7
Ope ación de emachado median e obo manipulado indus ial basado en ROS
 Robo s mó iles
Los obo s mó iles es án p o is os de algún ipo de mecanismo que les pe mi e desplaza se de luga
au ónomamen e, como pueden se pa as, uedas u o ugas y eciben la in o mación del en o no con sus p opios
sis emas senso es. Son empleados en plan as indus iales pa a el anspo e de me cancías y pa a la
explo ación de luga es de di ícil acceso o muy dis an es, como es el caso dela explo ación espacial y de las
in es igaciones o esca es subma inos.
 And oides o humanoides
In en an ep oduci o al o pa cialmen e la o ma y el compo amien o del se humano. Ac ualmen e, los
and oides es án bas an e e olucionados, sob e odo en Japón, pe o aún no ienen una u ilidad p ác ica po sus
p opias limi aciones y po su p ecio de ab icación. Básicamen e es án des inados a la in es igación y a a eas
de ma ke ing de las p opias emp esas desa ollado as.
Figu a 5: Robo humanoide.
 Zoomó icos
Los obo s zoomó icos ep oducen con mayo o meno g ado de ealismo, los sis emas de locomoción de
di e sos se es i os.
A con inuación, de alla emos y nos cen a emos en el obo usado en el p oyec o, el obo manipulado o
indus ial.
2.4 Robo manipulado o indus ial
Los obo s manipulado es o obo s indus iales (conocidos así po que inicialmen e ue on usados masi amen e
en la indus ia) ue on los enca gados de inaugu a la e a de los obo s en los años 60, con la he encia
adqui ida de los p ime os eleope ado es. Po ello, es el á ea de la obó ica donde la in es igación es á más
a anzada.
Es di ícil encon a in es igaciones que abajen en los aspec os básicos de los obo s manipulado es
adicionales y, las pocas que hay, es án cen adas en la inclusión de mode nos senso es o ac uado es. Un
ejemplo de es o es la adición de cáma as pa a econocimien o a anzado de imágenes que mejo en la
e ec i idad de dichos obo s.
Es ado del A e de la Robó ica
8
8
El á ea más in e esan e, y donde sí que se in es iga de mane a impo an e es en la búsqueda de no edosas
aplicaciones pa a los obo s manipulado es, como es el caso de los obo s qui ú gicos, los cuales es án
eniendo un g an auge en es os momen os. En es e caso, la in es igación no es á cen ada en el obo sino en
adap a és e a las necesidades p opias de la aplicación.
2.4.1 Es uc u a mecánica
Se conoce ambién al obo indus ial como b azo obó ico y es o se debe a que gua da cie a simili ud con la
ana omía del b azo humano. También po ello, pa a hace e e encia a los dis in os elemen os que componen
el obo , se u ilizan é minos como cue po, b azo, codo o muñeca.
Fo malmen e, se denomina base al pun o de apoyo del obo , gene almen e suje o de o ma ija al suelo. Los
elemen os o eslabones an unidos po medio de di e en es a iculaciones, que pe mi en un mo imien o
ela i o en e dos eslabones consecu i os. En la pa e inal, se si úa el e ec o inal, que son los enca gados de
in e acciona di ec amen e con el en o no del obo .
Figu a 6: Es uc u a mecánica del manipulado .
El mo imien o de cada a iculación puede se de desplazamien o, de gi o o de una combinación de ambos. De
es e modo son posibles los 5 ipos di e en es de a iculaciones, con sus di e en es g ados de libe ad, que
pueden se a iculación de o ación(1), p ismá ica(1), cilínd ica(2), plana (2) y es é ica(3).
Los g ados de libe ad son el núme o de mo imien os independien es que puede ealiza cada a iculación con
espec o a la an e io . El núme o de g ados de libe ad de un obo iene dado po la suma de los g ados de
libe ad de cada una de sus a iculaciones.
El empleo de di e en es combinaciones de a iculaciones en un obo , da luga a di e en es con igu aciones con
ca ac e ís icas a ene en cuen a an o en el diseño y cons ucción del obo como en su aplicación.
2.4.2 Aplicaciones
Los obo s manipulado es ienen su p incipal oco de abajo en la indus ia, au oma izando los p ocesos de
p oducción o almacenaje. Gene almen e no abajan de o ma independien e sino en conjun o con o as
máquinas y he amien as o mando células de abajo. Se enume an a con inuación algunos ejemplos:
9
9
Ope ación de emachado median e obo manipulado indus ial basado en ROS
 Ope aciones de p ocesamien o, como soldadu a, pin u a, e c. Es e ipo de obo s son muy comunes en
la indus ia de la au omoción.
 Ope aciones de ensamblaje, donde el abajo epe i i o acili a el uso de es e ipo de obo s.
 Ope aciones de empaque (en a imas o palle s), agilizando el p oceso y manejando g andes pesos.
 O o ipo de ope aciones como pueden se emachados, es ampados, co e po cho o de agua, sis emas
de medición, e c.
Conc e amen e, el obo manipulado del p oyec o iene una aplicación indus ial de p oceso de emachado a
una pieza de abajo p o is a de aguje os.
2.4.2.1 Remachados median e Robo
Con el in de consegui e iciencia y lexibilidad en la indus ia, se han desa ollado obo s des inados a ealiza
a eas como emachado o a o nillado, libe ando así una g an ca ga de abajo pa a el ope a io, po no habla de
la mayo e ec i idad conseguida con el obo .
Los obo s de emachado son aquellos u ilizados pa a sella ma e iales o a ículos me álicos. Gene almen e se
u lizan en in o má ica y elec ónica, así como en la ab icación y ensamblado de elec odomés icos. Es os
obo s ealizan una misma a ea con g an e ec i idad, como po ejemplo ambién pod ía se el a o nillado.
Es os emaches se pueden ealiza ambién median e soldadu a, en unción de lo que se p og ame en cada caso
y según las necesidades del clien e.
En el me cado ac ual podemos encon a nume osas emp esas dedicadas a la come cialización de obo s y sus
ab ican es como pueden se S äubli, Yaskawa o Adep Technology des inadas a dis in os sec o es como la
ca pin e ía me álica, o los elec odomés icos, in o má ica o elec ónica, ci ados an e io men e.
Figu a 7: Robo de emachado S äubli TX90.
2.4.3 Nue as es uc u as de obo s manipulado es
El obo manipulado adicional que se ha p esen ado an e io men e es el más usado, pe o desde hace iempo
se in es iga mucho en no edosas es uc u as pa a es e ipo de obo s:
 Robo s manipulado es edundan es: Básicamen e se componen de un g an núme o de eslabones,
los cuales además ienen la cualidad de se iguales y epe i i os. Se denominan ambién obo s de ipo
se pien e. Es os obo s ienen la capacidad de in oduci se po espacios poco es uc u ados debido a su
lexibilidad.

Es ado del A e de la Robó ica
10
10
 Robo s manipulado es an opomo os (gene almen e imi ando la o ma de la mano humana): Su
aplicación se aleja de la indus ia adicional y se ace ca más a la c eación de p ó esis pa a la
in es igación médica.
2.5 La simulación Robó ica
La simulación juega un papel c ucial p e io en los p oyec os obó icos g acias a que nos pe mi e ealiza las
ope aciones de alidación y e i icación de aplicaciones obó icas an es de la cons ucción ísica del obo , lo
que conlle a un impo an e aho o an o de iempo como de dine o. Su empleo no sólo se basa en un ca ác e ,
po llama lo de alguna mane a, p e en i o del p oduc o. La simulación i ual ambién iene una impo an e
aplicación a la ho a de en ena el manejo y comp ende las ca ac e ís icas del disposi i o an es de emplea el
obo eal.
Simulado es basados en compo amien os obó icos nos pe mi en c ea mundos simpli icados con obje os
ígidos y p og ama obo s que in e ac úen con ellos. En ocasiones, un en o no con condiciones ex emas u
ope aciones en un á ea emo a no nos pe mi en e i ica odos los p ocedimien os y p oblemas ísico-
mecánicos ni po ello es ea de una o ma sólo expe imen al. Po lo an o, pueden p esen a se ocasiones en las
que haya que oma decisiones con el único análisis de los esul ados de una simulación. Una de las
aplicaciones más popula es es el modelado 3D y su ep esen ación que equie en de buenos mo o es de leyes
ísicas y buenos g á icos pa a una emulación acep able del obo y el mundo que lo odea. Es o implica que
cada obo end á unas p opiedades g á icas y unas ísicas.
Las écnicas de simulación e implemen ación obó ica necesa iamen e pasan po una desc ipción analí ica del
sis ema ísico-mecánico. Sin emba go, a eces puede no con a se con odos los pa áme os eales pa a simula
un en o no cien po cien ealis a. Se emplean en onces écnicas de manejo de da os con la p obabilidad y
es adís ica. Es necesa io ambién el modelado numé ico con su espec i o so wa e y oolboxes. Son
empleados simulado es mul idominio e híb idos con mé odos que sopo en una simulación ápida en iempo
eal.
En la ac ualidad, la simulación obó ica nos p opo ciona una g andísima a iedad de elemen os: di e en es
amilias de obo s (UGV pa a ie a, UAV pa a ai e, AUV pa a agua, b azos obó icos, manos obó icas,
humanoides, a a a es…), ac uado es (Cadenas cinemá icas gené icas, ac uado es de ue za con olada…),
subac uado es, senso es (Odome ía, IMU, GPS, cáma as, láse s, emiso es y ecep o es…), mecanismos,
manipulado es, he amien as obó icas…). Llegando incluso a exis i simulado es empleados en a eas en el
espacio como sa éli es o ae ona es.
Cien í icos e ingenie os han enido desa ollando conjun amen e una g an a iedad de écnicas de modelado y
simulación c eando di e en es so wa es, de los cuales des aca é a con inuación los más impo an es. Mo se es
un simulado de un obo de código abie o con sopo e comple o a ROS . Usa OpenGL como su mo o 3D y
ienen un ende ealis a, que hace que la simulación sea más sencilla de en ende . El sis ema unciona con
comandos en línea, pe o ambién puede usa se Py hon pa a con ola los obo s en el simulado . Un
componen e so p enden e de Mo se es su habilidad pa a modela la in e acción humano/ obo . O o simulado
simila es Gazebo, ambién pa a ROS. Como es el usado en es e abajo, lo desa olla é más adelan e. El
siguien e ejemplo es Webo s, el cual u iliza ODE (Open Dynamics Engine) pa a la de ección de colisiones,
p opo cionando ambién una simulación p ecisa de elocidad, ine cia y icción. Una de las ca ac e ís icas que
han hecho amoso ARS es que unciona exclusi amen e con Py hon y su acilidad pa a gene a la
documen ación. O o simulado muy amoso es V-Rep no sólo po que los con olado es pueden se esc i os en
casi odos los lenguajes de p og amación exis en es, sino po la e sa ilidad que le p opo ciona su dis ibuida
a qui ec u a de con ol: cada obje o puede se con olado independien emen e po un sc ip in e no, un plugin,
un nodo de ROS, un clien e emo o API o el clien e. Es empleado pa a desa ollo ápido de algo i mo y
simulaciones de cadenas de mon aje.
11
11
Ope ación de emachado median e obo manipulado indus ial basado en ROS
3 MODELADO, CONTROL Y SIMULACIÓN DE ROBOTS
n el campo de la obó ica, pa a el modelado, simulación y con ol de obo s es necesa io dispone de
unas he amien as que nos pe mi an conoce su posición, o ien ación, elocidad, con igu ación en e
o os en el en o no de abajo. Pa a ello es necesa io conoce p e iamen e como ep esen a la posición y
la o ien ación en el espacio, es ablece un modelo an o cinemá ico como dinámico que nos pe mi a elaciona
alo es a icula es-posición, elocidades ca esiana- elocidades a icula es, pa aplicado-a iculación, e c.
Pueden encon a es a in o mación de o ma más ex ensa en los lib os de obó ica gene ales:
- Olle o Ba u one, (2007). Robó ica: Manipulado es y obo s mó iles.
- Lewis, F ank L., Abdallah, C. T. Dawson, D. M. (1993). Con ol o obo manipula o s.
3.1 Rep esen ación de posición y o ien ación
Sabemos que la posición de un pun o en el espacio euclídeo idimensional iene de e minada po es
can idades, que llamamos sus coo denadas, y decimos que es án exp esadas en algún sis ema de e e encia,
o mado po es ejes, usualmen e ec ilíneos.
En lo sucesi o usa emos exclusi amen e sis emas de e e encia ec ilíneos, o ogonales (es deci , con sus es
ejes pe pendicula es dos a dos), no malizados (es deci , las longi udes de los ec o es básicos de cada eje son
iguales) y dex ógi os (el e ce eje es p oduc o ec o ial de los o os dos).
Usa emos, pues, simplemen e el é mino "sis ema" pa a e e i nos a sis emas o ono males.
Figu a 8: Tipos de sis emas de e e encia.
Las coo denadas de un pun o, deno adas po (x; y; z), son las p oyecciones de dicho pun o pe pendicula men e
a cada eje, o, equi alen emen e, las componen es del ec o que lo une al o igen de coo denadas. En luga de
usa es as, nos se á más con enien e el uso de las llamadas coo denadas homogéneas, en la o ma:
donde
siendo w una can idad a bi a ia, que se suele oma como 1. Si, como esul ado de algún cálculo, w uese
E
Modelado, con ol y simulación de obo s
12
12
dis in o de 1, las coo denadas usuales se econs uyen simplemen e di idiendo las es p ime as coo denadas
homogéneas en e es a cua a.
La aslación de un pun o x, y, z po un ec o es un pun o x’, y’, z’ al que:
Pe o ambién como el p oduc o de una ma iz po un ec o homogéneo, en la o ma:
Es o iene la en aja de que, si: en onces
Donde se puede calcula la in e sa, que esul a se :
lo cual es consis en e con el hecho de que x es a asladado po un ec o – espec o a x'.
Respec o a la o ación al ededo de un eje, en el caso bidimensional, se es á o ando con espec o a un eje z
pe pendicula al plano de la igu a.
Figu a 9: Ro ación de sis ema de e e encia espec o al eje Z
Llamando i, j a los ec o es básicos del sis ema o iginal, e i' y j' a los del sis ema gi ado, se iene que
13
13
Ope ación de emachado median e obo manipulado indus ial basado en ROS
Es deci , que
Igualando componen e a componen e, esc ibimos la ma iz como
Si gene alizamos a es dimensiones, como la coo denada z no a ia y la cua a coo denada homogénea sigue
siendo 1, enemos
Pa a halla la ans o mación in e sa bas a e que desde el pun o de is a de R', R es a o ado un ángulo -θ ,
luego podemos a i ma que
Es a ope ación es más simple que in e i la ma iz, aunque po supues o, equi alen e.
En gene al, si hubié amos o ado al ededo de o o de los ejes básicos, se puede e que
Cabe des aca el cambio de signo en la o ación al ededo del eje y, debido a que, si el eje al ededo del cual
o amos nos apun a, los o os dos o man un ángulo de 90o en el caso de x y z, pe o de -90o en el caso de y. Se
pueden aplica a un pun o an as ans o maciones sucesi as ( o aciones y aslaciones) como se quie a. La
ope ación esul an e end ía dada po una ma iz que se ía p oduc o de las ma ices de cada ope ación,
aplicadas en el o den co ec o, dado que el p oduc o de ma ices no es conmu a i o.
Se pone más a la de echa la p ime a ans o mación que se aplique, siendo exp esado como
Signi ica que se aplica al pun o X la o ación 1, seguida de la o ación 2, seguida de la aslación 1, luego la
o ación 3 y po úl imo la aslación 2.
Veamos aho a cual se ía la ma iz de o ación espec o a un eje cualquie a. Sea un eje que pasa po el o igen
de inido po un ec o uni a io al ededo del cual gi a emos un ángulo θ.
ROS y he amien as de desa ollo
20
20
Las á eas que inclui án las aplicaciones de los paque es de ROS son:
 Pe cepción
 Iden i icación de Obje os
 Segmen ación y econocimien o
 Reconocimien o acial
 Reconocimien o de ges os
 Seguimien o de obje os
 Egomoción
 Comp ensión de mo imien o
 Es uc u a de mo imien os (SFM)
 Visión es é eo: pe cepción de p o undidad median e el uso de dos cáma as
 Mo imien os
 Robo s mó iles
 Con ol
 Plani icación
 Aga e de obje os
4.2 Diseño modula y dis ibu i o
Ros ue diseñado pa a se lo más dis ibu i o y modula posible, de modo que los usua ios pueden u iliza
ROS an o como deseen. Su modula idad le pe mi e selecciona y elegi qué pa es son ú iles y qué pa es
p e ie e implemen a uno mismo.
La na u aliza dis ibu i a de ROS ambién omen a una g an comunidad de paque es con ibuidas po usua ios
que añaden un g an alo a la pa e supe io del núcleo del sis ema ROS. En el úl imo ecuen o se con a on
más de 3000 paque es en el ecosis ema ROS, y eso que solo son los paque es que la gen e ha hecho público.
Es os paque es a ían en la idelidad, cub iendo desde p uebas de concep os de implemen ación de nue os
algo i mos has a d i e s de al a capacidad y calidad indus ial. La comunidad de usua ios de ROS cons uye
sob e la pa e supe io de una in aes uc u a común pa a p opo ciona un pun o de in eg ación que o ece
acceso a con olado es de ha dwa e, obo s gené icos, he amien as de desa ollo, biblio ecas ex e nas ú iles, y
mucho más.
4.3 His o ia
ROS es un la go p oyec o con muchos an epasados y colabo ado es. La necesidad de un ma co de
colabo ación abie o ue eque ido po muchas pe sonas en la comunidad de in es igación obó ica, y muchos
p oyec os se han c eado pa a alcanza es e obje i o.
Va ios es ue zos en la Uni e sidad de S an o d a mi ad de los años 2000 en ol iendo in eg a i amen e an o
STan o d Al Robo (STAIR) como el p og ama Pe sonal Robo s (PR) c ea on p o o ipos in e nos de sis emas
de so wa e lexibles y dinámicos des inado al uso obó ico. En 2007, Willow Ga age, una incubado a ce cana
de isiona ios obó icos, apo ó impo an es ecu sos pa a amplia es os concep os mucho más allá y c ea
implemen aciones bien p obadas. Es e es ue zo se ió impulsado po un sin ín de in es igado es que
con ibuye on con su iempo y expe iencia pa a las ideas p incipales de ROS y sus paque es de so wa e
undamen ales. En odo momen o, el so wa e ue desa ollado en abie o usando una licencia de código
abie o (BSD open-sou ce), y poco a poco se ha con e ido en una pla a o ma ampliamen e u ilizada en la
comunidad de in es igación obó ica.

21
21
Ope ación de emachado median e obo manipulado indus ial basado en ROS
Desde sus inicios, ROS se desa olló en múl iples ins i uciones y pa a múl iples obo s, incluyendo muchas de
las ins i uciones que ecibie on los obo s PR2 de Willow Ga age. A pesa de que hab ía sido mucho más ácil
pa a odos los con ibuyen es pone su código en los mismos se ido es, en los úl imos años, el modelo
“ ede ado” se ha con e ido en una de las g andes o alezas del ecosis ema ROS. Cualquie g upo puede
comenza su p opio eposi o io de código ROS en sus p opios se ido es, y man ene la p opiedad y con ol
po comple o, sin necesidad de pe miso de nadie. Si deciden pone su eposi o io a disposición del público,
pueden ecibi el econocimien o y c édi o que se me ecen sus log os, y bene icia se de la in o mación écnica
especí ica y mejo as como odos los p oyec os de so wa e de código abie o.
4.4 Comunidad ac i a y colabo a i a
En los úl imos años, ROS ha c ecido pa a con a con una g an comunidad de usua ios en odo el mundo.
His ó icamen e, la mayo ía de los usua ios se encon aban en los labo a o ios de in es igación, pa a cada ez
más es amos iendo una adopción en el sec o come cial, en pa icula en la indus ia y se icios obó icos.
La comunidad ROS es muy ac i a. Según las úl imas mediciones, la comunidad ROS cuen a con más de 1500
en la lis a de ROS dis ibu i a, más de 3300 usua ios en la wiki de documen ación colabo a i a y unos 5700
usua ios en o o de la página web. La wiki cuen a con más de 22000 páginas y más de 30 ediciones dia ias. En
el o o ienen luga unas 13000 p egun as hechas has a la echa, con una asa de espues a del 70%.
ROS no solo o ece po sí mismo un g an alo a la mayo ía de los p oyec os, sino que ambién ep esen a una
opo unidad pa a es ablece con ac os y colabo a con los expe os en obó ica mundiales que o man pa e de
la comunidad de ROS. Una de las iloso ías básicas de ROS es compa i el desa ollo de componen es
comunes.
4.5 Concep os básicos
Los concep os undamen ales de la aplicación de ROS son nodos, mensajes, opics y se icios.
Los nodos son p ocesos ejecu ables. ROS es diseñado pa a se modula en una escala de g ano ino: Un
sis ema ípicamen e se compone de muchos nodos. En es e con ex o, el é mino “nodo” es in e cambiable po
“módulo”. El uso del é mino “nodo” su ge de isualizaciones de ROS basados en sis emas iempo de
ejecución: cuando muchos nodos se es án ejecu ando, se ealizan las comunicaciones “pee - o-pee ”, es deci ,
el enlace en e los dis in os nodos son es as comunicaciones.
Los nodos se comunican en e sí median e paso de mensajes. Un mensaje es una es uc u a de da os que
pueden se dis in os ipos (en e os, pun o lo an es, booleanos, e c) apoyados po ipos p imi i os y cons an es.
Los mensajes pueden es a compues os de o os mensajes, y ma ices de o os mensajes, anidados de o ma
p o unda.
En nodo en ía un mensaje median e su publicación en un “ opic”. Un opic puede in e p e a se como un
buzón de mensajes dónde llega siemp e un mismo ipo o es uc u a de da o, de esa o ma, si un nodo es á
in e esado en un de e minado ipo de da o, se susc ibi á en el opic co espondien e. Puede habe a ios
publicados y susc ip o es pa a un solo opic, como un nodo puede publica y es a susc i os a a ios opics a la
ez.
ROS y he amien as de desa ollo
22
22
Figu a 12: Sis ema básico ROS.
Aunque el modelo de publicación-susc ipción basado en opics es lexible, no es ap opiado pa a ansacciones
sínc onas, que pueden pe mi i el diseño de algunos nodos. Pa a es o, ROS nos acili a los “se icios”, que se
de ine con un nomb e de cadena de ca ac e es y dos mensajes p opo cionados: Uno de solici ud y o o de
espues a. Es o es análogo a los se icios web, que son de inidos po los URI y ienen ipos de documen os de
solici ud y espues a bien de inidos. A di e encia de los opics, solo un nodo puede eque i un se icio,
mien as que se á espondido po o o.
Finalmen e ambién hay que des aca la capacidad de lexibilidad a la ho a de la p og amación, ya que pe mi e
usa a ios lenguajes de p og amación como C++, Py hon, Lisp u Oc a e, pe mi iendo además, que un sis ema
es é compues o po nodos esc i os en dis in o lenguajes, aumen ado su lexibilidad y modula idad.
Figu a 13: Sis ema Robó ico de isión en ROS.
23
23
Ope ación de emachado median e obo manipulado indus ial basado en ROS
4.6 He amien as ROS
4.6.1 Gazebo
La simulación obó ica es una he amien a esencial en el ool-box obó ico. Un simulado bien diseñado
pe mi e p oba ápidamen e algo i mos, obo s diseñados y ealiza p uebas de eg esión u ilizando escena ios
ealis as. Gazebo [4] o ece la posibilidad de simula con p ecisión y e iciencia poblaciones de obo s en
en o nos in e io es y ex e io es complejos. Gene a an o la ealimen ación de los senso es como las
in e acciones ísicas en e obje os a ándolos como cue pos ígidos, a a és de g á icos de al a calidad e
in e aces p og amables. Además de ene una comunidad ac i a.
Figu a 14: Gazebo.
4.6.2 RVIZ
RVIZ (ROS Visualiza ion) [5] es un isualizado 3D que pe mi e la isualización de da os de senso e
in o mación de es ado del sis ema de ROS. Usando RVIZ, se puede e la con igu ación ac ual de Bax e en
un modelo i ual del obo , además de las ep esen aciones en i o de los alo es de senso publicados en los
opics de ROS, incluyendo da os de cáma a, mediciones de senso es de dis ancia in a ojos, da os de sona ,
e c. Algunos de los da os más impo an es que podemos isualiza se encuen an:
 Robo Model: Mues a una ep esen ación isual de un obo en la posición dada (de inida po la
ans o mación TF del momen o).
 TF: Desc i a a con inuación, nos pe mi e isualiza los dis in os ejes de e e encia que con o ma
nues o en o no.
 Poin Cloud; Mues a los da os de una nube de pun os, con igu adas po di e en es opciones
disponibles.
ROS y he amien as de desa ollo
24
24
 Came a: C ea una nue a en ana con la pe spec i a de una cáma a, y supe pone la imagen en la pa e
supe io de la misma.
 Lase Scan: Mues a los da os de una explo ación po láse , con di e en es opciones pa a los modos de
ep esen ación, acumulación, e c.
 O as he amien as como Axes, E o , G id, Map, Ma ke s, Pa h, Pose, Pose A ay…e c.
Figu a 15: RVIZ.
4.6.3 TF
El sis ema TF de ROS [6] pe mi e coo dina múl iples sis emas de e e encias y man ene la elación en e
ellos en una es uc u a de á bol. TF se dis ibuye de mane a que la in o mación ace ca de la coo dinación de
odos los sis emas de e e encia del sis ema es á disponibles pa a odos los nodos de la ed de ROS. Además
del acceso a es a in o mación, nos pe mi e ecupe a ans o maciones en e ellos, ans o ma pun os, ec o es
y o as en idades en e dos ejes de e e encia. Algunas aplicaciones son:
 _moni o : Imp ime in o mación sob e el ac ual á bol de coo denadas po consola.
 _echo: Imp ime in o mación sob e la ans o mación ela i a en e dos ejes de e e encia.
 s a ic_ ans o m_publishe : Publica un nue o eje de e e encia a a és de una ans o mación es á ica
de un eje ya exis en e.
 iew_ ames: Gene a un PDF con nues o á bol de .
25
25
Ope ación de emachado median e obo manipulado indus ial basado en ROS
Figu a 16: TF ames en RVIZ.
De aquí en adelan e, a los ejes de e e encia ep esen ados po TF los llama emos “TF ames”.
Es a he amien a de ROS nos pe mi i á ob ene posiciones ela i as en e TF ames, que nos si e po
ejemplo, pa a calcula e o es de posición en e ellos y que usa emos pa a es ima la posición del aguje o
deseado pa a emacha . También pod emos ealiza ans o maciones en e es os ma cos de e e encia pa a las
ope aciones de cinemá ica in e sa o di ec a del modelo cinemá ico, sin ene que usa ma ices de
ans o mación, como po ejemplo en el caso de aplica la cinemá ica in e sa, no usa emos el obo has a el
úl imo eslabón como iene de inido en el a chi o URDF cp, sino que enemos que e ocede es eslabones
hacia a ás en el modelo cinemá ico has a el eslabón ee_link (end e ec o link).
4.6.4 URDF
URDF (Uni ied Robo Desc ip ion Fo ma ) [7] se a a de una he amien a de ROS que consis e en un
documen o o ma o XML que nos pe mi e ep esen a el modelo de un obo .
Los a chi os URDF se es uc u an básicamen e en o ma de á bol, con dos componen es p incipales pa a
cons ui el obo : Links (eslabones) y Join s (a iculaciones):
 Links:
Cons i uyen las componen es ísicas del obo , es deci , cada uno de los eslabones po lo que es á o mada
nues a cadena cinemá ica, ambién compone la pa e isual donde debemos in oduci nues os a chi os
.DAE pa a su isualización pos e io .
En el apa ado isual -> geome y -> mesh podemos in oduci nues o modelo .dae diseñado p e iamen e.
También podemos in oduci de o ma gené ica obje os geomé icos simples como cilind os, es e as, cubos,
e c… de iniendo odos sus pa áme os (a is a, adio…).
 Join s:
Median e la de inición de los “join s” es ablecemos la elación en e los dis in os links o eslabones que
componen nues o obo , an o las posiciones ela i as en e ellos, como la a iculación que hab á en e ellos.

ROS y he amien as de desa ollo
26
26
Figu a 17: Esquema URDF.
La o ganización EuRoC nos acili a el a chi o URDF de nues o obo UR5, que u iliza emos pa a ealiza las
ope aciones de cinemá ica in e sa y di ec a usando la lib e ía u _kin_py , que de allamos a con inuación, ya
que pa a ealiza ope aciones de cinemá ica necesi amos conoce el modelo cinemá ico del obo , y es e
a chi o URDF le se i á a nues a lib e ía odo lo necesa io sob e eslabones y a iculaciones de nues o
manipulado . Pa a una in o mación más comple a
4.6.5 UR_KIN_PY
UR_KIN_PY [8] se a a de un paque e exis en e en el eposi o io de códigos de ROS donde encon amos las
unciones de cinemá ica di ec a e in e sa de las con igu aciones UR5 y UR10, que necesi a emos pa a la
gene ación de ayec o ias pa a mo e nues o obo .
4.6.6 Me cu ial
Cie amen e Me cu ial no pe enece al sis ema ROS, es un so wa e p opio y o almen e ue a, es un sis ema
de con ol de e siones mul ipla a o ma, un so wa e lib e pa a la línea de comandos, implemen ado
p incipalmen e en Py hon pe o que incluye una implemen ación bina ia en C. Esc i o o igina iamen e pa a
unciona en GNC/Linux. Con una in e az web in eg ada, sus me as de desa ollo son el endimien o y
escalabilidad, un desa ollo dis ibuido sin necesidad de se ido , ges ión obus a de a chi os an o de ex o
como bina ios y capacidades a anzadas de ami icación e in eg ación man eniendo la sencillez concep ual.
Las di e en es acilidades que nos o ece Me cu ial son:
 Moni o iza los a chi os añadidos
 Reposi o io compa ido
 Gua da cambios localmen e de o ma empo al abajando en di e en es clones
 Copia y mo e a chi os
 Re isa his o ial
 A egla e o es en e siones an e io es
 Mezcla o usión de dos clones di e en es o iginales de un mismo código
27
27
Ope ación de emachado median e obo manipulado indus ial basado en ROS
Figu a 18: Me cu ial.
En o no de Simulación EuRoC
28
28
5 ENTORNO DE SIMULACIÓN EUROC
omo ya se desa olló an e io men e, mi p oyec o se encon aba en la p ima ase del Challenge 1 de
EuRoC, dónde se plan eaban 4 a eas en dos escena ios dis in os, las a eas 1 y 2 en uno, y la 3 y 4 en
o o. A con inuación mos a emos una desc ipción del en o no en el que eside el abajo, escena io
uno, pa a la a ea 2.
5.1 En o no de simulación
El en o no pa a las a eas 1 y 2 consis e en á ea de abajo donde encon amos p incipalmen e una mesa de
abajo, con un obo indus ial y un obje o, un ope a io mo iéndose de o ma o almen e alea o ia (en nues a
a ea 2) po nues o escena io, y dos cáma as 3D.
Sob e la mesa de abajo encon amos un obo indus ial de 6 g ados de libe ad (con igu ación UR5)
equipado en su e ec o inal con una he amien a de emachado. También iene luga un obje o, que
llama emos de aquí en adelan e “pieza de abajo” en el á ea de abajo de nues o obo . Es a pieza de abajo
iene a ios aguje os pa a ealiza ope aciones de emachado sob e ellos.
Como ya hemos mencionado, ambién enemos dos cáma as 3D. La p ime a en oca el escena io de o ma
global pudiendo e la escena con su o alidad, y la segunda en oca di ec amen e la mesa de abajo. Cabe
menciona que en nues a a ea 2 no usa emos es as cáma as, ya que és as es án des inadas pa a su uso en la
a ea 1, cen ada en la pe cepción.
Figu a 19: En o no de simulación.
C
29
29
Ope ación de emachado median e obo manipulado indus ial basado en ROS
5.1.1 Sis ema obó ico
Como ya iniciamos an es, pa a nues a a ea enemos un obo manipulado de 6 g ados de libe ad, cuya
con igu ación es la conocida y global UR5. La o ganización EuRoC nos da el acceso a su desc ipción
cinemá ica y dinámica a a és de su a chi o URDF (Uni ied Robo Desc ip ion Fo ma ).
Figu a 20: Robo UR5 con sus ma cos de e e encia.
En su e ec o inal, el obo es á equipado con una he amien a de emachado, desc i a con la siguien e igu a:
Figu a 21: Vis as de alladas del e ec o inal del obo .
Es a pa e cilínd ica de 5mm de diáme o de la he amien a iene que se inse ada en los aguje os de la pieza
de abajo, como de alla emos en la desc ipción de la a ea, los TF más ele an es se mues an en la imagen
an e io . Debemos des aca que nues a guía de la posición del obo se á el úl imo ame llamado cp.
Algo i mo de plani icación y con ol de p oceso de emachado
36
36
 GOING TO HOLE GUESS -> MOVING INTO HOLE: De
36
u om ansición au oma ic median e
a iable booleana
 MOVING INTO HOLE: Donde nues o obo se in oduci á en el aguje o has a 6 mm con la
he amien a de emachado cilínd ica.
Figu a 30: Remachado de la pieza.
 MOVING INTO HOLE -> RIVETING: T ansición
36
u oma ic po a iable booleana.
 RIVETING: Una ez in oducida la he amien a en el aguje o, llama emos al se icio ROS
igge _ i e , que simula el p oceso de emachado, y pod emos con abiliza un emachado con éxi o.
 RIVETING -> OUT FROM HOLE: T ansición que se ac i e as la llamada al se icio igge _ i e .
 OUT FROM HOLE: Mo imien o del obo de salida del aguje o has a la posición de enca e inicial.
 OUT FROM HOLE -> GOING TO RIVET HOME: Si odo se ha ealizado co ec amen e, nues o
opic a ge _ ame end á un alo de i e _home as comp oba es o, se ac i a á la ansición,
comenzando el p oceso de nue o.
Es a máquina de es ado es á implemen ada en código de p og amación que se adjun a en el Anexo B :“S a e
Machine Code”. Es e ejecu able es á implemen ado en el nodo clien e de la a ea, donde debe i la solución a
nues o p oblema.
6.2 Mo e el obo
Como expuse an e io men e, pa imos de la base de opciones de acciones en ROS que nos p opo ciona la
o ganización del concu so:
 simula ion_u 5/join _ ajec o y_con olle / ollow_join _ ajec o y: Nos pe mi e mo e el obo en el
espacio a icula .
 simula ion_in e ace_u 5/ca esian_posi ion:ipa325_msgs.msg.Robo Mo emen Ac ion : Nos pe mi e
mo e el obo en el espacio ca esiano.
T as una se ie de p uebas u ilizando ambas opciones, inalmen e me decan é po mo e nues o obo en el
espacio a icula , ya que la posibilidad del espacio ca esiano nos o ecía a eces p oblemas de cinemá ica
in e sa, p oduciendo mo imien os b uscos del obo , golpeando la pieza de abajo, cosa que debemos e i a .

37
37
Ope ación de emachado median e obo manipulado indus ial basado en ROS
T a ándose de una acción de ROS, debemos sabe que se a a de una comunicación bidi eccional, es deci ,
debemos hace una pe ición de ealiza la acción, es a pe ición se ealiza median e una llamada de código.
Una ez o mulada la pe ición, si és a se ha ealizado de o ma co ec a, se ejecu a á la acción, y nos en ia á
un mensaje de uel a con el esul ado: éxi o, e o o inde inido. Puede en ende se al que así:
Figu a 31: Comunicación en e acciones ROS.
Pa a ealiza nues a pe ición a la acción, debemos comple a una es uc u a de mensaje ROS, del ipo
con ol_msgs.msg.FollowJoin T ajec o yGoal, donde debemos comple a los siguien es campos de la
es uc u a con ol_msgs.msg.FollowJoin T ajec o yGoal. ajec o y:
 con ol_msgs.msg.FollowJoin T ajec o yGoal. ajec o y.join _names: Se a a de un ec o de
ca ac e es compues o po las dis in as a iculaciones de nues o obo , en nues o caso , as
de ini nues a a iable de mensaje:
goal_ ajec o y = con ol_msgs.msg.FollowJoin T ajec o yGoal();
goal_ ajec o y. ajec o y.join _names = [‘shoulde _pan_join ’, ‘shoulde _li _join ’,
‘elbow_join ‘w is _1_join ’, ‘w is _2_join ’, ‘w is _3_join ’];
 con ol_msgs.msg.FollowJoin T ajec o yGoal. ajec o y.poin s: Donde de iníamos la ayec o ia
de pun os de nues o mo imien o en el espacio a icula , den o de poin s debíamos de ini dos
subcampos:
- con ol_msgs.msg.FollowJoin T ajec o yGoal. ajec o y.poin s.posi ions: Valo a icula de
nues a ayec o ia, en nues o caso, e a álido en ia le simplemen e posición inicial y inal
- con ol_msgs.msg.FollowJoin T ajec o yGoal. ajec o y.poin s. ime_ om_s a .secs:
De iniendo así la elocidad del mo imien o, es deci , el ins an e en el que que íamos que
nues o obo es u iese en cada posición.
Una ez comple os es os campos ealizábamos la pe ición de acción y mo íamos el obo de o ma
sa is ac o ia.
Pe o an es de la pe ición había que ealiza un abajo p e io, ya que inicialmen e odas nues as
posiciones des ino las ob eníamos median e la he amien a TF en alo es ca esianos, po lo que hay que
aplica unciones de cinemá ica in e sa pa a pode con inua con el subp oblema.
Es o cons i uyó uno de los p incipales p oblemas pa a la esolución del p oyec o, ya que inicialmen e se
decidió usa la lib e ía de ROS KDL, la más gene alizada y usada pa a la esolución de p oblemas
cinemá icos an o di ec os como in e sos, pe o en nues o caso nunca dio esul ados sa is ac o ios, ya que
ealizaba los mo imien os con poca p ecisión, po lo que no nos e a álido debido a que es os e o es de
posición daban luga a con ac o obo -pieza de abajo, algo que debíamos e i a a oda cos a.
Algo i mo de plani icación y con ol de p oceso de emachado
38
38
Finalmen e, se decidió u iliza la lib e ía u _kin_py (explicada en capí ulos an e io es) pa a es e
p oblema, ealizando las unciones de cinemá ica in e sa de mane a sa is ac o ia.
Todo es o se implemen ó en la unción “mo e_ o” que usamos en la máquina de es ado, donde la unción
ecibe la posición des ino, y se enca ga á de ealiza odo lo necesa io desc i o an e io men e.
En esumen, pa a el mo imien o del obo e a necesa io:
1. Conoce , median e la he amien a ROS TF, la posición inicial y inal en coo denadas ca esianas
de nues o mo imien o deseado.
2. T ans o ma es os alo es ca esianos en a icula es aplicando cinemá ica in e sa.
3. Rellena la es uc u a de mensaje de la pe ición de acción ROS.
4. Pe ición de acción.
5. Espe a y ob ene esul ados.
Siguiendo es e esquema, se consiguió de mane a sa is ac o ia mo e nues o obo , ob eniendo siemp e
buenos esul ados y esol iendo nues os p oblemas iniciales con la cinemá ica in e sa con la lib e ía que
inalmen e usamos.
6.3 De ec a aguje o de emachado
Du an e la ejecución de la a ea, nues o aguje o a emacha de o ma cíclica no se publica de o ma
explíci a, sino a a és de un TF ame llamado hole_guess que se publica au omá icamen e en cada ciclo,
y que es una ap oximación del e dade o aguje o de la pieza a emacha , y que debemos iden i ica y
localiza noso os desde la posición inicial, que es uno de los obje i os de nues a a ea.
Nues a pieza de abajo es á compues a po un o al de 33 aguje os, dis ibuidos en es ca as o
supe icies dis in as de la pieza. Pa a localiza nues o aguje o a emacha , decidí usa la he amien a de
ROS TF, que nos pe mi e ob ene posiciones ela i as en e dis in os TF ame.
Nues a unción “de ec ing_hole_guess”, de o ma gene al, ealiza un bucle eco iendo los 33 aguje os
de la pieza, compa ando cada uno de sus TF ames con el TF ame hole_guess, buscando así, el aguje o
que minimice el alo absolu o de la compa ación en posición y o ien ación. És a nos de ol e ía una
cadena de ca ac e es indicando de qué aguje o se a a, pa a ealiza la ope ación de emachado sob e él
en ese ciclo.
Fo I in x ange(33):
h_i = “h”+s (i)
hi_pos = sel .ge _ _ ans o m(“base_link”, h_i)
pos_di = numpy.a ay(hi_pos)-numpy.a ay(hole_guess_pos)
esul _i = numpy.linalg.no m(pos_di )
i esul _i< esul :
h_ esul =”h”+s (i)
hole_numb = i
i hole_numb == -1:
ospy.login o(“Hole_guess no de ec ed”)
q_home = -1
else:
ospy.login o(“Hole_guess de ec ed: “)
ospy.login o(h_ esul )
39
39
Ope ación de emachado median e obo manipulado indus ial basado en ROS
Además, con is as a la e iciencia de la ejecución de nues o algo i mo, se que según la ca a en la cual se
encon ase nues o aguje o (conocemos cuál aguje o pe enece a cuál ca a), ealiza íamos una ayec o ia
di e en e con nues o obo , es deci , enca a íamos a la pieza de o ma di e en e, con el in de ealiza lo
con mayo elocidad y elimina posibles e o es de cinemá ica in e sa en nues a ayec o ia. Po lo que
en nues a unción, además de localiza el aguje o a emacha , nos de uel e en alo es a icula es las
dis in as posiciones de nues a ayec o ia pa a ealiza la ope ación en unción de la ca a de la pieza
donde se encuen e el aguje o. A con inuación mos amos imágenes sob e los “enca amien os” de nues o
obo hacia la pieza:
Figu a 32: Posición in e media de emachado pa a ca a supe io .
Figu a 33: Posición in e media de emachado pa a una ca a la e al.
Algo i mo de plani icación y con ol de p oceso de emachado
40
40
6.4 Ejemplo de ejecución
A con inuación mos a emos una se ie de imágenes que nos isualiza un ejemplo de ciclo de emachado
de nues o obo :
Figu a 34: Posición inicial “ i e _home”. Figu a 35: Robo de ec ando “hole_guess”.
Figu a 36: Robo enca ando pieza Figu a 37: Inse ción de la he amien a de emachado
A con inuación, ol e íamos a la posición inicial y ol e íamos a busca de nue o el siguien e
“hole_guess”.
41
41
Ope ación de emachado median e obo manipulado indus ial basado en ROS
7 RESULTADOS Y CONCLUSIONES
on la solución p opues a al p oblema expues a en el capí ulo an e io , as la e aluación de la
o ganización EuRoC, los esul ados de la a ea ue on los siguien es as su ejecución:
SUBTAREA
MEDIDA
RESULTADO
PUNTUACIÓN
OBTENIDA
Localización de
aguje os a emacha
Media de e o
0
10
Remacha aguje os
Núme o de aguje os
emachados
co ec amen e
9
2
Colisiones obo -pieza
de abajo
Núme o de colisiones
obo -pieza de abajo
0
10
Tabla 6: Resul ados de e aluación.
Den o de la a ea del p oyec o, ob u imos un o al de 22/30 pun os, y englobando odas las a eas de la
p ime a ase del concu so, ob u imos un o al de 43 pun os, 21 de la a ea 1, 22 de la p esen e, y 0 pun os an o
pa a la a ea 3 y 4, ya que inalmen e no p esen amos ninguna solución pa a es a pa e de la p ime a ase del
Challenge 1. Puede e la a ea 2 en el o ma o digi al aquí
Es a pun uación nos pe mi ió pa a clasi ica nos pa a la segunda ase del Challenge 1, ob eniendo un oc a o
pues o de los 32 pa icipan es ( eco damos que solo se clasi icaban los 15 p ime os clasi icados), que ya quedó
en manos del Cen o Tecnológico CATEC y en el que no pa icipé.
A la is a de los esul ados en la e aluación, nues o pun o lojo ue la apidez con la que ealizábamos cada
ciclo de emachado, pe o o maba pa e de la es a egia de op imización de ob ención de pun os, ya que como
desa ollamos en el capí ulo an e io , en el desa ollo, no ue posible da le mayo elocidad a la ayec o ia del
obo de mane a es able sin ob ene colisiones en e el obo y la pieza de abajo, po lo que se decidió
asegu a los 10 pun os e e en e al núme o de colisiones en e al núme o de emachado de aguje os, ya que
aunque aumen ásemos la elocidad del obo en sus mo imien os y u iésemos algunas colisiones (que es o
nos da ía di ec amen e 0 pun os en la sub a ea de colisiones) hab ía que ealiza un o al de 15 o más
emachados de o ma co ec a pa a ob ene los 10 pun os de esa sub a ea, cosa que no se consiguió, po lo que
se op ó po la es a egia p esen e.
Po o a pa e, median e la he amien a TF de ROS conseguimos ob ene de o ma exi osa odas las
localizaciones de los aguje os que había que emacha , eniendo un e o nulo en cada uno de ellos.
Si hubiésemos conseguido una he amien a más e icien e pa a cálculo de cinemá ica in e sa, ya que con la que
disponíamos debíamos ealiza ayec o ias p ede inidas que nos hacían mo e nos cíclicamen e de o ma más
len a, hubiésemos podido plani ica ayec o ias más di ec as en e posición inicial y posición de emachado y
gana iempo, pudiendo ealiza un mayo núme o de emachados en el iempo de ejecución dado.
C

Anexo A: A anque del Sis ema
42
42
ANEXO A: ARRANQUE DEL SISTEMA
a a inicia nues a in aes uc u a p opo cionada po EuRoC y la esolución al p oblema p opues o,
debemos ins ala p e iamen e los paque es desde el se ido de la o ganización, una ez ins alados
debemos dis ingui dos ex emos de comunicación:
 Nodo se ido : En su ejecución inicializa odo lo necesa io pa a comenza la a ea, es deci , el en o no
de simulación en gazebo (los modelos isuales de odos los elemen os del escena io), opics, se icios,
TF ames… e c. Una ez inicializado odo, se man iene a la espe a de la ejecución del nodo clien e,
que esol e á la a ea.
 Nodo clien e: Con iene el ejecu able pa a la esolución de la a ea, es deci , la llamada a la unción de
la máquina de es ado, una ez lo a ancamos, esuel e la a ea au omá icamen e.
Figu a 38: Esquema de comunicación se ido -clien e.
Pa a a anca el nodo se ido debemos hace un oslaunch en la consola Linux o sis ema ope a i o usado:
oslaunch ipa325_eu oc_sim ack1.launch eu oc_ es _com_se e :=False
Es e launch con iene el nodo se ido llamado ipa325_com_se e .py, una ez lanzado el launch, se nos
inicializa á oda la es uc u a y pe manece á a la espe a clien e, que ambién se llama po medio de oslaunch:
oslaunch ipa325_com_clien ipa325_com_clien .launch
De Nue o pe manece á en s adby la ejecución espe ando que le indiquemos que a ea amos a esol e , es o
se lo indica emos a a és de un se icio ROS osse ice, que iden i ica á qué a ea que emos esol e e i á a
la pa e del código del nodo clien e ela i a a la a ea seleccionada:
osse ice call sol e askid:’2’
Una ez iniciado el se icio comenza á au omá icamen e la esolución de la a ea, que pod emos segui sus
esul ados a a és de Gazebo o RVIZ de o ma isual, o a a és de los mensajes que an imp imiendo los
nodos po pan alla en la consola
P
43
43
Ope ación de emachado median e obo manipulado indus ial basado en ROS
ANEXO B: STATE MACHINE CODE
om enum impo Enum
impo ospy
impo con ol_msgs
impo s d_s s
impo s d_msgs
impo s d_msgs.msg
om ca ec_ 12_ ajec o ies impo ajec o y_gene a ion, obo _in e ace
om ca ec_ 12_ ajec o ies. obo _in e ace impo Robo In e ace
om ca ec_ 12_ ajec o ies impo ajec o y_gene a ion;
om ca ec_ 12_ ajec o ies. ajec o y_gene a ion impo T ajec o yGene a o
om h l_geom.pose_con e e impo PoseCon
impo
impo numpy
impo ma h
impo pylab
impo sys
impo ime
om ca ec_ 12_ ajec o ies.posi ion_manage impo Posi ionManage
class S a e(Enum):
STARTING = 1
GOING_RIVET_HOME = 2
DETECTING_HOLE_POSE = 3
GOING_HOLE_GUESS = 4
MOVING_INTO_HOLE = 5
RIVETING = 6
MOVING_OUT_FROM_HOLE = 7
class D illingWo k low:
de __ini __(sel ):
sel .cu en _s a e = S a e.STARTING
sel . obo = Robo In e ace(connec _se e =T ue)
sel . ajec o y_gene a o =T ajec o yGene a o ()
sel .coun _ i e _success=0
#--------------------------------------------------------------------
sel .loca ion_pose_aux=None;
sel . i e _aux=None;
sel .globalA =1;
sel .posi ions_manage =Posi ionManage (sel . ajec o y_gene a o );
sel .pm=sel .posi ions_manage ;
sel . i e _home_pos = None
Anexo B: S a e Machine Code
44
44
#Booleans a iables o change s a e
sel .de ec ing_hole_guess_bool = None
sel .going_hole_guess_bool = None
sel .mo ing_in o_hole_bool = None
sel . i e ing_bool = None
sel .mo ing_ou _ om_hol_bool = None
#--------------------------------------------------------------------
sel . i e _home=None;
sel .hole_guess=None;
ospy.Subsc ibe (" a ge _ ame",s d_msgs.msg.S ing,sel .on_ a ge _ ame_message_ ecei ed)
#---- PERIODIC UPDATE FUNCTIONS INSIDE STATES -------------------
de upda e_s a ing_s a e(sel ):
m = ospy.wai _ o _message(' a ge _ ame', s d_msgs.msg.S ing);
a ge _ ame = m.da a;
i a ge _ ame == '/ i e _home':
sel .change_s a e(S a e.GOING_RIVET_HOME)
de upda e_going_ i e _home(sel ):
m = ospy.wai _ o _message(' a ge _ ame', s d_msgs.msg.S ing);
a ge _ ame = m.da a;
i a ge _ ame == '/hole_guess':
sel .change_s a e(S a e.DETECTING_HOLE_POSE)
de upda e_de ec ing_hole_pose(sel ):
i sel .de ec ing_hole_guess_bool:
sel .de ec ing_hole_guess_bool=False
sel .change_s a e(S a e.GOING_HOLE_GUESS)
de upda e_going_hole_guess(sel ):
i sel .going_hole_guess_bool:
sel .going_hole_guess_bool=False
sel .change_s a e(S a e.MOVING_INTO_HOLE)
de upda e_mo ing_in o_hole(sel ):
i sel .mo ing_in o_hole_bool:
sel .mo ing_in o_hole_bool=False
sel .change_s a e(S a e.RIVETING)
de upda e_ i e ing(sel ):
i sel . i e ing_bool:
sel . i e ing_bool=False
sel .change_s a e(S a e.MOVING_OUT_FROM_HOLE)
de upda e_mo ing_ou _ om_hole(sel ):
i sel .mo ing_in o_hole_bool:
sel .mo ing_in o_hole_bool=False
sel .change_s a e(S a e.GOING_RIVET_HOME)
45
45
Ope ación de emachado median e obo manipulado indus ial basado en ROS
#------- TRANSITIONS -----------------------------------------
de change_s a e(sel , new_s a e):
i sel .cu en _s a e == S a e.STARTING and new_s a e==S a e.GOING_RIVET_HOME:
ospy.login o("s a e s a ing");
ospy.login o("s a e going_ i e _home")
sel . i e _home_pos = sel .pm.ge _ _ ans o m("base_link"," i e _home");
sel .mo e_ o(sel . i e _home_pos, " i e _home")
sel . obo .load_ i e ();
eli sel .cu en _s a e == S a e.GOING_RIVET_HOME
and new_s a e==S a e.DETECTING_HOLE_POSE:
ospy.login o("s a e de ec ing hole guess")
sel .loca ion_pose_aux, sel . i e _aux, q_in =sel .pm.de ec ing_hole_guess()
i q_in == -1:
ospy.login o("S a e Machine blocked, ying o de ec ed hole guess again")
sel .change_s a e(S a e.DETECTING_HOLE_POSE)
else:
in _con igu a ion_ ajec o y=[ sel . obo .ge _cu en _con igu a ion(), q_in ];
goal_join _ ajec o y=sel . ajec o y_gene a o .con e _goal_ ajec o y_ om_join
_ ajec o y(in _con igu a ion_ ajec o y,[0,1.0], A =sel .globalA )
sel . obo .send_goal_ ajec o y(goal_join _ ajec o y,"in e media e posi ion")
ospy.sleep(2)
sel .de ec ing_hole_guess_bool=T ue
eli sel .cu en _s a e == S a e.DETECTING_HOLE_POSE
and new_s a e==S a e.GOING_HOLE_GUESS:
ospy.login o("s a e going hole guess")
sel .mo e_ o(sel .loca ion_pose_aux, "loca ion_pose_aux")
sel .going_hole_guess_bool=T ue
eli sel .cu en _s a e == S a e.GOING_HOLE_GUESS and new_s a e==S a e.MOVING_INTO_HOLE:
ospy.login o("s a e mo ing in o hole")
sel .mo e_ o(sel . i e _aux, " i e _aux")
sel .mo ing_in o_hole_bool=T ue
eli sel .cu en _s a e == S a e.MOVING_INTO_HOLE and new_s a e==S a e.RIVETING:
ospy.login o(" i e ing")
sel . obo . igge _ i e ();
sel .coun _ i e _success = sel .coun _ i e _success + 1
ospy.login o("Ri e ing eached:")
p in (sel .coun _ i e _success)