Poisot Manipulacion Objetos Donaxi

UNIVERSIDAD POPULAR AUTNOMA
DEL ESTADO DE PUEBLA
DEPARTAMENTO DE INGENIERAS
FACULTAD DE ELECTRNICA
Tesis:
SISTEMA DE MANIPULACIN DE OBJETOS PARA
ROBOT DE SERVICIO DONAXI
Trabajo de Investigacin
que para obtener el Ttulo de
INGENIERO EN MECATRNICA
Presenta:
Victor Poisot Martnez
Asesor:
Dr. Hctor Simn Vargas Martnez
Puebla, Pue., Mxico. Septiembre de 2014.

CARTA DE APROBACIN DE OPCIN DE TITULACIN.
II
CARTA DE APROBACIN DE ASESOR.
III
A mis padres
que esperaron, trabajaron y aguantaron.
IV
AGRADECIMIENTOS.
Antes de empezar a aventar nombres a los cuatro puntos cardinales, quiero dejar en claro
que este trabajo no es el punto final de una etapa, al contrario, es un punto de partida para una
vida. Quiero decir: no espero que vean estas lneas como un pequeo gesto de agradecimiento
de lo que me ayudaron a terminar. No. Lo que quiero es que las entiendan como una muestra
de lo agradecido que estoy por su apoyo en el comienzo de la siguiente etapa que se
materializa el da de hoy.
Primero que nada, quiero agradecer a Dios por poner los medios para que yo haya
podido caminar hasta donde estoy y tener las fuerzas para enfrentar el camino que sigue. Sin
embargo, para poder caminar, por mucho que uno tenga los pies y el camino adecuados, es
importante un apoyo, que alguien sostenga nuestra mano y que nos ensee a dar los pasos, uno
a uno hasta que al fin logremos seguir por nuestro propio pie; por eso es tan importante
agradecer a mis paps: por estar ah siempre que hacan falta, por no abandonarme nunca, por
ensearme a caminar y a ser quien soy. Tambin a mi hermano, Paul, por estar presente en los
momentos importantes y ser mi apoyo desde hace casi trece aos.
Al resto de mi familia: tos, abuelos y primos, tambin quiero darles las gracias por su
apoyo y por su confianza. Agradezco en particular a mis tos: Juan Carlos, Jacqueline y Hugo;
por ofrecerme su mano en mltiples ocasiones a lo largo de estos aos.
Quiero agradecer a todos mis profesores que me apoyaron de diversas formas posibles al
compartirme sus conocimientos, sus experiencias y sus consejos, por la confianza que varias
veces depositaron en m y por los retos que me imponan sus tareas. No me gustara dejar a
nadie en el tintero, pero quiero hacer mencin particular a algunos profesores que por una o
por otra razn han sido importantes en mi formacin profesional: Dr. Hctor Vargas, Dr.
Casimiro Gmez, Mtro. Marco Antonio Ramrez, Dra. Luz Garca, Mtro. David Malpica, Dra.
Arllene Prez, Mtro. Francisco Calvo y Mtro. Enrique Snchez.
Agradezco a mis amigos y compaeros, principalmente a los que son ms mis amigos
que mis compaeros, por apoyarme cuando el trabajo apretaba, por acompaarme a disfrutar
enormemente la etapa universitaria, por soportar conmigo el estrs de las tareas, en fin, por
haber hecho que sta sea una etapa que valga la pena. Agradezco a Perroni, Jaime, Carlos,
V
Jos Luis, Lauro, Peto, Aldo Daniel, a los que formaron parte del equipo de Donaxi@Home y
a todos los dems que no puedo mencionar por cuestiones de espacio, pero que sin ellos, este
trabajo no hubiera sido posible.
Tampoco puedo dejar fuera a todos aquellos que estuvieron ya sea en algn momento o
todo el tiempo, y que su presencia no era necesariamente en el aula ni en el laboratorio, pero
que sin duda han sido y son muy importantes para m. A Mara Jos, Melissa, Fernanda,
Juanito, Sainos, Elena, y por ltimo, pero no menos importante, a Brenda, que ha sido un gran
apoyo para m en esta recta final.
Quiero hacer un agradecimiento en particular a la Asociacin Periodstica Sntesis por el
fundamental apoyo que me ofrecieron a lo largo de toda mi carrera, sin el cual no me hubiera
sido posible llegar hasta donde estoy.
Tampoco puedo dejar fuera al Mtro. Carlos Martnez, a quien agradezco enormemente
por su apoyo tan valioso en mltiples ocasiones a lo largo de estos aos.
Espero no haber olvidado a nadie, y espero me perdonen si lo hice; de verdad estoy muy
agradecido con todos, y no queda ms que desearles lo mejor.
VI
SUMARIO.
Actualmente existen varios proyectos en desarrollo en el laboratorio de Control y

Robtica de la UPAEP, entre los cuales se encuentra el proyecto Donaxi@Home, del cual se
desprenden varias reas de investigacin que deben tratarse por separado. Una de esas reas de
investigacin es precisamente la que atae a este trabajo: manipulacin de objetos.
El Robot de Servicio Donaxi est diseado con base en las exigencias de la categora
@Home de la competencia ms importante de robtica a nivel mundial: RoboCup. En esta
categora se prueban robots con capacidad de apoyo e interaccin con las personas en la vida
cotidiana, tales como la habilidad de seguir una persona; entender y obedecer instrucciones de
modo que el robot sea capaz, por ejemplo, de buscar un objeto solicitado y llevarlo a una
ubicacin especfica; apoyar a personas en situaciones de emergencia como incendios y
dems; por nombrar de manera general sus capacidades. Como un complemento, en Donaxi se
ha puesto un especial nfasis en su apoyo a personas con capacidades diferentes as como
gente de la tercera edad.
Para cumplir estas exigencias, fue necesario incluir un brazo robtico en el prototipo, el
cual fue diseado y construido por el estudiante de Ingeniera Mecatrnica Jaime Robles
Ramos durante el perodo primavera 2012. Desde entonces se comenz con la programacin
del sistema de control. Tras hacerse algunas versiones previas del software que sirvieron para
conocer el funcionamiento de ROS, de la tarjeta CM700 y de los servomotores Dynamixel; se
plante un nuevo y ms estable sistema que incluye cinemtica inversa para manipulacin de
objetos. ste sistema de control es el descrito en el presente trabajo.
Los servomotores Dynamixel son desarrollados por Robotis empresa que se encarga de
fabricar materiales para robtica tanto motores, tarjetas de control, como kits de robtica. Los
modelos de servomotores utilizados se valen de comunicacin serial siguiendo el protocolo
RS-485 para ser controlados, y la tarjeta CM700, propia de Dynamixel, permite hacerlo. sta
tarjeta funciona con un microcontrolador Atmel, el cual es capaz de comunicarse con todos los
servomotores y con el ordenador al ser programada apoyndose de una SDK de parte de
mismo Robotis.
La comunicacin con esta tarjeta se hace por medio de las libreras de comunicacin
serial implementadas en Python, las cuales son utilizadas dentro de ROS interactuando con un
VII
nodo programado en C++ que aporta todos los clculos correspondientes de posiciones y
velocidades de los motores. Se detallar en la arquitectura de control de la que se habla.
La parte crtica del software, la que toma las decisiones importantes, es decir la
cinemtica inversa que es donde se calculan los ngulos y las velocidades de los servomotores,
se desarroll comenzando por los clculos matemticos que se lograron por medio de la
convencin Denavit-Hartenberg, lo que permiti hacer los clculos referentes a la cinemtica
directa, y por extensin, de la inversa.
Los resultados obtenidos en este proyecto pueden ser mudados a otros prototipos, sin
embargo se debe tener en cuenta, que el modelo cinemtico del brazo debe ser replanteado, as
como los protocolos de comunicacin con los servomotores deben ajustarse a las necesidades
especficas del robot.
VIII
ABSTRACT.
Nowadays there are many projects in current development in the Control and Robotics
Laboratory of UPAEP, among them the Project Donaxi@Home is found, in which coexist
many research areas that have to be treated separately. One of those research areas is precisely
the one meant for this work: object manipulation.
Donaxi Service Robot is designed based in the requirements of the @Home League
from the most important Robotics Competition in the world: RoboCup. In this league robots
are tested in the capacity of helping and interacting with people in daily life; like the ability to
follow a person; understand and obey spoken instructions, for example, in order to look for an
asked object, find it and take it to a specific location; helping people in emergency situations
as fires, earthquakes and so on. As a complement, Donaxi has a special emphasis in the
support for handicapped people and also elder people.
In order to fulfill these requirements, it was necessary to attach a robotic arm to the
prototype, which was designed and built by the Mechatronics Engineering student Jaime
Robles Ramos during the spring 2012 period. Since then, the programming of the control
system was begun. After a few previous versions of the software which were useful to get to
know and understand the way ROS, the CM-700 board and the Dynamixel motors work; so a
new and steadier system was proposed that includes Inverse Kinematics for Object
Manipulation. This control system is the described in the present work.
Dynamixel servomotors are developed by Robotis, which is a company responsible of
manufacturing materials for Robotics, from motors, control boards and even robotics kits. The
used models of servomotors utilize serial communication, with the RS-485 protocol to be
controlled, and the CM-700 board, also from Dynamixel, is meant to do that. This board
works with an Atmel microcontroller, which is able to communicate with all of the
servomotors and with the computer since it is programmed with the support of an SDK from
Robotis itself.
The communication with this board is by the serial communication libraries
implemented in Python, which are used inside ROS and interact with a node programmed in
C++ which provides all of the appropriate calculations of positions and velocities of the
servomotors. More details will be provided about the mentioned control architecture.
IX
The most critical part of the software, the one that makes all important decisions, in
other words the Inverse Kinematics, which is where angles and velocities of servomotors are
calculated, it was developed starting with the Mathematical calculations that were achieved
with the Denavit-Hartenberg convention. This allowed to make the calculations about de
Direct Kinematics, and by extension, the Inverse Kinematics.
The obtained results in this project can be transferred to other prototypes, but it must be
considered that the kinematics model of the arm should be set again, and also the
communication protocols with the servomotors should adjust with the specific requirements of
the robot.
X
NDICE.
Carta de aprobacin de opcin de titulacin. ................................................................. II
Carta de aprobacin de asesor. ..................................................................................... III
Agradecimientos. .......................................................................................................... V
Sumario. .................................................................................................................... VII
Abstract. ...................................................................................................................... IX
ndice. .......................................................................................................................... XI
Captulo 1. Introduccin................................................................................................. 1
1.1. Antecedentes. ................................................................................................. 2
1.2. Justificacin. .................................................................................................. 6
1.3. Objetivos. ....................................................................................................... 7
1.3.1. Generales. .................................................................................................. 7
1.3.2. Especficos. ................................................................................................ 7
1.4. Pregunta de investigacin. .............................................................................. 8
1.5. Alcances y limitaciones. ................................................................................. 8
1.6. Viabilidad de la investigacin. ........................................................................ 9
1.7. Impacto esperado. .......................................................................................... 9
1.8. Estructuras de memoria de tesis. ................................................................... 10
Captulo 2. Modelos cinemticos. ................................................................................ 12
2.1. Brazos Robticos. ........................................................................................ 13
2.2. Modelos Cinemticos. .................................................................................. 19
2.3. Convencin Denavit-Hartenberg en manipulador de Donaxi......................... 25
XI
2.4. Cinemtica Directa de Brazo de Donaxi. ...................................................... 34
2.5. Mtodos de Cinemtica Inversa. ................................................................... 36
2.5.1. Mtodos Geomtricos. ............................................................................. 37
2.5.2. Transformacin inversa. ........................................................................... 37
2.5.3. Desacoplo Cinemtico.............................................................................. 38
2.5.4. Tcnicas Iterativas. .................................................................................. 39
2.6. Viabilidad de Mtodos de Cinemtica Inversa en Robot de Servicio Donaxi. 41
2.7. Propuesta de Cinemtica Inversa para Manipulador de Donaxi. .................... 44
2.8. Planificacin de Rutas para Manipulacin. ................................................... 50
2.8.1. Tipos de Trayectorias. .............................................................................. 51
2.8.2. Generacin de Trayectorias. ..................................................................... 52
2.9. Propuesta de Trazado de Rutas para Manipulacin de Donaxi. ..................... 53
Captulo 3. ROS. .......................................................................................................... 60
3.1. Arquitecturas de Control para Robots. .......................................................... 61
3.2. Arquitectura de Control Utilizada en Donaxi. ............................................... 69
3.3. Ventajas del uso de ROS. ............................................................................. 72
3.4. Programacin en ROS. ................................................................................. 74
3.5. Programacin de Secuencias con Brazo de Donaxi. ...................................... 78
3.6. Simulacin de un Brazo en Gazebo. ............................................................. 82
Captulo 4. Implementacin de Algoritmos. ................................................................. 86
4.1. Librera de Cinemtica y Dinmica (KDL) de OROCOS. ............................. 87
4.2. Programacin de Cinemtica Directa. ........................................................... 90
4.3. Programacin de Cinemtica Inversa. ........................................................... 94
4.4. Programacin de Algoritmo de trazado de rutas. ........................................... 96
4.5. Simulacin de Cinemtica Inversa. ............................................................... 98
XII
Captulo 5. El Brazo Robtico de Donaxi. .................................................................. 101
5.1. Servomotores Dynamixel. .......................................................................... 102
5.2. CM700 ....................................................................................................... 105
5.3. Programacin de la Interfaz. ....................................................................... 109
5.4. Entorno de Usuario..................................................................................... 111
5.5. Implementacin con ROS de la Interfaz. .................................................... 115
Captulo 6. Conclusiones y perspectivas ..................................................................... 119
6.1. Conclusiones. ............................................................................................. 120
6.2. Perspectivas y trabajo futuro....................................................................... 123
Anexos....................................................................................................................... 126
Anexo A. Algoritmo Denavit-Hartenberg. .............................................................. 126
Anexo B. Cdigo de Matriz de TRANSFORMACIN para wxMaxima. ............... 127
Anexo C. Cdigos Generales.................................................................................. 131
Anexo D. Cdigos Utilizados en la Programacin de Secuencias. .......................... 132
Anexo E. Cdigos de Cinemtica. .......................................................................... 146
Anexo F. Cdigos de Interfaz de Comunicacin. .................................................... 155
Bibliografa ................................................................................................................ 162
XIII
Ley Cero de la Robtica. Un robot no puede causar dao a la humanidad

o, por inaccin, permitir que la humanidad sufra dao.
(Isaac Assimov)
CAPTULO 1. INTRODUCCIN.
En este captulo se describen los antecedentes de investigacin en materia de

manipulacin de objetos, as como el estado del arte del problema que se aborda. Se dejarn
claros los objetivos y las limitaciones que presenta para su posterior desarrollo. Adems se
definirn ciertos trminos de importancia que permitirn comprender la teora descrita en el
presente trabajo.
1
2
1.1. ANTECEDENTES.
Para la presente tesis se desarroll, en el Laboratorio de Control y Robtica de la

UPAEP, un software de control del brazo robtico implementado en el Robot de Servicio
Donaxi, el cual ha participado en tres ocasiones en la RoboCup (Vargas M., y otros, 2013) y
dos ms en el Torneo Mexicano de Robtica. El proyecto Donaxi@Home tiene casi tres aos
de desarrollo que han sido continuacin del proyecto Nanisha el cual tambin represent a la
Universidad en mltiples certmenes de robtica.
Para el desarrollo de este sistema se requiri de gran parte de los conocimientos
adquiridos en las asignaturas correspondientes a la carrera de Ingeniera Mecatrnica; tanto
con materias pertenecientes al rea de Matemticas, como de Electrnica y de Control; as
como tambin una infinidad de fuentes externas propias del equipo utilizado como manuales y
tutoriales adems del invaluable apoyo de distintos libros.
El problema de manipulacin de objetos es motivo de estudio muy recurrente por
centros de investigacin de robtica en todo el mundo. Esto se debe a que el uso de
manipuladores es de vital importancia en la industria y en muchas de las reas en las que se
estn explorando nuevas aplicaciones para la robtica como es el caso de la robtica de
servicio.
Como ya se mencion, Donaxi es un robot que compite contra robots del mismo tipo,
provenientes de distintas partes del mundo. Todos estos proyectos han presentado soluciones
muy variadas para resolver los retos que representa la manipulacin de objetos, y a
continuacin se har una mencin de las propuestas ms destacadas en lo que respecta a
manipulacin de objetos para comprender el estado del arte en la materia.
Algunos prototipos cuentan con brazos comerciales montados en sus respectivas
plataformas como es el caso del robot presentado por Berlin United (Llarena, Boldt, Steinke,
Engelmeyer, & Rojas, 2013) que cuenta con un Neuronics Katana y el apoyo de un Robotis
Darwin-OP para objetos pequeos; y el proyecto Homer@UniKoblenz (Seib, y otros, 2013)
que de igual manera presenta un Neuronics Katana 400HD a la par que el apoyo, para levantar
objetos del suelo, de un manipulador de dos grados de libertad (por comodidad, en el presente
documento se les llamar grados de libertad o GDL de manera indistinta); de la misma
forma, presentando un Neuronics Katana 6M arm, est el equipo Markovito de INAOE (Sucar,
3
y otros, 2012) el cual es capaz de manipular objetos asumiendo que se han reconocido, se
cuenta con una nube de puntos del entorno y hay un camino para la manipulacin, es entonces
cuando se hace una planificacin del movimiento utilizando una tcnica de Rapidly-exploring
Random Tree (rbol Aleatorio de Exploracin Rpida); procedentes de Espaa, est el
equipo Reem@IRI (Aleny & Tellez, 2013) quienes presentan el robot comercial REEM de
PAL Robotics, y que enfocan su investigacin en la manipulacin de objetos flexibles como
textiles, lo que ha representado un reto muy grande debido a lo impredecible del estado del
objeto deformable; tambin est el caso de b-it bots (Dwiputra, y otros, 2013) que trabajando
con un brazo KUKA, presentan un sistema que puede ser utilizado como una interfaz hptica,
es decir una forma de controlar al robot por medio del contacto con este manipulador; esta
propuesta considera dos posibilidades: el brazo como el control para gua del movimiento del
robot, y para el apoyo en el transporte cooperativo de objetos grandes. Las pruebas han
demostrado movimientos estables e intuitivos para el usuario.
En el equipo BORG (van Elteren, y otros, 2013) se utiliza un NAO de Aldebaran
montado sobre la plataforma, el cual les da la posibilidad de manipular objetos pequeos, e
implementaron un sistema de Online Behavior Learning que consiste en una base de datos
de comportamientos preprogramados, as eligen el que ms se adapte a las circunstancias
particulares, adems de Machine Learning que les permite encontrar una trayectoria de accin
sin ningn entrenamiento previo; el equipo MRL (Mahmoudi, y otros, 2013) opt por montar
en su robot un brazo Kinova Jaco hecho de fibra de carbono con seis GDL.
De la misma forma hay muchos equipos que utilizan manipuladores diseados
nicamente para sus plataformas robticas. Tal es el caso de robots como AssistBot de BART
LAB (Suthakorn, y otros, 2013) con dos manipuladores de seis GDL cada uno; el prototipo de
Dong Yang (Smithmaitrie, y otros, 2013) cuenta con dos brazos de cuatro GDL controlados
por servomotores Robotis Dynamixel, adems de la utilizacin de un sensor ultrasnico en el
gripper; el equipo TRCC (Maneewarn, Sooktip, Thungod, Suparat, & Aurmyou, 2013) que
presentan una propuesta de un brazo robtico con una webcam y un sensor infrarrojo
montados en el propio manipulador, lo que ofrece ciertas ventajas y una mayor precisin para
el reconocimiento y localizacin de los objetos; entre los pocos equipos latinoamericanos est
el de Uchile (Correa, y otros, 2013) quienes presentan dos brazos de siete GDL, controlados
por servomotores Robotis Dynamixel, adems de un pequeo brazo gripper con cinco GDL
4
ubicado en la parte inferior para recoger objetos del suelo; una propuesta interesante es la del
equipo Sun (Qizhi, Yali, Xinxin, Ye, & Jun, 2013) que presenta un prototipo con un
manipulador que se mueve nicamente en un plano 2D, tambin controlado por servomotores
Dynamixel, cuya altura es controlada por un actuador lineal en el hombro, y que por lgica, es
la base mvil quien se encarga de la alineacin del objeto con el plano de accin del brazo;
entre los proyectos mexicanos est el robot Justina del equipo Pumas de UNAM (Savage, y
otros, 2013), el cual cuenta con dos manipuladores de siete GDL cada uno, construidos con
base en servomotores Robotis Dynamixel, y que son controlados por un control PID y un
sistema que permite compensar la gravedad, adems de un control basado en visin para la
manipulacin de objetos; siguiendo con los proyectos que se valen de la visin del robot para
el control del brazo est KeJia (Chen, y otros, 2013), el cual cuenta con un brazo de cinco
GDL, que es calibrado y utilizado a travs del seguimiento con visin de una serie de
marcadores colocados en el brazo en lugar de utilizar la propia nube de puntos del brazo;
tambin es muy importante mencionar al robot Amigo, del equipo Tech United Eindhoven
(Lunenburg, y otros, 2013), quienes utilizan dos brazos de siete GDL unidos a un torso con
una articulacin que le permite subir y bajar, adems de que cada brazo es capaz de levantar
1.5 kg estando bien estirado, los manipuladores tienen un marcador en el gripper, el cual es
identificado por el sistema de visin una vez que se ha acercado al objeto.
Dentro de los equipos con prototipos de brazos propios, hay dos que se destacan por la
simpleza de sus diseos: el robot Golem II (Pineda, 2013) el cual presenta un brazo de tres
grados de libertad, los cuales limitan el movimiento del gripper en un plano horizontal,
adems de tres sensores infrarrojos en el gripper, el brazo tambin cuenta con un elevador para
el brazo que aade un cuarto grado de libertad. En el Torneo Mexicano de Robtica 2013, el
Dr. Luis A. Pineda explic cmo funciona dicho sistema: el mdulo de visin reconoce el
objeto y alinea aproximadamente el eje central del robot con el objeto, luego aproximan el
brazo hacia el frente hasta que los tres sensores detectan la presencia del objeto, se es el
momento de tomarlo.
Otro equipo con una solucin peculiar es Delft Robotics quienes presentan un prototipo
de bajo costo, DPR2 (Bharatheesha, y otros, 2012), el cual cuenta con un manipulador muy
simple, de un nico GDL. La base mvil es quien alinea el robot con el objeto reconocido, y el
5
gripper, que cuenta con un sensor infrarrojo, se limita a cerrarse cuando el objeto est en el
alcance.
Precisamente el equipo que gan tres veces consecutivas la categora @Home (2011-
2013) es NimbRo, proveniente de la Universidad de Bonn en Alemania; Cosero, el principal
robot de NimbRo se destaca por su capacidad para manipular objetos; Cosero (Stckler, y
otros, 2013) tiene dos brazos de 7 GDL, adems de un torso con 2 GDL que incrementa
considerablemente el espacio de trabajo de los manipuladores. Todos los actuadores son
servomotores Robotis Dynamixel EX-106+. Adems de las enormes ventajas que provee un
diseo como el descrito, estn presentando algunas interesantes soluciones en la parte de
control: mapas de altura representando el entorno para lograr una planeacin que evite
colisiones, el llamado fast grasp planning que es un algoritmo vinculado con su sistema de
visin para trazar rutas libres de colisiones para tomar objetos sobre superficies planas;
compliant control que es un control en el torque de algunas articulaciones que facilita la
programacin de tareas como abrir puertas; tambin han implementado el grasp planning
(Nieuwenhuisen, Stckler, Berner, Klein, & Behnke, 2012), (Stckler & Behnke, 2012) que es
un algoritmo mucho ms complejo y flexible que permite tomar objetos tanto en modo
horizontal como vertical; adems de un sistema de Calibracin Bayesiana de la cinemtica
ojo-mano de un robot antropomrfico (Hubert, Stckler, & Behnke, 2012), el cual consiste en
una serie de entrenamientos del movimiento del brazo en la que recorren el espacio de trabajo
y el robot guarda ejemplos de entrenamiento, los cuales permiten reducir considerablemente
los errores que aparecen tras los clculos de cinemtica.
Tomando como punto de partida el estado del arte en manipuladores, y utilizando como
base los recursos disponibles en el laboratorio y el tiempo con que se contaba se llev a cabo
la construccin de un brazo Robtico de cinco GDL, ms un gripper que es justamente el
elemento que se encarga de poder sujetar los objetos. Y que posteriormente se trabaj en la
implementacin del software de control de dicho sistema.
Se plante originalmente un software de control cuyo modelo matemtico estaba basado
en una convencin propia para este brazo en particular, y por extensin, los clculos
matemticos eran bastante simples. Estos clculos aunque tenan una muy buena precisin, no
permitan llegar a clculos de la orientacin del manipulador. En lo que respecta a la
comunicacin con la computadora, tambin se simplific el protocolo al grado de slo enviar a
6
los servomotores instrucciones cada vez que se necesitaba que ejecutaran alguna instruccin;
este protocolo ofreca una gran estabilidad en la comunicacin y en el comportamiento de los
servomotores, sin embargo era demasiado rgida, pues no permita encontrar errores, conocer
el estado de los motores ni enviar algn otro tipo de instruccin distinta. Adems careca de
interfaz de usuario, por lo que era un proceso complicado cada vez que se pretenda poner el
brazo en funcionamiento, y era una actividad exclusiva de la persona que lo desarroll.
Cuando se observaron todas estas deficiencias, se comenz a disear desde cero un
nuevo software que solucionara todos estos problemas, es decir, incluir clculos de cinemtica
inversa realmente funcionales que permitan encontrar la configuracin de las articulaciones
que diera la orientacin adecuada del gripper. Adems se busc que el propio software
permitiera simplificar la programacin rutinaria del brazo a travs de una estructuracin del
cdigo con miras a que, en pocas lneas de programa y con una sintaxis lo ms natural posible,
se pudieran crear secuencias nuevas o estrategias para objetos distintos y, finalmente, que se
pudiera observar a simple vista un movimiento ms natural del robot.
1.2. JUSTIFICACIN.
En la actualidad, la robtica de servicio en el hogar no ha alcanzado niveles en los que

pueda llegar a ser comercializada; por tanto no tiene una utilidad a corto plazo, sin embargo se
est trabajando en diversas instituciones en todo el mundo para crear robots que sin ser tan
costosos puedan ofrecer una asistencia til con un mnimo de conocimientos tcnicos para su
operacin.
El Laboratorio de Control y Robtica de la UPAEP se ha sumado a esta iniciativa de
investigacin con el proyecto Donaxi@Home con miras a participar en el torneo RoboCup, lo
que adems representa una necesidad de construir y programar un robot competitivo; es por
esto que se han incluido diversas capacidades, entre ellas el brazo para manipular objetos.
Dado que el Robot de Servicio Donaxi cuenta con un brazo, se vuelve de vital
importancia el diseo de un software que permita utilizarlo. Este software debe ser innovador
en el rea y debe estar diseado especficamente para este brazo en particular. Se requiere
innovador en el rea pues es importante buscar alternativas que solucionen los distintos
7
problemas que presenta, en la actualidad, la manipulacin de objetos, una solucin que sea
estable y sencilla de operar. Y que sea diseado especficamente debido a las medidas y
caractersticas propias del brazo, adems de las capacidades del resto del robot tales como la
informacin que es capaz de ofrecer la visin o la movilidad de Donaxi, y por ltimo que el
software debe ser integrable con el resto del sistema de control.
1.3. OBJETIVOS.
1.3.1. Generales.
Escribir un software que permita a un brazo robtico tomar objetos tras recibir
datos como las coordenadas de los mismos y su naturaleza y luego colocarlos en
algn otro lugar con sus respectivas coordenadas.
1.3.2. Especficos.
Generar un modelo matemtico del brazo que permita hacer clculos de
cinemtica directa e inversa.
Implementar en software los clculos de cinemtica directa y una propuesta de
cinemtica inversa.
Programacin de la interfaz que permita la comunicacin entre el brazo robtico
y el ordenador.
Diseo de interfaz de usuario para simplificar la operacin del Robot.
Integracin del software generado en los tres puntos anteriores con arquitectura
de control de Robot de Servicio Donaxi.
8
1.4. PREGUNTA DE INVESTIGACIN.
Tal como se han planteado los objetivos de este trabajo, resulta evidente que lo que se
pretende disear y elaborar es un software de control para el brazo del Robot de Servicio
Donaxi que pueda ser funcional, a la vez que pueda ser base para posteriores desarrollos
propios del equipo de trabajo del proyecto. Es por eso que se puede plantear la siguiente
pregunta de investigacin:
Cmo se puede hacer un software eficiente, estable y de sencilla operacin que permita
manipular objetos al brazo del Robot de Servicio Donaxi con base en los actuales avances con
el resto del proyecto?
1.5. ALCANCES Y LIMITACIONES.
Actualmente es posible encontrar una gran variedad en brazos robticos destinados a la

manipulacin, tanto en el rea industrial como en la robtica de servicio, pasando desde los
autnomos hasta los teleoperados. Lo que nos da una idea de la vastedad de posibilidades que
ofrece el rea de manipulacin de objetos, as como la cantidad enorme de elementos que son
utilizados de Matemticas, Teora de Control, Mecnica, Electrnica y dems.
Hacer un estudio de todas estas posibilidades sera interminable, es por eso que la
presente investigacin se enfocar en el desarrollo del software de control de un manipulador.
Se trabajar nicamente con el software debido a que el prototipo del brazo ya est construido,
y no se cuenta con recursos tanto materiales como de tiempo para la construccin de un nuevo
brazo o un cambio del diseo de la parte electrnica. As, no ser necesario explicar el diseo
mecnico del brazo ni sus cuestiones electrnicas, pues es menester de otras investigaciones.
El brazo robtico fue construido hace ms de un ao por otro estudiante de Mecatrnica,
y dicho brazo es quien va a determinar las posibles acciones que se pueden programar, pues
tiene un espacio de trabajo claramente definido debido a sus dimensiones, cinco grados de
libertad ms un gripper, lo que hasta cierto punto representar una limitante como se explicar
ms adelante.
9
Al limitar la investigacin al software de control, se pueden observar nuevamente
muchas posibilidades por la gran cantidad de capacidades que se pueden programar en
manipuladores de objetos. Por eso es importante aclarar que el presente trabajo se limitar a
ofrecer una solucin para hacer al brazo de Donaxi capaz de tomar un objeto y colocarlo en
otro lugar considerando que el sistema de visin del robot le comunica la naturaleza del objeto
y su ubicacin en el espacio.
1.6. VIABILIDAD DE LA INVESTIGACIN.
Como ya se ha hablado en la seccin 1.1 existen equipos de investigacin que han

podido hacer funcionar sistemas de manipulacin de objetos. Esto se ha logrado gracias a las
importantes aportaciones de las matemticas. Actualmente se cuenta con las herramientas
adecuadas que nos permiten realizar este tipo de clculos. Adems, se cuenta con equipos de
cmputo con la capacidad de atender mltiples tareas, lo que nos lleva a la posibilidad de tener
una visin artificial que pueda guiar a otro programa que funcione a la par y que sea capaz de
manipular los objetos.
El hecho de contar con el brazo robtico y que tanto el diseo mecnico como el
electrnico sean funcionales nos da la idea de que realmente la mitad del sistema est hecho, y
pues lo nico que podra limitar las posibilidades de xito sera que alguno de los motores
fallara. Cosa que se evit trabajando con sumo cuidado en todo momento.
1.7. IMPACTO ESPERADO.
Como se menciona en la seccin 2.7, una de las razones por las que se ha diseado un
algoritmo de cinemtica inversa propio de Donaxi se debe al inconveniente de no poder
controlar directamente una de las seis dimensiones en las que se mueve el brazo debido a los
cinco grados de libertad, el diseo mecnico y el espacio de trabajo. sta limitante hizo pensar
en una alternativa: un nuevo algoritmo que buscara la posicin y que nos permitiera descartar
10
sin problemas una o varias dimensiones, cuestin que se complica si se utilizara algn mtodo
matemtico conocido.
De esta forma, este algoritmo va a ser perfectamente funcional para dems brazos
robticos con limitantes similares, pues al implementarse no representar gran problema
eliminar las variables que no interesen del resultado final. Incluso el cdigo est diseado y
comentado de forma en que sera posible reutilizarlo cambiando nicamente ciertas variables
por otras que de una forma o de otra estn consideradas para casos de brazos con diseos,
capacidades y limitantes diferentes.
Del mismo modo, se espera que el presente trabajo sea de gran importancia para la
participacin del Robot de Servicio Donaxi en las futuras competencias RoboCup en la
categora @Home. Pues se pretende presentar el robot ahora con dos brazos, y sin duda, con el
avance actual slo sera necesario duplicar el modelo existente y aadir algunos motores ms
al sistema de control, tareas que no representarn gran problema.
A largo plazo, dentro del laboratorio se espera que se contine con la investigacin
dentro de este mismo proyecto, con el fin de que se obtengan funcionalidades completamente
nuevas que permitan un mejor desempeo en manipulacin de objetos dando pie a posteriores
trabajos de parte de ms estudiantes de la facultad.
1.8. ESTRUCTURAS DE MEMORIA DE TESIS.
El presente trabajo se estructur de una manera en que pueda resultar ms claro para el
lector el desarrollo del proyecto, pues la gran cantidad de elementos que el software considera
y la forma en que stos se relacionan entre s implica que las etapas de programacin tuvieron
que seguir un orden progresivo que optimizara el tiempo de escritura de cdigo y de
depuracin.
As se comenz por una investigacin y una serie de clculos que permitieran plantear
un modelo matemtico del brazo y que a la vez, conduzca a los clculos de cinemtica directa
e inversa, as como el planteamiento de un algoritmo que trace la ruta que debe seguir el brazo
para alcanzar los puntos deseados. Esta etapa es tratada en el Captulo 2. .
11
Teniendo estos clculos y algoritmos planteados, se comenz la etapa de programacin
en ROS cuidando en todo momento que se mantenga acorde y compatible con la arquitectura
de Donaxi. Adems fue necesaria la construccin y programacin de un sencillo modelo
virtual del brazo que permita simular cmo se comportara el brazo real. El Captulo 3.
permite profundizar en estos temas.
Como la tercer etapa del proyecto, se implementaron los algoritmos y clculos que se
propusieron en la primera (Captulo 2. ), y se integraron a la plantilla de programacin y a la
simulacin generada en la etapa anterior. Esta etapa queda detallada en el Captulo 4.
La ltima etapa de programacin consiste en el desarrollo de la interfaz con los motores,
el diseo del entorno de usuario y la implementacin final del cdigo generado a lo largo de
todas las etapas. En el Captulo 5. se encuentra toda la informacin referente a esto.
Finalmente, en el Captulo 6. , se presentan las conclusiones generales del proyecto,
incluyendo observaciones, recomendaciones y advertencias del desempeo del sistema de
manipulacin de objetos presentado.
No puedo definir un robot, pero reconozco uno cuando lo veo.

(Joseph Engelberger)
CAPTULO 2. MODELOS CINEMTICOS.
Este segundo captulo presentarn las herramientas matemticas utilizadas actualmente

para los clculos de cinemtica que debe realizar el software de control que permiten al brazo
realizar sus funciones de manipulacin de objetos. Adems de las descripciones de las
herramientas, se menciona tambin los clculos especficos realizados con el brazo y el
resultado de dichos clculos. Todo esto se utilizar a modo de base para la programacin
hecha en captulos posteriores.
12
13
2.1. BRAZOS ROBTICOS.
En la actualidad existen infinidad de robots utilizados en la industria, en aplicaciones de

servicio y en investigacin; as mismo existe una gran cantidad de literatura al respecto y con
distintos enfoques y conceptos. Es por eso que este subtema est dedicado particularmente a
aclarar ciertas definiciones importantes para la presente investigacin.
Barrientos, y otros, (2007) explican que el concepto de robot surgi de la palabra eslava
robota en la obra Rossum's Universal Robot (1921) del escritor checo Karel Capek, lo que
influy en la literatura posterior popularizando el trmino; y que despus de la aparicin, en
1948, de los telemanipuladores, el concepto de robot se comenz a utilizar en cuanto se opt
por la sustitucin del operador por un programa de ordenador que controlase los movimientos
del manipulador. Existen distintas definiciones para robot, que llegan a variar incluso
geogrficamente; sin embargo la definicin que da la Asociacin de Industrias Robticas
(RIA) es recurrente como la ms acertada en diversas fuentes (Jazar, 2006), (Fu, Gonzlez, &
Lee, 1987), (Barrientos, Pen, Balaguer, & Aracil, 2007) siendo de esta ltima de dnde se
toma la traduccin al castellano de la definicin: Un robot industrial es un manipulador
multifuncional reprogramable, capaz de mover materias, piezas, herramientas, o dispositivos
especiales, segn trayectorias variables, programadas para realizar tareas diversas. Resulta
conveniente, como complemento, notar ciertos aspectos importantes de los robots. De acuerdo
con Jazar (2006) son dispositivos complejos y verstiles conformados por una estructura
mecnica, un sistema sensorial y un sistema de control automtico, y segn Fu, y otros,
(1987) debe poseer inteligencia, la cual normalmente se debe a algoritmos de computadora
asociados con sus sistemas de control y de sensores.
En general se pueden distinguir tres tipos de robots: robots industriales, robots de
servicio y teleoperados. Los robots industriales, que son los que actualmente cuentan con
niveles mayores de desarrollo, y que son utilizados para manipulacin en ensamblajes y dems
etapas de los procesos, y que ofrecen la flexibilidad de poder ser reprogramados, lo que
permite aprovechar su espacio de trabajo adems de que su uso no se limita a secuencias fijas
(Lewis, Dawson, & Abdallah, 2004). Con este orden de ideas, lo que diferencia a los robots
industriales de los de servicio, es que estos ltimos son utilizados para cuidados mdicos,
educacin, domsticos, uso en oficinas, ambientes peligrosos y dems, mientras que los
14
teleoperados, debido a que requieren de un operador humano, no son considerados dentro de
esta categora (Barrientos, Pen, Balaguer, & Aracil, 2007).
Los robots actualmente tienen infinidad de aplicaciones entre las cuales encontramos:
carga de mquinas, pick and place, pintura, ensamblaje, inspeccin, muestreo, manufactura,
aplicaciones biomdicas, asistencia (Jazar, 2006), barrenado, pulido, paletizado (Lewis,
Dawson, & Abdallah, 2004), manejo de materiales, soldadura por puntos o por arco, carga y
descarga de mquinas de control numrico, exploracin submarina y del espacio,
investigacin de prtesis de brazos y manejo de materiales peligrosos (Fu, Gonzlez, & Lee,
1987). Barrientos y otros (2007), refirindose en particular a los robots de servicio, mencionan
que sus aplicaciones no son sistemticas, por tanto ofrecen soluciones aisladas a problemas
concretos, condicin que exige mayor inteligencia, siendo aplicados en agricultura,
silvicultura, ayuda a discapacitados, construccin, domsticos, entornos peligrosos, espacio,
medicina, minera, submarino, vigilancia y seguridad.
Un robot est conformado de manera general por dos elementos separados: un
manipulador y un controlador (Lewis, Dawson, & Abdallah, 2004). Sin embargo es necesario
identificar elementos especficos para la correcta comprensin del funcionamiento de los
robots. Tales elementos se describen a continuacin.
De acuerdo con Jazar (2006), un manipulador es el cuerpo principal del robot formado
por eslabones, articulaciones y otros elementos estructurales; un manipulador es considerado
un robot cuando la mueca y el gripper son unidos y el sistema de control es implementado.
Tambin Barrientos y otros (2007) refieren a una definicin dada por la Asociacin Francesa
de Normalizacin, segn la cual un manipulador es un mecanismo formado generalmente por
elementos en serie, articulados entre s, destinado al agarre y desplazamiento de objetos. Es
multifuncional y puede ser gobernado directamente por un operador humano o mediante
dispositivo lgico.
En las definiciones anteriores se menciona la importancia de eslabones en serie que son
cuerpos rgidos que pueden tener movimiento con respecto al resto de los dems eslabones, de
modo que si no hay movimiento relativo entre ms de un elemento, se le considerar un solo
eslabn (Jazar, 2006). Esto nos lleva a la necesidad hablar de una estructura mecnica, que
recordando la anatoma humana, se dice que est conformada por un brazo, una mueca y una
15
herramienta, lo que permite combinar los movimientos del brazo para alcanzar algn punto,
con los movimientos de la mueca para orientar la herramienta (Fu, Gonzlez, & Lee, 1987).
Los eslabones estn conectados por articulaciones, que son lo que permite al robot
realizar movimientos que pueden ser expresados con una sola coordenada; existen varios tipos
de articulaciones, pero nos limitaremos a hablar de los dos ms importantes que son
rotacionales y prismticas. Las articulaciones rotacionales son como bisagras y permiten la
rotacin relativa entre dos eslabones, mientras que las prismticas permiten la traslacin
relativa (Jazar, 2006). La Fig. 2.1 ilustra estos dos tipos de articulaciones.
Fig. 2.1 Representacin de las articulaciones de revolucin y prismtica.

Tomadas de (Jazar, 2006).
Barrientos y otros (2007) definen grado de libertad como cada uno de los movimientos
independientes que puede realizar cada articulacin con respecto a la anterior, y tambin
sostienen que es precisamente la cantidad de grados de libertad de un manipulador lo que
determina el espacio al que tiene acceso y las orientaciones que puede alcanzar; siendo as
muy recurrente en los robots industriales tener seis grados de libertad: tres en el brazo para
alcanzar un punto en su espacio de trabajo, ms tres en la mueca para orientar la herramienta,
sin embargo muchas aplicaciones pueden ser resueltas con cuatro o cinco GDL; y en
contraparte, existen aplicaciones que requieren un sptimo GDL, los cuales son llamados
manipuladores redundantes y son capaces de librar obstculos o ampliar su espacio de trabajo.
Para el manipulador de Donaxi se opt por cinco grados de libertad pues para aplicaciones de
16
pick and place la cantidad de orientaciones que requerir la herramienta no son tan delicadas
como es el caso de aplicaciones como soldadura o pintura.
Las articulaciones mencionadas, por lgica requieren un accionamiento, para eso es que
se utilizan distintos tipos de actuadores, que son las unidades que brindan la potencia
requerida para mover la estructura mecnica; los tipos de actuadores ms comunes son
elctricos, hidrulicos o neumticos (Jazar, 2006). Los actuadores hidrulicos y neumticos
representan la necesidad de bombas, compresores, depsitos y dems componentes
pertinentes, esto implica complicaciones para un robot mvil como Donaxi, es por eso que se
utilizan servomotores para la actuacin del manipulador, es decir actuadores elctricos de DC
que son alimentados por bateras.
Los sensores son elementos de gran importancia para los robots pues son los medios que
tiene para conocer tanto su estado como su entorno. En este sentido, los sensores con que
cuenta pueden ser internos como los encoders que permiten conocer la posicin de las
articulaciones, o sensores externos que permiten detectan variables como proximidad,
contacto, fuerza, e incluso la cmara utilizada para visin artificial (Fu, Gonzlez, & Lee,
1987). En este sentido, Donaxi cuenta con los sensores de posicin y de carga internos de los
servomotores (ver Captulo 5. ) que permiten el correcto control del brazo, adems del sistema
de visin que ofrece el conocimiento del entorno, y stos slo por mencionar los sensores
involucrados nicamente en la manipulacin.
Barrientos y otros (2007), en lo que respecta a los elementos terminales, sostienen que
son los eslabones cuyo propsito es la interaccin directa con el medio, por tanto, estos
componentes pueden ser clasificados de acuerdo a si se trata de herramientas o mecanismos de
sujecin. Para las aplicaciones de pick and place propias de Donaxi, result necesaria la
colocacin de un gripper que funciona como elemento de aprehensin. Dicho gripper consiste
en una pinza que abre y cierra accionada por uno de los servomotores que conforman el
manipulador.
Como se menciona anteriormente, un robot necesita de un controlador para cumplir con
su propsito. Como sealan Lewis y otros (2004), los controladores son sistemas
computarizados y nicos para cada robot, encargados de realizar funciones como la
planificacin de las trayectorias, su respectiva ejecucin, interpretacin de la informacin
obtenida por los sensores o entradas digitales y efectuar las operaciones lgicas necesarias,
17
adems de brindar al usuario la interfaz adecuada para poder ser programado y monitoreado.
De acuerdo con Barrientos y otros (2007) las caractersticas del controlador de un manipulador
definen el nmero de posibilidades que puede ofrecer el robot, de modo que existen
controladores para brazos con movimientos punto a punto, o algunos con trayectorias
continuas, la seleccin depender de la aplicacin en particular. Para el caso de Donaxi, como
se explicar ms adelante, el sistema de control es punto a punto, sin embargo el software
tabula una serie de puntos emulando una trayectoria continua. Adems, una parte importante
del control ha sido simplificada con la utilizacin de servomotores Dynamixel, pues la
implementacin de herramientas de control como los PID para la velocidad de giro ya no
resultan necesarios, ya que esas etapas son resueltas internamente.
Como se menciona en el prrafo anterior, son necesarios elementos que permitan la
entrada y salida de datos para el usuario. Los dispositivos ms recurrentes son teclados,
monitores y cajas de comando (tambin llamados teach pendants), con los cuales es posible
introducir y ver los datos del controlador, mandar instrucciones, e incluso dar de alta nuevos
programas (Gonzlez, 2002-03).
La Fig. 2.2 muestra las partes en que puede dividirse un manipulador al ser stas
comparadas en funcionalidad anloga con la de un brazo humano. As podemos distinguir un
hombro, un brazo, un codo, un antebrazo o una mueca por nombrar slo algunos de los
elementos.
Una parte muy importante en el manipulador es la mueca, pues es lo que permite
orientar a la herramienta una vez que sta ha alcanzado el punto deseado. Como se mencion
anteriormente las muecas, tienden a contar con tres grados de libertad gracias a articulaciones
rotativas, lo que representara una mueca esfrica (Jazar, 2006). Esta configuracin resulta
bastante conveniente, pues con ella se simplifican los clculos de los ngulos de roll-pitch-
yaw; sin embargo Donaxi cuenta con una mueca de dos grados de libertad, pues estos son
suficientes para cumplir con los requerimientos de las aplicaciones del robot, lo que permiti
economizar en la construccin del prototipo.
18
Fig. 2.2 Elementos de un manipulador. Tomada de (Gonzlez, 2002-03).

Hay un concepto fundamental en el diseo o seleccin de un robot: el espacio de trabajo,
el cual es el volumen total del espacio que puede alcanzar el elemento terminal; y por lgica,
ste espacio est determinado por la geometra y limitantes del manipulador, de modo que se
divide en dos espacios: el espacio alcanzable, que depende de los puntos que puede alcanzar la
herramienta al menos en una sola orientacin; y en segundo lugar el espacio de destreza, que
es el conjunto de puntos en los que el elemento terminal puede estar en todas las posiciones
posibles (Jazar, 2006).
Existe una infinidad de caractersticas propias de los robots que no se mencionan en el
presente trabajo por estar vinculadas meramente al diseo mecnico, por tanto se da nfasis en
las que afectarn directa o indirectamente al software de control.
El manipulador con que se trabaja es del tipo articulado antropomrfico, pues imita las
dimensiones y posturas humanas. De acuerdo a las clasificaciones propuestas por Jazar (2006,
pg. 8), Donaxi pertenece a la sexta clase: Robot Inteligente, pues es capaz de realizar su tarea
sin importar que hubiera cambios en su entorno, porque puede comprender estos cambios y
tomar decisiones con base en eso.
19
2.2. MODELOS CINEMTICOS.
Para el correcto control de los servomotores del manipulador es necesario conocer los
ngulos y la velocidad que necesita tener cada uno de dichos actuadores y as lograr ir a una
posicin especfica. En la resolucin de estos problemas, la robtica se vale de dos estudios
que permiten conocer el estado del robot bajo ciertas condiciones: cinemtica y dinmica.
La cinemtica es la rama de la ciencia que se encarga de estudiar la geometra del
movimiento sin tomar en cuenta los factores que lo causan, tales como las fuerzas o
momentos; es decir, se encarga del anlisis del desplazamiento del robot con relacin a un
sistema de referencia fijo y el tiempo en que el movimiento ocurre; se puede decir que en
particular estudia la relacin existente entre los valores de las articulaciones y la posicin del
elemento final (Fu, Gonzlez, & Lee, 1987).
A diferencia de la cinemtica, la dinmica ofrece herramientas para el anlisis de
sistemas que experimentan cambios en el tiempo, y que en el caso de los robots, se obtienen
ecuaciones de movimiento que describen tales cambios; estas ecuaciones son de gran
importancia para el diseo, el anlisis, el control del sistema, simulacin a computadora del
robot y evaluaciones de desempeo del diseo (Jazar, 2006). Adems Fu y otros (1987)
sostienen que se pueden encontrar ecuaciones reales de movimiento por medio de las leyes de
fsica newtoniana y lagrangiana, lo que nos permitira conocer el movimiento para cada
articulacin en trminos de los parmetros geomtricos e inerciales de los eslabones.
En el presente trabajo, la cinemtica juega un papel vital para el control del brazo. Sin
embargo, debido a la baja complejidad de los movimientos que debe efectuar el robot, el
estudio dinmico no ha sido requerido gracias a la implementacin de otros procesos mucho
ms sencillos que permiten solucionar los problemas de movimiento y que sern detallados
ms adelante. En lo que corresponde a la cinemtica, sta ofrece dos herramientas de
particular utilidad en robtica: la cinemtica directa y la cinemtica inversa.
En robtica, el problema de la cinemtica directa, que se explica a fondo en la Seccin
2.4, consiste en encontrar la posicin y orientacin del elemento terminal partiendo de que se
conocen los valores determinados de cada una de las articulaciones; mientras que la
cinemtica inversa, tambin explicada ms adelante, es un problema considerablemente ms
complejo, pues consiste en encontrar cada uno de los valores articulares partiendo de la
20
posicin y orientacin deseada para el efector final (Barrientos, Pen, Balaguer, & Aracil,
2007); la complejidad del problema recae en el hecho de que no siempre es posible encontrar
una configuracin del manipulador que permita llegar a la posicin deseada debido a
limitantes propias de la geometra y la mecnica del brazo, o en caso contrario, que existen
infinidad de soluciones diversas para algunas posiciones.
Estos dos problemas se valen de herramientas matemticas, por medio del uso de
modelos cinemticos, los cuales son representaciones matemticas de la estructura mecnica a
controlar. Existen diversas formas de plantear estos modelos, lo que hace posibles distintas
representaciones de un mismo prototipo. Segn Fu y otros (1987, pgs. 13-14) Se utiliza
lgebra vectorial y matricial para desarrollar un mtodo generalizado y sistemtico para
describir y representar la localizacin de los elementos de un brazo con respecto a un sistema
de referencia fijo.
Como se mencion anteriormente, los clculos cinemticos se efectan con base en un
sistema de referencia. Esto cobra importancia debido a que se requiere especificar tanto
posicin como orientacin de los cuerpos rgidos que conforman un robot, por lo que existen
diversas herramientas para especificar estos parmetros con respecto al sistema de referencia;
para la representacin de la posicin existen las coordenadas polares , coordenadas
cilndricas , coordenadas esfricas y las coordenadas cartesianas
(Barrientos, Pen, Balaguer, & Aracil, 2007); siendo esta ltima la utilizada con el
manipulador concerniente a la presente investigacin debido a la propia estructura, pues al ser
un brazo antropomrfico, las dems coordenadas no resultan igual de prcticas en sus
clculos, adems de que el propio sistema de visin se basa en este tipo de coordenadas.
Barrientos y otros (2007) tambin hablan de las herramientas para la orientacin, que como
sabemos, son de vital importancia para la descripcin de la posicin, pues para el elemento
terminal no es suficiente con conocer nicamente su ubicacin en el espacio, sino tambin es
necesaria su orientacin; con este propsito se han planteado las matrices de rotacin, los
ngulos de Euler, los pares de rotacin y los cuaternios.
Para resolver las dos necesidades descritas en el prrafo anterior, existen las matrices de
transformacin homognea, que consisten en una solucin que engloba los parmetros de
posicin y de orientacin (Barrientos, Pen, Balaguer, & Aracil, 2007). Estas matrices estn
formadas por cuatro submatrices que indican rotacin, traslacin, perspectiva y escalado, cada
21
una, tal como muestra la Ecuacin 2.1 que representa una matriz de
transformacin homognea para tres dimensiones (Barrientos, Pen, Balaguer, & Aracil,
2007):
[ ] [ ] Ecuacin 2.1
Las matrices de rotacin (3x3) y de traslacin (3x1) son de vital importancia en robtica,
mientras que las correspondientes de perspectiva (1x3) y escalado (1x1) es conveniente que se
pasen por alto, de modo que las componentes de perspectiva se mantengan en valores nulos y
la componente de escalado se mantenga en la unidad (Barrientos, Pen, Balaguer, & Aracil,
2007). Por tanto, slo ser necesario explicar las matrices de rotacin y de traslacin.
En lo que respecta a las de traslacin se cuenta con las tres componentes
correspondientes a los ejes X, Y, y Z. De modo que una transformacin de un vector r en un
sistema de referencia, se puede llevar a un vector r' en otro sistema de referencia si tenemos
un vector p que represente la distancia entre ambos sistemas de referencia (Barrientos, Pen,
Balaguer, & Aracil, 2007).
[ ] [ ][ ] [ ] Ecuacin 2.2
Las rotaciones de manera anloga se representan en la matriz 3x3 correspondiente, de la

forma que muestra en: Ecuacin 2.3, Ecuacin 2.4 y
Ecuacin 2.5 (Barrientos, Pen, Balaguer, & Aracil, 2007):
[ ] Ecuacin 2.3
[ ] Ecuacin 2.4
22
[ ] Ecuacin 2.5
En Ecuacin 2.3, Ecuacin 2.4 y Ecuacin 2.5 se

muestran rotaciones alrededor de los ejes X, Y y Z de manera independiente (Barrientos,
Pen, Balaguer, & Aracil, 2007). Es lgico, en aplicaciones reales son necesarias las matrices
de transformacin que consideren los tres ejes, las cuales se desarrollan por medio de
multiplicaciones sucesivas, en las cuales el orden en que ocurren las rotaciones es importante,
pues la multiplicacin de matrices no es conmutativa. Ms adelante se explicar la matriz de
rotacin resultante de la utilizacin de los ngulos de Euler (roll, pitch, yaw) implementados.
Las matrices de transformacin homognea utilizadas en robtica, como ya se
mencion, tienen una componente de posicin y una de orientacin, es decir, estn formadas
por una rotacin y una traslacin. De manera anloga a las rotaciones compuestas, estas
matrices se construyen por medio del producto matricial de las dos matrices (rotacin y
traslacin) dependiendo el orden que resulte conveniente para los clculos. La
Ecuacin 2.6 muestra el caso de una rotacin simple en el eje x seguida de una
traslacin (Barrientos, Pen, Balaguer, & Aracil, 2007).
[ ] Ecuacin 2.6
Este tipo de representacin, donde la rotacin va seguida de la traslacin, es la utilizada

en los clculos posteriores, con la diferencia de incluir las rotaciones en los tres ejes como se
explicar en la prxima seccin.
Por ltimo, entre los mtodos de representacin de rotaciones se encuentran los
cuaternios que pueden ser utilizados como herramienta matemtica de gran versatilidad
computacional para trabajar con giros y orientaciones. [...] Un cuaternio Q est constituido por
cuatro componentes que representan las coordenadas del cuaternio en una base
(Barrientos, Pen, Balaguer, & Aracil, 2007). Tambin mencionan estos autores el
hecho de la simplicidad que los cuaternios implican en cuanto a recursos computacionales,
23
pues utilizan un reducido nmero de componentes (slo cuatro); adems de que los
procedimientos para utilizarlos son tan simples como sumas o productos.
En este punto resulta importante explicar la relacin del concepto de cadena cinemtica
con el de robot. Barrientos y otros (2007, pg. 32) afirman que una cadena cinemtica es una
serie de eslabones o barras unidas por articulaciones", y sealan una clasificacin de dichas
cadenas, tal que una cadena cinemtica cerrada es aquella en la que se puede llegar de un
eslabn a otro por medio de dos caminos distintos y una cadena cinemtica abierta, en cambio,
es en la que existe un nico camino como conexin entre eslabones. Fu y otros (1987), en
cambio, apuntan que un manipulador mecnico se puede modelar como una cadena
articulada en lazo abierto con algunos cuerpos rgidos (elementos) conectados en series por
una articulacin. En ambas definiciones se encuentran los elementos comunes que son los
elementos rgidos unidos por articulaciones, sin embargo la definicin de (Barrientos, Pen,
Balaguer, & Aracil, 2007) parece ms precisa, pues incluye tambin a los robot paralelos que
implican precisamente cadenas cinemticas cerradas.
As se tiene que, para lograr la descripcin del elemento terminal del robot en la
orientacin deseada, es necesario considerar el movimiento relativo de cada uno de los
cuerpos rgidos que constituyen una cadena cinemtica con respecto a un sistema de referencia
fijo (Fu, Gonzlez, & Lee, 1987, pg. 13).
El hecho de que un robot est formado por dicha cadena cinemtica implica que los
modelos matemticos planteados lleven a una serie de tantas transformaciones como
elementos rgidos tenga. Tales transformaciones deben efectuarse en serie de acuerdo a los
requerimientos propios de la metodologa seleccionada.
En este punto, que se han mencionado las herramientas matemticas y las necesidades
de los modelos, aparecen dos enfoques distintos para la generacin de los modelos propios de
las cadenas cinemticas que pueden ser mtodos geomtricos y mtodos basados en cambios
de sistemas de referencia.
Los mtodos geomtricos utilizan herramientas trigonomtricas o vectoriales, las cuales
se pueden obtener de manera sencilla para robots de pocos GDL; son mtodos muy prcticos y
que requieren pocos recursos computacionales, sin embargo, tienen la desventaja de no ser
sistematizados, por lo que su implementacin en brazos con mayor nmero de GDL resulta
inviable (Barrientos, Pen, Balaguer, & Aracil, 2007, pgs. 120-122). El software de control
24
del brazo de Donaxi, contaba originalmente con un mtodo geomtrico que consideraba los
primeros tres grados de libertad, especficamente, los tres que se encargan de dar la posicin,
razn por la cual, no se poda encontrar una orientacin exacta, sino que deba ser estimada
por el programador a travs de calibraciones previas.
A diferencia de los mtodos geomtricos, los mtodos basados en cambios de sistemas
de referencia, s pueden ser empleados para brazos de n GDL, debido a que su implementacin
consiste en clculos de matrices de transformacin homognea. El proceso consiste en
encontrar una matriz que permita representar las rotaciones y traslaciones relativas entre dos
eslabones unidos por una articulacin (Barrientos, Pen, Balaguer, & Aracil, 2007, pg. 122);
de esta forma, y tras encontrar todas las matrices correspondientes a cada articulacin, se
realiza una multiplicacin de todas estas matrices para obtener una matriz de Transformacin
T que sea capaz de relacionar la base del robot con el elemento final, siempre en funcin de
los valores de las articulaciones. Dicha multiplicacin debe ser en orden consecutivo partiendo
de la matriz correspondiente a la articulacin ms cercana a la base del robot, y terminando
con la respectiva de la articulacin ms cercana al elemento terminal.
De estos mtodos de matriz de transformacin se desprende el Algoritmo de Denavit-
Hartenberg que es una serie de pasos sistematizados y estandarizados para encontrar la matriz
de transformacin T aplicable a un brazo robtico. El mtodo utilizado en la presente
investigacin corresponde precisamente a la convencin Denavit-Hartenberg, que se eligi por
varias razones: a) la comodidad que representa el uso de las matrices de transformacin
homognea en los clculos y su respectiva interpretacin y b) la facilidad de su
implementacin en cdigo gracias a la existencia de libreras como KDL (ver Captulo 4. ) que
ofrece clases que permiten realizar clculos cinemticos a partir de un modelo generado
siguiendo las reglas de dicha convencin. En el siguiente subtema se explica cmo se llev a
cabo el planteamiento del modelo.
25
2.3. CONVENCIN DENAVIT-HARTENBERG EN MANIPULADOR
DE DONAXI.
Como se mencion en el subtema anterior, se eligi el algoritmo Denavit-Hartenberg

para los clculos de cinemtica del brazo, pues ofrece diversas ventajas como simplificar los
clculos de cinemtica debido a que este mtodo permite reducir de seis parmetros que se
consideran para cada grado de libertad, a un mximo de cuatro parmetros en la correcta
descripcin de cada una de las articulaciones; con los dems mtodos, pueden resultar muchas
matrices de transformacin distintas, y Denavit-Hartenberg ofrece la ventaja de un nmero
finito y reducido de posibles matrices de transformacin para un nico brazo; el hecho de que
sea el mtodo ms difundido, implica que fcilmente se obtiene acceso a informacin sobre l,
y con esto se pudieron encontrar en varias fuentes las ecuaciones necesarias para acelerar los
clculos, pues son ecuaciones ampliamente probadas.
Segn Barrientos y otros (2007), a cada eslabn i del brazo, se liga un sistema de
coordenadas , lo que permite pasar de un eslabn al siguiente mediante 4 transformaciones
bsicas que relacionan el elemento i-1 con el elemento i. Tales transformaciones son:
: Es el ngulo que forman los ejes y medido en un plano perpendicular
al eje , utilizando la regla de la mano derecha. Se trata de un parmetro
variable en articulaciones giratorias.
: Es la distancia a lo largo del eje desde el origen del sistema de
coordenadas (i-1)-simo hasta la interseccin del eje con el eje . Se trata de un
parmetro variable en articulaciones prismticas.
: Es la distancia a lo largo del eje que va desde la interseccin del eje
con el eje hasta el origen del sistema i-simo, en el caso de articulaciones
giratorias. En el caso de articulaciones prismticas, se calcula como la distancia
ms corta entre los ejes y .
: Es el ngulo de separacin del eje y del eje , medido en un plano
perpendicular al eje , utilizando la regla de la mano derecha.
Los mismos autores establecen que esas cuatro transformaciones siguen el orden
establecido por las siguientes ecuaciones:
26
Ecuacin 2.7
[ ][ ][ ][ ]
[ ]
Ecuacin 2.8
Para la correcta determinacin de los cuatro parmetros D-H es necesario seguir ciertos
pasos concretos, los cuales estn especificados en el Anexo A.
En el caso que concierne al presente trabajo, tales parmetros fueron obtenidos con base
en el modelo del brazo tal como lo muestra la Fig. 2.3.
Fig. 2.3. Modelo simplificado del brazo. Muestra la nomenclatura de los pasos 1-3 del
Anexo A.
27
La Fig. 2.3 muestra encerrados en crculos los nmeros asignados a los eslabones; los
cuadrados enumeran las articulaciones; y las lneas punteadas representan los ejes alrededor de
los cuales giran las articulaciones.
Habiendo identificado las articulaciones con que contar tal modelo matemtico, se
ubicaron y orientaron cada uno de los respectivos sistemas de referencia tal como el algoritmo
D-H lo establece. Siempre teniendo en cuenta las tres reglas que (Fu, Gonzlez, & Lee, 1987)
establecen:
1. El eje yace a lo largo del eje de la articulacin.
2. El eje es normal al eje y apunta hacia afuera de l.
3. El eje completa el sistema de coordenadas dextrgiro segn se requiera.
Fig. 2.4. Modelo simplificado del brazo mostrando los sistemas de referencias normalizados.
Izquierda vista lateral. Centro vista frontal. Derecha vista isomtrica. (Pasos 4-9 de Anexo A)
En este punto fueron calculados los cuatro parmetros D-H descritos anteriormente y
presentados en la Tabla 2.1:
28
1 0 18.5
2 22.44 0
3 3.1 0
4 0 0
5 0 15.67
6 15 0
7 0 0
Tabla 2.1. Los cuatro parmetros D-H calculados para cada articulacin.
Tales parmetros se ajustaron, por una parte, tomando las medidas del modelo digital (en
SolidWorks) como del modelo en fsico, lo que nos permite una alta fiabilidad de su
proximidad a la situacin real. De los parmetros encontrados, se utiliz la Ecuacin 2.8 para
generar las matrices de transformacin , las cuales son desarrolladas en las ecuaciones:
Ecuacin 2.9, Ecuacin 2.10, Ecuacin 2.11, Ecuacin 2.12, Ecuacin 2.13, Ecuacin 2.14 y
Ecuacin 2.15:
[ ][ ][ ]
[ ]
[ ]
[ ]
Ecuacin 2.9
29
[ ][ ][ ]
[ ]
[ ]
[ ]
Ecuacin 2.10
[ ][ ][ ]
[ ]
[ ]
[ ]
Ecuacin 2.11
30
( ) ( )
[ ][ ][ ]
( ) ( )
[ ]
( ) ( )
( ) ( ) [ ]
( ) ( )
[ ]

[ ]
Ecuacin 2.12
[ ][ ][ ]
[ ]
[ ]
[ ]
Ecuacin 2.13
31
( ) ( )
[ ][ ][ ]
( ) ( )
[ ]
( ) ( )
( ) ( ) [ ]
( ) ( )
[ ]
Ecuacin 2.14
[ ][ ][ ][ ]
[ ] [ ] [ ]
Ecuacin 2.15
Teniendo estas matrices de transformacin se vuelve sencillo llegar a la matriz de
transformacin T, pues basta con una multiplicacin de todas las matrices en un orden
consecutivo.
Ecuacin 2.16
Esta etapa, sin embargo, resulta bastante compleja para ser calculada en papel, por lo
que se recurri al software wxMaxima para obtener la matriz de transformacin T. El Anexo B
presenta el cdigo utilizado y que permiti encontrar cada una de las componentes de la matriz
T.
[ ]

Ecuacin 2.16 - 1
32

Ecuacin 2.16 - 2

[ ]

Ecuacin 2.16 - 3
[ ]

Ecuacin 2.16 - 4
[ ]

Ecuacin 2.16 - 5

Ecuacin 2.16 - 6
[ ]

Ecuacin 2.16 - 7
33
[ ]

Ecuacin 2.16 - 8
[ ]

Ecuacin 2.16 - 9

Ecuacin 2.16 - 10
[ ]

Ecuacin 2.16 - 11
[ ]

[ ]

Ecuacin 2.16 - 12
Ecuacin 2.16 - 13
Ecuacin 2.16 - 14
Ecuacin 2.16 - 15
34
Ecuacin 2.16 - 16
Con la matriz obtenida en la Ecuacin 2.16 se vuelven bastante
sencillos los clculos de cinemtica directa que sern explicados en la siguiente seccin.
2.4. CINEMTICA DIRECTA DE BRAZO DE DONAXI.
En la seccin anterior se calcul la matriz de transformacin homognea T con el apoyo

del algoritmo Denavit-Hartenberg. Las componentes de tal matriz permiten conocer tanto las
coordenadas XYZ del elemento terminal del manipulador como la matriz de rotacin que
puede ser convertida a la representacin roll-pitch-yaw de ngulos de Euler que representa la
orientacin del manipulador que se est tratando de controlar.
La Ecuacin 2.1 muestra las distintas partes que conforman tal matriz de
transformacin homognea y lo que cada una representa. Por lo que para explicar la forma de
hacer los clculos de cinemtica directa se separ y aisl cada una de las partes que interesan.
Se eligi la representacin de giro, elevacin y desviacin (roll-pitch-yaw) debido a que
es un conjunto de rotaciones fcil de comprender y que puede ser fcilmente analizado por
contar nicamente con tres parmetros en lugar de nueve como la matriz de rotacin. Adems
de que la Kinematics and Dynamics Library de OROCOS cuenta con una funcin que
permite obtener esos tres ngulos automticamente a partir de la matriz de transformacin (ver
Captulo 4. ). De acuerdo con Fu y otros (1987) esta representacin consiste en tres rotaciones:
1. Una rotacin de respecto del eje OX -desviacin (yaw).
2. Una rotacin de respecto del eje OY -elevacin (pitch).
3. Una rotacin de respecto del eje OZ -giro (roll).
Lo que permite encontrar una matriz de rotacin como la siguiente:
35
[ ][ ][ ]
[ ]
Ecuacin 2.17
Dado que la matriz de rotacin ya es conocida a estas alturas, se puede igualar a la
matriz presentada en la Ecuacin 2.17.
[ ] [ ]
Ecuacin 2.18
De esta forma, existen numerosas posibilidades para encontrar los ngulos roll, pitch y
yaw. Barrientos y otros (2007) proponen que se efecte un procedimiento como el siguiente
para encontrar tales ngulos.
Ecuacin 2.19
( ) Ecuacin 2.20
Ecuacin 2.21
( ) Ecuacin 2.22
Ecuacin 2.23
( ) Ecuacin 2.24
Es evidente que tal procedimiento pierde validez cuando pues tal condicin
tiene divisiones que no se pueden calcular, adems de la desventaja de las mltiples soluciones
de la funcin arcoseno. Para eso se puede utilizar el resto de los elementos de la matriz, y as
tener la certeza de los valores de los ngulos buscados. No fue necesario profundizar en esta
36
etapa debido a que, como se mencion arriba, las funciones de la librera de OROCOS
permiten encontrar tales ngulos.
Teniendo la orientacin del actuador final, hace falta encontrar las coordenadas XYZ,
las cuales pueden ser conocidas evaluando los elementos que corresponden a la matriz de
traslacin de T. Tal que:
[ ] [ ] Ecuacin 2.25
De este modo se obtienen las seis variables que describen la posicin del gripper del
brazo robtico que interesa en el presente trabajo, a pesar de que tal proceso se simplific
bastante al ser programado gracias a las libreras utilizadas dentro de ROS, como se explica en
el Captulo 4.
2.5. MTODOS DE CINEMTICA INVERSA.
En las secciones anteriores se explicaron los clculos de cinemtica directa del

manipulador, los cuales consisten en encontrar la posicin y orientacin del efector final dados
los ngulos de cada una de las articulaciones del brazo. Estos clculos, si bien son tiles, no
resultan suficientes en s mismos para el control completo de la manipulacin, pues al buscar
la completa autonoma, es necesario que los sistemas de deteccin sean quienes determinen la
posicin a donde debe ir el manipulador y as interactuar con un entorno dinmico. Con esto
aparece la necesidad de la cinemtica inversa que consiste en encontrar la configuracin de
ngulos de cada una de las articulaciones dada la posicin y la orientacin a la que el brazo
debe llegar.
Como se revis, existen diversas herramientas y mtodos para el clculo de la
cinemtica directa, y de la misma manera existen varias formas de realizar tales clculos de
cinemtica inversa. A continuacin se presentan algunas de las ms importantes.
37
2.5.1. Mtodos Geomtricos.
Barrientos y otros (2007) proponen herramientas algebraicas para resolver el problema
cinemtico inverso por mtodos geomtricos, los cuales simplifican mucho los clculos
cuando se trata de brazos robticos con pocos grados de libertad como es el caso, por ejemplo,
de los que tienen su espacio de trabajo restringido a un plano. Por esta razn no es viable
implementarlos en el presente trabajo.
2.5.2. Transformacin inversa.

(Barrientos, Pen, Balaguer, & Aracil, 2007) y (Jazar, 2006) tambin proponen
herramientas para despejar las variables articulares a partir de la matriz de transformacin
homognea. Este mtodo parte de la separacin de los grados de libertad del robot en dos
secciones, los primeros darn la posicin del elemento terminal y los siguientes darn la
orientacin. En un robot ideal de seis grados de libertad, se podra decir que los tres primeros
se encargan de ubicar la herramienta en el espacio de tres dimensiones, mientras las tres
restantes son las responsables de dar la orientacin requerida (roll-pitch-yaw).
De acuerdo con Barrientos y otros (2007) el primer paso consiste en encontrar la matriz
de transformacin homognea que relacione el sistema de referencia con el sistema ,
lo que implica las transformaciones correspondientes a los tres primeros grados de libertad,
todo esto teniendo en cuenta un manipulador de seis GDL. Lo que se realiza a continuacin es
una serie de pasos para despejar las coordenadas articulares de los vectores n, o, a y p de la
matriz de transformacin.
Para los clculos expresados a continuacin, T se presenta como la matriz de
transformacin homognea de los primeros tres grados de libertad. Ahora bien, de la ecuacin
se pueden proponer las siguientes ecuaciones, tomadas de (Barrientos, Pen,
Balaguer, & Aracil, 2007, pg. 140):
Ecuacin 2.26
Ecuacin 2.27
Si se tiene en cuenta que la inversa de una matriz de transformacin homognea, segn
Barrientos y otros (2007, pg. 141), est dada por la Ecuacin 2.28.
38
[ ] Ecuacin 2.28
[ ]
Y dado que los valores numricos que toman los elementos de T pueden ser conocidos
con la cinemtica directa, se puede deducir que en el lado izquierdo de la
Ecuacin 2.26 se obtiene una expresin nicamente en trminos de , y en el lado
derecho est expresado nicamente en trminos de y , de este modo, al tenerlo aislado no
resulta complicado encontrar . De la Ecuacin 2.27, es sencillo
encontrar , pues se tiene despejado en el lado izquierdo ahora que se conoce , mientras
que en el lado derecho est slo en trminos de . Este ltimo puede ser calculado de manera
sencilla ahora que se conocen los otros dos trminos.
ste mtodo tambin es aplicable para encontrar las seis variables articulares tal como
Jazar (2006) lo describe utilizando n nmero de matrices inversas y de ecuaciones para
encontrar las articulaciones, el inconveniente de el uso de este mtodo de esta forma es la
complejidad que toman los clculos de esta manera. Es por eso que Barrientos y otros (2007)
proponen obtener las siguientes tres articulaciones por medio del desacoplo cinemtico.
2.5.3. Desacoplo Cinemtico.

De acuerdo con Jazar (2006), se pueden hacer los clculos de cinemtica inversa por
medio de despejes directos de las variables articulares en el caso de que se aslen el vector de
traslacin para las primeras tres articulaciones ( , , ). Y en una segunda etapa, calcular
las tres articulaciones restantes ( , , ), que se pueden encontrar por medio de la matriz
de rotacin deseada y las tres primeras articulaciones que se encontraron en la primer etapa.
El mtodo de desacoplo cinemtico funciona cuando las variables ms prximas al
gripper coinciden en un nico punto generando una articulacin de tipo esfrico (Barrientos,
Pen, Balaguer, & Aracil, 2007) y (Jazar, 2006), es decir que estas ltimas variables se
encargan de la orientacin del manipulador, lo que permite describir la posicin nicamente
con las tres primeras articulaciones.
Por cuestiones de complejidad, Barrientos y otros (2007) tambin recomiendan el uso
combinado del desacoplo cinemtico con la transformacin inversa, de modo que las tres
39
ltimas articulaciones se calculen por despejes directos ya que se conoce la configuracin
necesaria para obtener la posicin deseada.
2.5.4. Tcnicas Iterativas.

El problema cinemtico inverso puede ser interpretado como la bsqueda de la solucin
de un conjunto de ecuaciones algebraicas no lineales [...] (Jazar, 2006).
Existen diversas tcnicas iterativas que intentan resolver el problema cinemtico inverso,
entre las cuales est el mtodo de la Jacobiana inversa. De acuerdo con Barinka y Berka (s.f.),
existe una relacin entre el espacio cartesiano del elemento terminal y las variables angulares
de las articulaciones, as, la matriz Jacobiana transforma los cambios diferenciales en los
ngulos a los movimientos diferenciales del elemento terminal. De este modo, se plantea lo
siguiente:
Ecuacin 2.29
Dado que en este caso, lo que se pretende encontrar es la velocidad angular para cada
articulacin, se despeja:
Ecuacin 2.30
As en (Barinka & Berka, s.f.) se plantea la resolucin en dos etapas: primero se calcula
la transformacin parcial con los ngulos de las articulaciones, se encuentra la posicin del
efector final y la matriz Jacobiana; en la segunda etapa se hace la inversin de la matriz
Jacobiana, y con la Ecuacin 2.30 los valores articulares cambian,
entonces se regresa a la primer fase para encontrar la nueva posicin del elemento terminal.
Jazar (2006) propone un mtodo un poco distinto, pero bastante similar, el cual se basa
en el siguiente algoritmo:
1. Ajustar un contador a cero.
2. Encontrar o proponer una estimacin inicial para las articulaciones: .
3. Calcular el residuo (o error): . Si cada elemento de
o su norma es menor a una tolerancia entonces se puede dejar
de iterar, y ser la solucin.
4. Calcular .
5. Calcular i = i+1 y regresar al paso 3.
40
Barinka & Berka (s.f.) tambin presentan algunas alternativas ms para mtodos
iterativos, como el mtodo basado en optimizacin, el cual de la misma forma busca
minimizar el error que se tiene a partir de una propuesta o suposicin inicial. La frmula del
error quedara como:
Ecuacin 2.31
Donde es la funcin de error, P es la posicin objetivo y es la posicin actual.
El descenso cclico de coordenadas, explicado por Barinka & Berka (s.f.) es un mtodo
que optimiza de manera separada cada una de las articulaciones. Los pasos son ordenados del
segmento ms lejano al segmento de base, modificando nicamente una variable articular por
paso.
Tambin dan una introduccin a la programacin gentica, la cual es una aproximacin
utilizada para minimizar un proceso. La solucin local implica utilizarla como mtodo
numrico para simplificar una ecuacin, como la Ecuacin 2.31 de
modo que permita encontrar la posicin deseada, o la solucin global que puede ser utilizada
para optimizar el movimiento de un modelo como un todo, es decir, produciendo la secuencia
de movimientos. El mtodo consiste en generar una serie de aproximaciones y combinarlas,
para que la nueva generacin est formada por la seleccin de las mejores soluciones.
Por ltimo, Barinka & Berka (s.f.) proponen un mtodo muy similar al de la matriz
Jacobiana inversa, pero que se basa en una matriz Jacobiana transpuesta, la cual parte del
principio de trabajo virtual y fuerzas generalizadas, de modo que la fuerza F es proporcional a
la velocidad angular tal como lo expresa la Ecuacin
2.32.
Ecuacin 2.32
Con base en la revisin y anlisis de los mtodos previamente citados, se evalu la
posibilidad de generar un algoritmo que recoge algunas de las caractersticas mostradas, a la
par que simplifica algunas etapas y sugiere algunos cambios con el propsito de lograr
resolver el problema cinemtico inverso del brazo robtico que es motivo de estudio en el
presente trabajo.
41
2.6. VIABILIDAD DE MTODOS DE CINEMTICA INVERSA EN
ROBOT DE SERVICIO DONAXI.
En la seccin anterior se presentaron diversas propuestas que pueden ser utilizadas para
resolver el problema cinemtico inverso, sin embargo hay algunas consideraciones que deben
hacerse en cuanto al brazo en particular con el que se est trabajando.
Como se ha explicado, el manipulador cuenta con cinco grados de libertad, lo que limita
en cierta forma las posibilidades de sus movimientos, a diferencia de una cadena de seis
variables articulares. Matemticamente se puede observar a travs de las frmulas de
cinemtica directa, que el brazo tiene movimiento en X, Y y Z, adems de presentar rotaciones
que impliquen las tres variables utilizadas (roll-pitch-yaw). Sin embargo, a travs de la
observacin y la experiencia de programar tal brazo, se puede notar que una de las rotaciones
se altera solamente de manera indirecta y depende forzosamente de la configuracin del brazo
en general. Es decir que la variable pitch, que representa la rotacin con respecto al eje Y del
gripper, no puede ser controlada directamente por las articulaciones, sino que es la posicin
final quien obliga a tal variable a tomar algn valor determinado. Tal dificultad se debe al
diseo y distribucin mecnica de las articulaciones del brazo desde la etapa de diseo, lo que
condiciona la programacin y por ende el presente trabajo.
Esta dependencia del valor de pitch con respecto a la posicin final es un problema muy
complejo en s mismo. La lgica de la resolucin del problema cinemtico inverso implica que
se requiere como punto de partida la matriz de transformacin homognea con las posiciones y
rotaciones finales deseadas para el manipulador; sin embargo, por lo expresado en el prrafo
anterior, el pitch no puede ser calculado sino hasta que se conoce la configuracin general del
brazo; tal configuracin slo la podemos conocer a travs de la cinemtica inversa; esto
implica que la rotacin pitch se vuelve una ms de las variables que la resolucin de la
cinemtica inversa nos debe proporcionar.
Lo explicado en el prrafo anterior conduce a la premisa de que la eleccin de un
mtodo para resolver el problema cinemtico inverso debe forzosamente implicar una lgica y
una serie de ecuaciones en las cuales el valor de pitch pueda no ser requerido, y que una vez
que se resolvi la cinemtica, es cuando tal rotacin se vuelve conocida.
42
En lo que respecta a los mtodos geomtricos, tienen el inconveniente de no ser
adecuados para robots con muchos grados de libertad, pues su planteamiento no est basado
en metodologas definidas, lo que para un robot de cinco grados de libertad implica una gran
complejidad en los clculos.
Para poder implementar el mtodo conocido como desacoplo cinemtico es
indispensable que el robot cuente con ciertas caractersticas en su morfologa; es decir, como
se explica en la Seccin 2.5.3, que las tres ltimas articulaciones coincidan en un mismo
punto, lo que resulta en una articulacin de tipo esfrica. Como se muestra en la Fig. 2.4, el
brazo del robot Donaxi no cumple con esta caracterstica, por lo que no es posible
implementar tal mtodo.
La cinemtica inversa por medio de la transformacin inversa, es un mtodo bastante
prctico y universal para lograr una resolucin directa, secuencial y con un bajo consumo de
recursos computacionales; sin embargo, tras lo explicado en prrafos anteriores, resulta
evidente que no puede ser implementado. Esto debido a que el mtodo exige conocer la matriz
de transformacin homognea (T) deseada al final del movimiento del robot, para la cual el
valor de pitch debe ser conocido antes de iniciar el proceso.
Esto conduce a nuevas condiciones para la seleccin del mtodo: debe ser un algoritmo
iterativo, pues las tcnicas matemticas directas para resolver el problema requieren por fuerza
del conocimiento previo de la matriz de rotacin. Se vuelve crucial, entonces, el comenzar a
evaluar los mtodos iterativos para la obtencin de las variables articulares del robot. Una de
las principales complicaciones de los mtodos de cinemtica inversa es el hecho de que todos
los existentes ofrecen ms de una solucin, y aunque para los mtodos directos, de una forma
o de otra pueden conocerse varias de esas soluciones, es posible seleccionar la ms adecuada o
econmica para el robot. Sin embargo, los mtodos iterativos tienen la complicacin de que si
logran converger en una configuracin para las articulaciones, stos entregan una sola
solucin, y no es posible determinar si es la mejor solucin posible para el brazo. As se llega
a un requisito ms: el mtodo debe poder orientarse hacia la posibilidad de converger en la
solucin ms econmica, no slo en cualquiera de las soluciones posibles.
Teniendo en cuenta tales consideraciones, se evaluaron las aproximaciones iterativas.
Una categora de stas son los mtodos basados en optimizacin, los cuales ofrecen una
caracterstica muy interesante que es la de aproximar una a una las articulaciones del robot que
43
se estudia. Sin embargo, el mtodo no se puede implementar en su forma pura, pues
nuevamente exige el conocimiento de la matriz P final para cada iteracin. Es importante
mencionar que la idea de optimizar una sola articulacin por iteracin ofrece buenos
resultados como se explica en la siguiente seccin.
Para resolver la cinemtica inversa de robots se llegan a utilizar tambin algoritmos
genticos, los cuales si bien son bastante tiles en casos donde es necesario encontrar y
simplificar un modelo matemtico complejo, tienen diversos puntos en contra. Uno de ellos, es
el hecho de que no garantiza encontrar la solucin ms adecuada, pues para funcionar genera
mutaciones al azar derivadas de un conjunto previo de posibles soluciones que se tienen y
selecciona a las ms adecuadas, y el proceso se repite durante una cierta cantidad de
generaciones, as elige a la aproximacin ms apta. Esto adems implica una gran
cantidad de recursos computacionales, pues entre mejor se desee aproximar la solucin, ms
generaciones es conveniente procesar.
Tambin dentro de las aproximaciones iterativas estn los mtodos de la matriz
Jacobiana inversa y de la matriz Jacobiana transpuesta. Estos procedimientos fueron revisados
hasta que se tenan importantes avances con la propuesta de cinemtica inversa explicada en la
siguiente seccin. No se profundiz en su estudio, pues implicaba un retroceso en el desarrollo
a la par de que no se tiene garanta de la funcionalidad del mtodo en el brazo que se estudi.
Esto se debe a que existen casos particulares en que una matriz Jacobiana no tiene inversa, los
cuales estn condicionados por el diseo mecnico inicial del brazo, tales casos implican
eliminar grados de libertad de los clculos (Barrientos, Pen, Balaguer, & Aracil, 2007),
efecto indeseable para el robot. Tambin los clculos por medio de matrices Jacobianas exigen
el conocimiento de las funciones de velocidad articulares y del efector final, lo que incrementa
la cantidad de recursos computacionales requeridos para el robot.
44
2.7. PROPUESTA DE CINEMTICA INVERSA PARA
MANIPULADOR DE DONAXI.
Como se comentaba en la seccin anterior, debido a las caractersticas propias del

prototipo, resulta conveniente un mtodo iterativo que permita descartar alguno o algunos de
los parmetros de la matriz de transformacin, y que a la par permita orientar sus resultados
hacia la solucin ms econmica. Por tales razones se plante lo siguiente.
Dado que, como se ha repetido mucho, el valor de pitch no es conocido, el
procedimiento de cinemtica inversa puede ser considerado la bsqueda de un camino, el cual
implicara una serie de pasos discretos para cada articulacin, lo que permitira alcanzar al
gripper tanto la posicin como la orientacin deseada, a la par que se llega al valor de pitch
adecuado para la posicin sin el conocimiento previo del mismo.
Tales pasos discretos de las articulaciones resulta conveniente calcularlos en una sola
articulacin a la vez, como ocurre con los mtodos de optimizacin, para que de esta forma
pueda ser evaluado el efecto que los distintos cambios posibles puedan tener en la posicin del
elemento terminal.
Gran parte de los mtodos iterativos requieren de una aproximacin inicial que puede ser
arbitraria y la funcin del algoritmo se limita a corregir el error que produce tal aproximacin.
Es por esto que entre ms cercana sea tal aproximacin a la posicin deseada, mayores
probabilidades de converger en la posicin idnea. De esta forma es posible trazar el
movimiento del robot si se calcula punto a punto de la ruta deseada para que el manipulador
pueda alcanzar su objetivo. Esto, sin duda, plantea una nueva necesidad: la implementacin de
un algoritmo que permita trazar las rutas que debe seguir el manipulador para realizar sus
tareas, lo cual ser explicado en secciones posteriores. Este algoritmo, por tanto, va a permitir
a la cinemtica inversa realizar las aproximaciones en pasos pequeos teniendo siempre una
aproximacin inicial bastante cercana a la posicin deseada, pues la configuracin del paso
anterior, va a diferir en un error muy pequeo en comparacin con el nuevo punto deseado.
La minimizacin del error resulta ser un asunto clave en la cinemtica inversa, por lo
que tiene un gran nfasis el clculo de tal error de modo que pueda ser interpretado de la
manera correcta.
45
Teniendo en cuenta las consideraciones de los cuatro prrafos anteriores, se trabaj en
un algoritmo un poco extenso pero que toma en cuenta las necesidades particulares, el cual es
presentado en el Algoritmo 2.1, donde se mencionan distintas variables, estructuras, matrices,
clases y dems.
Las estructuras de tipo InstruccionesInterfaz contienen los datos de posicin y velocidad
para cada uno de los servomotores que conforman el brazo, as previous contiene los datos
que sirven a modo de posicin inicial del brazo, que se tomar como punto de partida.
Las clases Frame, que sern explicadas en el Captulo 4. , pueden entenderse como una
matriz de transformacin que da un vector de posicin y una matriz de rotacin, de esta forma
el Frame goal contiene la posicin y la orientacin a las que se pretende llegar con el
algoritmo. En el algoritmo se utilizan algunos Frames que tambin son importantes. Present
guarda la posicin inicial como se ve en los pasos 2 y 3. Por su parte, iteration contiene la
posicin que alcanza el manipulador en una iteracin determinada, y finalmente trial
contiene la posicin que alcanza el manipulador para valores de prueba y que se utilizan
durante etapas muy breves del algoritmo.
El tipo arreglo de articulaciones, que se explicar tambin en el Captulo 4. , contiene los
valores de los ngulos de cada articulacin que utiliza el solver de cinemtica directa, de
forma que presentpos, iterapos y trialpos contienen las configuraciones: inicial, de
iteracin y de prueba respectivamente.
Algoritmo 2.1
KinematicsSolverDonaxi con parmetros de entrada: previous (tipo
InstruccionesInterfaz), goal (tipo Frame), freq (tipo double).
1. Definicin e inicializacin de variables, estructuras, clases y matrices
a utilizar:
1.1. Definicin de present, iteration y trial de tipo Frame.
1.2. Definicin de clase chain como la cadena cinemtica representativa
del brazo y sus respectivos segmentos.
1.3. Definicin de clase fksolver para cinemtica directa con parmetros
de entrada chain.
1.4. Definicin de presentpos, iterapos, trialpos de tipo arreglo de
articulaciones.
2. Inicializar presentpos con los valores de previous por ser la posicin
inicial.
3. Calcular cinemtica directa con parmetro de entrada presentpos y guardar
resultados en present.
4. Definir iterapos e iteration iguales a presentpos y present
respectivamente.
5. Calcular errores iniciales y sumarlos en errsum.
46
6. Repetir mientras terminar sea falso y numero_de_pasos sea menor que
numero_de_columnas:
6.1. Comparar los distintos errores en X, Y, Z, Roll y Yaw y elegir el
mayor.
6.2. Incrementar nmero de pasos.
6.3. Prueba de casos.
6.4. Si hubo casos que reduzcan el error:
6.4.1. Identificar el costo menor.
6.4.2. Aplicar_caso con parmetros de entrada iterapos y el ltimo
paso del camino con el costo menor.
6.4.3. Evaluar_caso con parmetros de entrada iterapos.
6.4.4. Guardar caso en matriz itepath, que es el camino de la
iteracin actual y se borra de paths, pues como se explorar en
la siguiente iteracin ya no ser necesario guardarlo.
6.4.5. Evaluar y sumar todos los errores.
6.4.6. Si la suma de errores es menor que una tolerancia:
6.4.6.1. Activar bandera de solucin encontrada y activar bandera
de terminar iteraciones.
6.5. Si no:
6.5.1. Repetir Mientras romper_ciclo sea falso:
6.5.1.1. Descartar ruta.
6.5.1.2. Decrementar nmero de pasos.
6.5.1.3. Si el nmero de pasos es cero o igual a
numero_de_columnas:
6.5.1.3.1. Terminar, pues no se puede encontrar una solucin.
6.5.1.4. Si no:
6.5.1.4.1. Seleccionar el menor costo de los caminos restantes
en la matriz.
6.5.1.4.2. Si la fila del menor costo es distinta al mximo de
columnas:
6.5.1.4.2.1. Aplicar los pasos del camino elegido a
presentpos y guardarlos en iterapos, luego
eliminarlos de la matriz paths.
6.5.1.4.2.2. Evaluar la cinemtica directa para el camino
elegido.
6.5.1.4.2.3. Evaluar y sumar los errores.
6.5.1.4.2.4. Romper_ciclo igual a verdadero.
6.5.1.4.3. Si no:
6.5.1.4.3.1. Romper_ciclo igual a falso.
7. Solucin igual a previous.
8. Si solucin encontrada es igual a verdadero:
8.1. Solucin igual a iterapos.
8.2. Calcular velocidades y guardarlas en solucin.
9. Regresar solucin.
Es importante observar el paso 6.1 del Algoritmo 2.1, que consiste en una pequea
secuencia de condicionales para guardar en una variable un valor entero que se identifica
como el eje que tiene un error mayor, y donde el error en pitch es ignorado por todo lo que se
ha explicado previamente.
47
Existen algunos pasos que en s mismos no dejan muy clara su funcin, tal es el caso del
paso 6.3 Prueba de casos, el cual se encarga de escribir en la matriz paths todos los casos
que reducen el error. El Algoritmo 2.2 explica su funcionamiento.
Los llamados casos son tipos de cambios discretos que pueden ser aplicados a las
articulaciones, e implican incrementos o decrementos de una o dos articulaciones nicamente,
siguiendo el ejemplo de los mtodos basados en optimizacin; se tiene un total de 22 casos,
los cuales estn pensados para reducir el nmero de iteraciones requeridas para lograr la
convergencia. Adems, los casos consideran la necesidad de nunca exceder los lmites de
movimiento de los servomotores. El Algoritmo 2.3 explica los casos y su implementacin.
Algoritmo 2.2
6.3. Prueba de casos.
6.3.1. Inicializar ndice_de_filas, ndice_de_casos y bandera que
indica si algn caso mejora el error.
6.3.2. Repetir mientras ndice_de_casos sea menos que 22:
6.3.2.1. Incremento al ndice de casos.
6.3.2.2. Aplicar_caso con parmetros de entrada iterapos y
case_index y guardar en trialpos.
6.3.2.3. Evaluar trialerror, que es el error de trialpos con
respecto a goal en el eje seleccionado en 6.1.
6.3.2.4. Si trialerror es menor que error mximo encontrado en 6.1:
6.3.2.4.1. Guardar que hubo casos que mejoran.
6.3.2.4.2. Buscar una fila en blanco de la matriz costs, si no
hubiera filas en blanco, terminar.
6.3.2.4.3. Evaluar_costo con parmetros de entrada trialpos y
goal.
6.3.2.4.4. Escribir en la matriz paths el camino que corresponde
al caso.
En el paso 6.3.2.1, se llama a una funcin que no slo incrementa el valor del caso, sino
que adems evita que se evalen casos que hagan la funcin contraria a la que se efectu en la
iteracin previa, esto es importante para reducir la posibilidad de que el algoritmo se cicle
evaluando una y otra vez los mismos casos.
En el paso 6.3.2.2 ocurre un llamado a la funcin aplicar_caso, la cual se encarga de
hacer los incrementos o decrementos correspondientes al caso seleccionado a un determinado
arreglo de articulaciones. Esta etapa corresponde al Algoritmo 2.3.
48
Algoritmo 2.3
Aplicar_caso con parmetros de entrada: arreglo de articulaciones
jointconfig, entero case_index.
1. Segn case_index hacer:
1.1. Caso 1:
1.1.1. Incrementar jointconfig(0) en MIN_CHANGE.
1.2. Caso 2:
1.2.1. Decrementar jointconfig(0) en MAX_CHANGE.
1.3. Caso 3:
1.4. Caso 4:
1.5. Caso 5:
1.6. Caso 6:
1.7. Caso 7:
1.8. Caso 8:
1.9. Caso 9:
1.10. Caso 10:
1.11. Caso 11:
1.12. Caso 12:
1.13. Caso 13:
1.13.2. Incrementar jointconfig(2) en MAX_CHANGE.
1.14. Caso 14:
1.15. Caso 15:
1.16. Caso 16:
1.17. Caso 17:
1.18. Caso 18:
1.19. Caso 19:
1.20. Caso 20:
1.21. Caso 21:
49
1.22. Caso 22:
2. Verificar lmites de articulaciones:
2.1. jointconfig(0):
2.1.1. Lmite superior 156.
2.1.2. Lmite inferior 6.
3. Si articulacin_modificada es menor a 360:
3.1. Restar 360 a la articulacin.
4. Si articulacion_modificada <0:
4.1. Sumar 360 a la articulacin.
En tal funcin es observable que no solamente se cuidan los lmites de las distintas
articulaciones, sino que tambin se cuida no tener valores de ngulos negativos ni mayores a
360. Esto es muy importante, pues al mantener los ngulos dentro de rangos controlados es
considerablemente ms sencillo efectuar el control de los servomotores. Por otra parte, durante
la depuracin, se observ que ocurran ocasiones en que el algoritmo se ciclaba tras una serie
compleja e impredecible de casos contrarios, regresando a estados de iteraciones previas; por
tales razones se opt por el uso de dos constantes distintas, una de un valor mayor que la otra,
utilizando una para incrementos y la otra para decrementos, causando que en tales casos, sea
mucho ms improbable que el cdigo se cicle, pues se vuelve mucho ms complicado hacer
que se regrese a un estado de iteraciones previas.
Para el caso 6.3.2.4.3 se llama a una funcin llamada evaluar_costo. Esta funcin recibe
tal nombre porque se origin a raz de la inquietud de optimizar el algoritmo por medio del
concepto de costo propio del algoritmo de bsqueda A*, el cual se forma de la suma de la
distancia del punto origen a un punto intermedio en el camino, ms la distancia del punto
intermedio al punto objetivo (Popirlan & Dupac, 2009); esto en teora, permitira que cada
iteracin mantuviera lo ms cerca posible al desplazamiento del manipulador en el camino
ms corto al objetivo. Sin embargo, en la prctica se observ que tal suma no resultaba
50
conveniente, pues el hecho de incluir la distancia del punto inicial al de la iteracin, causa en
ocasiones que el algoritmo regrese a estados de iteraciones anteriores, esto se debe a que de
esa manera no hay forma de reconocer cul de los dos parmetros es el que disminuye y cual
aumenta. Por esa razn, se opt por eliminar el clculo del parmetro de distancia del punto
inicial al de la iteracin, y qued nicamente como la evaluacin de la distancia del punto de
la iteracin al punto objetivo, es decir, la suma de los valores absolutos de los errores.
2.8. PLANIFICACIN DE RUTAS PARA MANIPULACIN.
Ya se explic cmo se calcula la cinemtica inversa del manipulador del robot, sin
embargo tales clculos funcionan para valores puntuales. Con esto, si nicamente se mandan
los valores articulares a los servomotores, es lgico que el brazo alcance la posicin deseada;
sin embargo, no es suficiente con hacer eso, pues dado que cada servomotor se mover una
distinta cantidad de grados, la trayectoria que se seguira sera impredecible. Esto representa
un peligro para tanto el robot como para su entorno, pues puede ocasionar colisiones. Para
evitar estos problemas, en robtica se utilizan mtodos de planificacin de rutas para
manipulacin, que permiten calcular el camino que debe seguir el manipulador y que le
permita cumplir con su objetivo de manipulacin as como hacerlo de una manera segura.
El problema del control del manipulador se puede dividir en dos partes: planeacin de la
trayectoria y control del movimiento (Fu, Gonzlez, & Lee, 1987). Estas dos partes estn
fuertemente relacionadas, sin embargo, en esta seccin se hablar de la planeacin de
trayectorias.
Jazar (2006) explica que la planeacin implica tres etapas: primero est la definicin de
una curva geomtrica para el elemento terminal entre dos puntos, en segundo lugar, se define
un movimiento rotacional y finalmente la definicin de una funcin de tiempo para
variaciones en las coordenadas. Tambin Lewis y otros (2004) defienden que la planificacin
implica encontrar una ruta prescrita, que en ocasiones se trata por separado e incluye evasin
de colisiones, saturacin de actuadores y dems.
51
Las funciones del control cinemtico, de acuerdo con Barrientos y otros (2007), deben
ser la conversin del movimiento a una trayectoria en el espacio cartesiano y su relacin con el
tiempo; la obtencin de un nmero finito de puntos de tal trayectoria; conversin de tales
puntos a coordenadas articulares por medio de cinemtica inversa siguiendo con la generacin
de una funcin del tiempo para cada articulacin; y finalmente un muestreo de tal trayectoria
articular para generar referencias al control dinmico. Todo esto teniendo siempre en cuenta el
espacio de trabajo y las velocidades mximas de los actuadores. Tales funciones son de una
vital importancia, pues como se ver en los mtodos de planificacin explicados adelante, para
evitar movimientos caticos en los manipuladores es importante cubrir tales clculos.
2.8.1. Tipos de Trayectorias.

Existen distintos tipos de trayectorias y por tanto distintos mtodos para trazarlas. Sern
explicadas en las clasificaciones siguientes.
Trayectorias punto a punto. Barrientos y otros (2007) explican que en este tipo,
cada articulacin evoluciona sin tener en cuenta el estado de las dems, tratando
cada una de alcanzar el punto de destino en el menor tiempo. Adems dividen
estas trayectorias en dos tipos:
a) Movimiento eje a eje. En el cual solo hay movimiento en un eje a la vez.
b) Movimiento simultneo de ejes. Todos inician el movimiento al mismo
tiempo, pero como cada articulacin debe moverse una distancia distinta,
algunas tardan ms que otras.
Trayectorias coordinadas o iscronas. Para estas trayectorias lo que se hace es
ralentizar el movimiento de las articulaciones que les toma menos tiempo para
adaptarlas al tiempo que requiere la articulacin ms lenta para completar su
movimiento. Sin embargo, tambin representa trayectorias impredecibles para el
usuario (Barrientos, Pen, Balaguer, & Aracil, 2007).
Trayectorias continuas. De acuerdo con Barrientos y otros (2007), es necesario
calcular de manera continua las trayectorias articulares cuando se requiere que el
usuario conozca de antemano la trayectoria del robot.
52
2.8.2. Generacin de Trayectorias.
En los robots donde la trayectoria del manipulador es importante, no hay otra opcin que
utilizar las trayectorias continuas, para lo cual se utilizan interpoladores entre los puntos inicial
y final que buscan ajustar lo ms posible al tipo de trayectoria deseada que ms comnmente
son lneas rectas o segmentos de crculos.
De acuerdo con (Fu, Gonzlez, & Lee, 1987) existen dos aproximaciones para poder
planificar tales trayectorias: en la primera, el usuario debe especificar el conjunto de limitantes
en posicin, velocidad y aceleracin en los diversos puntos de la trayectoria, para que luego el
planificador seleccione una trayectoria parametrizada con una funcin que interpola y
satisface las limitantes especificadas para los puntos, esta aproximacin tiene el inconveniente
de que si existieran obstculos no es posible evitarlos; en la segunda aproximacin, el usuario
especifica la trayectoria que el manipulador debe seguir por una funcin analtica, es decir que
se dan los medios para conocer la trayectoria a travs de coordenadas cartesianas, lo que
implica que el planificador debe ser capaz de convertir tales coordenadas cartesianas a
coordenadas articulares. Esta segunda aproximacin resulta ser un punto de partida para el
control del brazo de Donaxi como se detalla en la siguiente seccin.
La primera aproximacin, explican Fu y otros (1987), que tiene las ventajas de ser ms
simple para planificar las trayectorias de las articulaciones; sin embargo resulta ms
complicado conocer la posicin del elemento terminal en los distintos momentos de la
trayectoria. Esto resulta en un algoritmo muy simple que altera las distintas posiciones para
cada articulacin en funcin del tiempo. Estas posiciones deben considerar las limitantes
propuestas. Por lo cual resulta conveniente el planteamiento de secuencias polinmicas, las
cuales permitiran garantizar la continuidad del movimiento.
Para eso existen diversas tcnicas explicadas por Barrientos y otros (2007) como:
interpoladores lineales, en los cuales no se tienen variaciones de velocidad; cbicos, que
utilizan polinomios de tercer grado para unir dos puntos e implican control continuo de la
velocidad, pues sta vara en el tiempo; a tramos, que utilizan la misma velocidad del punto
intermedio, as ya no es necesario tener que acelerar y desacelerar ms que en los puntos
inicial y final; y algunos otros tipos de interpoladores cuyo propsito comn es asegurar
continuidad en la trayectoria. (Jazar, 2006) presentan, adems de las funciones polinmicas, la
posibilidad de utilizar funciones harmnicas o cclicas para la interpolacin de las trayectorias.
53
La segunda aproximacin de que (Fu, Gonzlez, & Lee, 1987) hablan, implica un
algoritmo un poco mayor que consiste en determinar la posicin del efector final en
coordenadas cartesianas en funcin del tiempo y el clculo de las coordenadas articulares para
la posicin especificada. Todo esto para cada cambio en el tiempo.
Barrientos y otros (2007) tambin explican que para la generacin de los puntos, ya sea
con una trayectoria en lnea recta o circular, resulta evidente que el error existente en el
movimiento puede ser reducido enormemente entre mayor sea la cantidad de puntos
seleccionados; sin embargo, tambin es importante tener en cuenta los lmites
computacionales, pues determinar puntos de manera indiscriminada puede resultar
computacionalmente inviable.
Fu y otros (1987) proponen tambin un mtodo para calcular las trayectorias cartesianas
a travs del uso de cuaternios, el cual ofrece algunas ventajas como el reducir el coste
computacional de la planificacin de la trayectoria, sin embargo no se profundizar en l dado
que el uso de cuaternios en el presente trabajo fue descartado en secciones anteriores.
En general, esta aproximacin es utilizada para sistemas ms complejos con tareas ms
especficas que implican movimientos ms especficos, pues una tarea deseada para un robot
es ms sencilla de describir utilizando el espacio cartesiano (Lewis, Dawson, & Abdallah,
2004) aunque de cualquier modo, sea requerido el seguimiento de la trayectoria en el espacio
articular. Esto implica la combinacin de las dos aproximaciones: planteamiento de una
trayectoria en el espacio cartesiano que luego de pie a la planificacin de una trayectoria en el
espacio articular.
2.9. PROPUESTA DE TRAZADO DE RUTAS PARA

MANIPULACIN DE DONAXI.
Como se plante en la Seccin 2.7, para el funcionamiento del algoritmo propuesto de

cinemtica inversa, se requiere una aproximacin inicial lo ms cercana posible a la nueva
posicin deseada. Esto se favorece con el planteamiento de un algoritmo de planificacin de
rutas que entregue punto a punto las coordenadas por las que el brazo debe pasar, pues as se
aprovecha la cercana entre tales puntos para disminuir el nmero de iteraciones.
54
En la seccin anterior se explic la importancia en algunos robots de la utilizacin de un
mtodo que sea capaz de generar una trayectoria cartesiana. En el presente caso resulta vital,
pues en la manipulacin de objetos requerida por Donaxi, no se tiene un espacio
completamente libre para llevar el manipulador al punto deseado. Es decir, pudiera haber otros
objetos en el espacio de trabajo del manipulador, o la presencia de superficies planas donde
tales objetos estaran colocados.
Por tanto se plante la necesidad de generar un algoritmo lo ms simple posible para
generar trayectorias. Para tal mtodo se parti de las siguientes consideraciones:
Dadas las condiciones del sistema de reconocimiento de objetos, si hubiera algn
elemento entre el robot y el objeto deseado, no sera posible determinar la
ubicacin del objetivo, pues la cmara debe colocarse de frente. Esto conduce a
la premisa de que no habr objetos obstruyendo el paso del manipulador en el
camino a tomar el objeto deseado.
Los objetos estarn colocados sobre una superficie plana y horizontal que puede
ser una mesa, una repisa, una estantera, y dems.
La altura de la superficie puede variar, as como la cercana del borde de sta con
el robot.
Las consideraciones anteriores llevan a la consideracin final de que slo es
necesario evitar las colisiones con la superficie sobre la cual se encuentra el
objeto que se pretende tomar.
Observando tales premisas, y teniendo en cuenta que es imperante maximizar la
simpleza del mtodo, para reducir el riesgo de errores, se determin lo que en el presente
trabajo se llamar un patrn de comportamiento.
Este patrn de comportamiento consiste en una serie de movimientos preestablecidos
que se ajustan al caso particular que enfrenta el robot. El patrn para tomar un objeto se puede
resumir en los siguientes pasos:
Algoritmo 2.4
1. Llevar el gripper a una posicin inicial conocida.
2. Realizar un movimiento vertical en lnea recta (eje Y) hasta la altura
especificada a la que se encuentra el objeto. Esta altura debe tener en
cuenta las dimensiones particulares del objeto que se pretende tomar y
las dimensiones del manipulador para evitar colisiones con la
55
superficie.
3. Abrir el gripper.
4. Llevar el gripper en un movimiento en lnea recta sobre el plano
horizontal en que se encuentra (a lo largo de los ejes X y Z) hasta la
coordenada del objeto que se desea tomar.
5. Cerrar gripper.
6. Realizar un movimiento vertical en lnea recta (eje Y) de algunos
centmetros para levantar el objeto tomado.
7. Realizar un movimiento en lnea recta sobre el plano horizontal en que se
encuentra el gripper hasta la posicin inicial de los ejes X y Z.
Tras realizar esa serie de pasos, el manipulador habr tomado el objeto de inters.
Teniendo en cuenta el patrn de comportamiento, es posible analizarlo como una secuencia,
para la cual los pasos se pueden agrupar en dos tipos:
Movimientos fijos: los cuales consisten en cambios en los valores articulares
siempre constantes a velocidades igualmente constante. Se pueden apreciar en
los pasos 1, 3 y 5 del Algoritmo 2.4, pues ellos no dependen en nada de la
posicin del objetivo.
Trayectorias en lnea recta: las cuales estn presentes en los pasos 2, 4, 6 y 7
del Algoritmo 2.4, y cuyas series de coordenadas dependen estrictamente de la
posicin del objetivo, por tanto no sern valores articulares siempre constantes,
sino que variarn y debern ser calculados en tiempo real.
Para el primer tipo, los movimientos fijos, resulta evidente que al ser posiciones
conocidas, el programador especificar manualmente las coordenadas articulares antes de la
ejecucin de la tarea. Este tipo de movimientos es bastante sencillo y se explicar en el
siguiente captulo, en la Seccin 3.5.
El segundo tipo, las trayectorias en lnea recta, deben ser calculadas automticamente
por el robot, por lo que se plante un sencillo algoritmo para interpolar esta serie de puntos
sobre una lnea recta desde un punto inicial a uno final. Teniendo en cuenta el Algoritmo 2.4,
se puede observar que para los pasos 2, 4, 6 y 7 se tienen puntos iniciales y finales conocidos,
es decir que se conocen sus coordenadas cartesianas.
Tales puntos en el espacio sirven matemticamente para encontrar la ecuacin vectorial
de una recta. Tal como explica Grossman (1992), es posible obtener una ecuacin de la forma:
56
Ecuacin 2.33
Donde representa el vector del origen a un punto cualquiera, el vector del origen
a un punto inicial conocido, un nmero real y un vector paralelo a la recta. Por tanto, si se
tiene un punto inicial:
Ecuacin 2.34
Y un punto final:
Ecuacin 2.35
Es posible encontrar un vector director con una simple resta entre el punto inicial y final,
sin embargo, dado que la nica caracterstica til del vector es su direccin, es decir, debe ser
paralelo a la recta sin importar su magnitud (Grossman, 1992), se opt por calcular el vector
unitario que tenga la misma direccin, el cual es multiplicado por una constante c:

(
) Ecuacin 2.36
Es importante considerar que para obtener un movimiento regular resulta conveniente

que se mantenga una misma distancia entre los puntos de la trayectoria, lo cual se vuelve
bastante sencillo si se calcula el vector director con la magnitud igual a la distancia deseada; es
por eso que la constante c debe tener un valor equivalente al paso deseado en el movimiento.
De esta forma, slo es necesario definir dos vectores ms para poder plantear la ecuacin
de la recta. El vector A, que representa las coordenadas actuales del brazo, esto es, la posicin
en la que se encuentra el gripper en un momento determinado, y el vector O, que representa el
siguiente punto a donde debe moverse. Ambos son puntos que sern siempre coincidentes con
la recta.
Ecuacin 2.37
Ecuacin 2.38
As, la ecuacin de la recta quedara as:
Ecuacin 2.39
Y desarrollando:
(
) Ecuacin 2.40
Con tal ecuacin resulta bastante sencillo el clculo de los puntos de la trayectoria por
medio de iteraciones en las cuales, se comienza igualando el punto A a la posicin inicial y
durante el resto de las iteraciones, A simplemente cambia por el punto O de la iteracin
57
anterior. Teniendo en cuenta esto, y el Algoritmo 2.4 se plante el Algoritmo 2.5 que es ms
especfico.
En el Algoritmo 2.5 se tienen ciertas variables que es importante mencionar. La
estructura brazo_obj contiene los parmetros para cada uno de los servomotores a los cuales se
pretende llegar, de mismo modo, brazo_actual, contiene los parmetros del estado de las
articulaciones en se momento especfico de la ejecucin; ambas estructuras son de tipo
InstruccionesInterfaz, los cuales son explicados en la seccin 2.7. Aparecen tambin clases de
tipo Frame: frpresent, frstep y frgoal; las cuales representan posicin (un vector) y orientacin
(una matriz de rotacin); en el caso de frpresent guarda las coordenadas en las que se
encuentra el brazo en cada momento de las distintas etapas, mientras que frstep, indica la
posicin que se pretende alcanzar al finalizar la etapa correspondiente; de la misma forma,
frgoal, guarda las coordenadas donde se encuentra el objeto. Tambin hay vectores utilizados:
distancia guarda el vector que va desde la posicin actual del gripper a la que se pretende
llegar en un momento determinado de la ejecucin.
Algoritmo 2.5
1. Definir brazo_obj igual a valores conocidos.
2. FaseSecuencia con parmetros de entrada brazo_actual, brazo_obj, 5 y
guardar resultado en brazo_actual.
3. Definir posicin actual conocida (coincidente con las coordenadas del
gripper que se alcanzaron con el paso 1) y guardarla en frpresent.
4. Definir frstep igual a frpresent.
5. Ajustar coordenadas a alcanzar:
5.1. Definir la coordenada en y de frstep igual a la coordenada en y de
frgoal.
6. Calcular vector distancia igual a la resta de los vectores de posicin de
frstep frpresent.
7. EvalMagnitude con parmetros de entrada distancia y guardar en magnitud.
8. Calcular vector director con parmetros de entrada distancia y magnitud.
9. Trazar movimiento desde frpresent hasta frstep.
10. Secuencia para abrir gripper:
10.1. Definir brazo_obj igual a brazo_actual.
10.2. Definir valor de servo 6 en abierto.
10.3. FaseSecuencia con parmetros de entrada brazo_actual, brazo_obj, 3
y guardar resultado en brazo_actual.
12.1. Definir la coordenada en x de frstep igual a la coordenada en x de
frgoal.
12.2. Definir la coordenada en z de frstep igual a la coordenada en z de
frgoal.
13. Calcular vector distancia igual a la resta de los vectores de posicin
de frstep frpresent.
58
17. Secuencia para cerrar gripper:
17.1. Definir brazo_obj igual a brazo_actual.
17.2. Definir valor de servo 6 en cerrado.
17.3. FaseSecuencia con parmetros de entrada brazo_actual, brazo_obj, 3
y guardar resultado en brazo_actual.
19.1. Definir la coordenada en y de frstep igual a la coordenada en x de
frgoal + 4.
25.1. Definir la coordenada en x de frstep igual a 0 (posicin inicial).
25.2. Definir la coordenada en z de frstep igual a 18.5 (posicin
inicial).
30. Fin de movimiento.
Como se observan los pasos 2, 10.3 y 17.3 contienen llamadas a la funcin

FaseSecuencia, la cual es explicada en la seccin 3.5. Las llamadas a evalMagnitude resultan
evidentes que su funcin es calcular la magnitud de un vector. En los pasos 8, 15, 22 y 28 se
realizan clculos de vector director, los cuales estn fundamentados en la Ecuacin 2.36.
El caso de los pasos 9, 16, 23 y 29 son donde se encuentra particularmente lo ms
importante del algoritmo; tal trazado de movimiento se detalla en el Algoritmo 2.6.
Algoritmo 2.6
1. Repetir mientras magnitud sea mayor o igual a PASO_RUTA.
1.1. Definir el vector frpresent igual a la suma del vector de frpresent
y director.
1.2. KinematicsSolverDonaxi con parmetros de entrada brazo_actual,
frpresent, FREQUENCY y guardar resultados en brazo_actual.
1.3. Si se recibi mensaje del paro de emergencia entonces
1.3.1. Apagar servomotores.
1.3.2. Fin de Si
1.4. Enviar datos de brazo_actual a interfaz y a simulador.
1.5. Calcular vector distancia igual a la resta de los vectores de
posicin de frstep frpresent.
1.6. EvalMagnitude con parmetro de entrada distancia y guardar en
59
magnitud.
1.7. Verificar mensajes.
1.8. Esperar tiempo requerido por la frecuencia de la interfaz.
1.9. Fin de repetir.
En el Algoritmo 2.6, se observa el uso de una constante llamada PASO_RUTA, la cual

tiene el valor de la distancia que debe recorrer cada pequeo paso en el momento de trazado
de rutas, que adems va acorde con la velocidad esperada y la frecuencia de iteracin del
cdigo. Tambin aparece el resto de la ecuacin de la recta; en el Algoritmo 2.5 se realizan
clculos del vector director en varias etapas, y de manera complementaria, en el Algoritmo
2.6, est el paso 1.1, el cual realiza la suma entre los vectores actual y director cuyo resultado
se guarda nuevamente en el vector actual, para que se contine sumando en iteraciones
sucesivas.
Los algoritmos y clculos explicados en el presente captulo requieren de bastante ms
cdigo que les permita funcionar e interactuar, adems de la vital comunicacin con los
servomotores, lo cual ser explicado en los captulos siguientes.
Una mquina puede hacer el trabajo de 50 hombres corrientes.

Pero no existe ninguna mquina que pueda hacer el trabajo
de un hombre extraordinario.
(Elbert Green Hubbard)
CAPTULO 3. ROS.
En el presente captulo se da una breve introduccin a las arquitecturas de robots y

seguidamente, se explicar la arquitectura desarrollada en el Laboratorio de Robtica y
Control de UPAEP, la cual se utiliza en el Robot de Servicio Donaxi. Se fundamentarn las
razones que llevaron a elegir ROS como la plataforma de programacin para el proyecto, as
como la forma en que se construyen los sistemas sobre ROS, y particularmente cmo est
hecho el software del brazo de Donaxi.
60
61
3.1. ARQUITECTURAS DE CONTROL PARA ROBOTS.
Si se toma en cuenta que los robots se diferencian del resto de las mquinas por su
capacidad de salir de lo repetitivo y adaptarse al problema particular que debe resolver,
implica que sern mquinas con una serie de elementos que les permitan conocer su entorno, a
la par de algunos otros sistemas que les capaciten para actuar sobre todo lo que lo rodea. Esto
deja entrever la necesidad de inteligencia de un robot, tal como lo han expresado distintos
autores: (Murphy, 2000), (Fu, Gonzlez, & Lee, 1987), y (Siciliano, Sciavicco, Villani, &
Oriolo, 2009) definen a la robtica como la ciencia que estudia la conexin inteligente entre
la percepcin y la accin, e incluso (Jazar, 2006) que sugiere que los robots inteligentes sean
una clasificacin.
Esta inteligencia debe venir por fuerza de su sistema de control, el cual, de acuerdo con
(Siciliano, Sciavicco, Villani, & Oriolo, 2009) debe tener las siguientes funciones: capacidad
de mover objetos fsicos; capacidad de obtener informacin sobre el propio sistema as como
el entorno; capacidad de aprovechar la informacin y modificar el comportamiento del
sistema; y capacidad de almacenar y presentar informacin de las actividades del sistema.
Murphy (2000) explica estas mismas funciones llamndoles primitivas de robtica. Para
eso es importante primero abordar el concepto de paradigma, que lo define como la filosofa
o conjunto de suposiciones y/o tcnicas que caracterizan una aproximacin a una clase de
problemas. Este concepto se refiere a una forma particular de abordar un problema,
abarcando tanto la forma de estudiarlo como las herramientas utilizadas. As, para organizar la
inteligencia de un robot, se proponen tres paradigmas principales: jerrquico, reactivo e
hbrido. Con esto, Murphy (2000) tambin sugiere dos formas de describir los paradigmas en
robtica: 1) Por la relacin entre las tres primitivas de robtica: sensar, planificar y actuar y 2)
por la forma en la que la informacin es procesada y distribuida en el sistema. Como se
observa, estas dos descripciones engloban las funciones descritas en el prrafo anterior, por
tanto, es posible abordar el sistema de control como el paradigma de inteligencia del robot.
El paradigma jerrquico consiste en que el robot primero sensa su entorno, con base en
esa informacin, en una segunda etapa, planifica la accin a realizar, y finalmente ejecuta la
accin planeada sin recaudar informacin nueva sobre el entorno (Murphy, 2000). Estos tres
pasos se repetiran tantas veces el robot est funcionando. Este paradigma no resulta muy
62
prctico para todos los robots o tareas, pues dan por sentado que el entorno sera esttico y
constante.
En cuanto al paradigma reactivo, Murphy (2000) explica que surgi en oposicin al
paradigma jerrquico, pues en esta nueva aproximacin el robot tiene una serie de
comportamientos, los cuales se deciden con base en cierta informacin recibida de los
sensores, por tanto la actuacin es el resultado de la combinacin de los distintos
comportamientos dictados por los sensores. Este paradigma carece de consideracin ante
cualquier etapa de planificacin, se limita completamente a generar la interaccin entre las
primitivas de sensado y actuacin.
Esta carencia de planificacin desemboc en la aparicin del tercer paradigma:
Paradigma hbrido deliberativo reactivo. El cual delibera, en una etapa inicial, cul sera la
forma ptima de dividir una tarea para su solucin y cules seran los comportamientos que
permitiran lograr cada subtarea y dems; para que en una segunda etapa, se realice el sensado
y la actuacin de manera simultnea tal como ocurre en el paradigma reactivo (Murphy,
2000). Este paradigma entonces puede considerarse una combinacin de los paradigmas
jerrquico y reactivo.
Es importante definir entonces una arquitectura. Mientras Siciliano y otros (2009),
definen una arquitectura funcional como la superposicin de varios niveles de actividades
organizados en una estructura jerrquica. Murphy (2000) propone, a travs de citar a otros
autores (ver fuente), una definicin que implica que la arquitectura ofrece una manera de
organizar el sistema de control, adems de la descripcin del conjunto de componentes y la
forma en que estos interactan.
Es muy importante diferenciar el paradigma de la arquitectura. Mientras el paradigma se
refiere a la forma de abordar o resolver un problema, la arquitectura es la manera de organizar
la solucin propuesta por el paradigma en particular. Esto quiere decir que para un mismo
paradigma existen diversas arquitecturas que lo resuelven.
As, para el caso del paradigma jerrquico, Murphy (2000) propone dos arquitecturas
populares:
Control jerrquico anidado (Nested hierarchical controller), en el cual se
pueden identificar claramente las tres etapas: sensado, planificacin, y actuacin.
En la primer etapa, los sensores reconocen el mundo y generan un modelo virtual
63
de ste, para lo cual se requieren ciertos conocimientos a priori que
complementen la informacin adquirida por los sensores. Luego se pasa a la
etapa de planificacin que consiste en tres partes: el planeador de misin, el
navegador y el piloto; la primera determina a grandes rasgos lo que el robot debe
hacer para cumplir la tarea; y as el navegador toma esa informacin, y en
conjunto con el modelo del mundo traza una ruta para lograr lo establecido por el
planeador de misin; y finalmente el piloto toma cada segmento de la ruta y
define la serie de acciones especficas que permiten ejecutar cada uno de tales
segmentos. As, por ltimo en la etapa de actuacin, se traduce a seales de
actuadores cada una de las acciones indicadas por el piloto.
Fig. 3.1. Control Jerrquico anidado. Tomado de (Murphy, 2000).
Sistema de Control en tiempo Real (RCS), que presenta un sistema que agrupa
las actividades en las tres etapas de las que se ha hablado: sensar, planificar y
actuar. En la etapa de sensado se agrupan varias etapas, entre las cuales, las
lecturas de los sensores pasan a un mdulo de preprocesamiento de la
informacin y de ste al de modelado del mundo virtual; luego, en la etapa de
planificacin se encuentra el mdulo de juicio de valor el cual planifica y simula
lo planeado para asegurar que sea funcional, para que entonces, el mdulo de
64
generacin de comportamientos traduzca el plan en acciones que el robot pueda
generar, lo que constituye la etapa de actuacin.
Fig. 3.2. Esquemas de RCS: a) esquema jerrquico y b) descomposicin funcional.

Tomado de (Murphy, 2000).
Para el caso del paradigma reactivo, es importante tener en cuenta que las arquitecturas
presentadas deben contar con mecanismos para activar comportamientos y para determinar
qu ocurre cuando varios comportamientos estn activos al mismo tiempo (Murphy, 2000). De
igual manera Murphy (2000) propone tres arquitecturas representativas:
Subsumption architecture. En estas arquitecturas se agrupan mdulos
esquemticos en capas de competencia o comportamientos abstractos. Los
65
niveles altos de estas capas pueden incluir o suprimir comportamientos de
niveles inferiores, y los comportamientos de niveles inferiores no pueden ser
reescritos ni reemplazados; adems de que no existe planificacin alguna, los
comportamientos surgen por la presencia de estmulos en el entorno. Al no
elaborar un modelo virtual del mundo, no utiliza mecanismos para recordar el
pasado ni para monitorear cambios en el entorno, pues se limita a atender a
estmulos. A pesar de sus ventajas, estas arquitecturas son muy complicadas de
disear de modo que el robot pueda recibir la orden en algn momento de hacer
una tarea distinta sin ser reprogramado.
Metodologas de campos potenciales. Se utilizan vectores para representar
comportamientos, de modo que una suma de vectores produce un
comportamiento emergente. Tales vectores se organizan en arreglos de
vectores que equivalen a fuerzas en cada punto del espacio en que el robot puede
moverse. Las fuerzas pueden configurarse en cinco perfiles bsicos (uniforme,
perpendicular, atractivo, repulsivo y tangencial) los cuales se pueden combinar
para generar campos potenciales ms complejos. Adems se recurre a perfiles de
magnitud, que se refiere a los cambios que tienen tales vectores en su magnitud
dependiendo la distancia al objeto que causa tal campo.
Fig. 3.3. Ejemplo de combinacin de campos potenciales: uno atractivo y uno repulsivo.
Tomado de (Murphy, 2000).
66
Rule encoding. En este estilo de arquitecturas los componentes de los
comportamientos y el mecanismo de combinacin son implementados como
reglas lgicas.
Por ltimo, para el paradigma hbrido deliberativo/reactivo, Murphy (2000) explica que
este paradigma surgi a raz de la necesidad de cierta inteligencia y capacidad de deliberar
para hacer planes y ejecutarlos. As se lleg a la idea de que los robots hbridos cuentan con
las mejores arquitecturas debido a que utilizan procesamientos asncronos para que las
funciones deliberativas se ejecuten de manera independiente de los comportamientos
reactivos; adems de que al tener un software modular, permite que los subsistemas puedan
ser mezclados y combinados para las aplicaciones que se requieran.
Las arquitecturas hbridas, segn Murphy (2000) tienen componentes comunes, como
agentes secuenciadores, que se presentan como redes de dependencias o mquinas de
estados, las cuales generan los comportamientos o secuencias para lograr subtareas; un gestor
de recursos, que se encarga de la distribucin de las fuentes de informacin para los
comportamientos, as como la seleccin de libreras; un cartgrafo responsable de crear,
almacenar y mantener un mapa de informacin espacial; un planificador de misin que se
encarga de la interaccin con el humano a la vez que la traduccin de sus instrucciones a
trminos en que el robot pueda comprender sus tareas para as construir un plan para la
misin; y por ltimo un agente encargado de monitorear el desempeo y de resolver
problemas, el cual permite analizar si el robot est progresando o no en el cumplimiento de su
tarea. Adems se han establecido tres estilos de arquitecturas hbridas: estilos de gestin,
jerarquas de estados y estilos orientados a modelos.
En cuanto a los estilos de gestin, Murphy (2000) presenta como las principales:
Arquitectura de robot autnomo (AuRA), que consiste en cinco subsistemas
agrupados en las dos porciones propias de una arquitectura hbrida: la parte
deliberativa comprende un planificador y el cartgrafo, mientras que la porcin
reactiva est formada por el subsistema sensorial y el motriz; as el ltimo
subsistema Homeostatic control est en un rea intermedia entre ambas
porciones. Estas arquitecturas se apoyan en los mtodos de campos potenciales y
en un planificador jerrquico anidado.
67
Efectos de fusin de sensores (SFX), que inicialmente fue concebida como una
extensin de AuRA y con el paso del tiempo se convirti en una reorganizacin
de los mismos componentes de AuRA, utilizando un modelo sensorial
neurofisiolgico.
Murphy (2000) presenta una nica arquitectura para explicar las de jerarquas de
estados, que es la de 3-niveles (3-Tiered) o 3T. La principal caracterstica de estas
arquitecturas es la presencia de tres capas en la arquitectura de control, es por eso que se limita
a explicar 3T. En la capa superior se encuentra el planificador, que implica un planificador de
misin y un cartgrafo que generan los planes estratgicos de acuerdo a los objetivos. De ellos
se pasa a la capa intermedia, que est formada por un secuenciador, que utiliza una tcnica de
planificacin reactiva y que genera un conjunto de comportamientos, adems de estar a cargo
de monitorear las funciones de tales comportamientos. stos ltimos forman la capa inferior y
que por definicin son llamados habilidades.
Las arquitecturas orientadas a modelos ms importantes son (Murphy, 2000):
Saphira. Se apoya en el principio de las claves de un robot mvil: coordinacin,
coherencia y comunicacin. El robot debe ser capaz de coordinar no slo sus
actuadores y sensores, sino tambin sus objetivos en un perodo de tiempo; as, la
coherencia es importante para un buen comportamiento; y la comunicacin para
interactuar con humanos y comunicarse con otros robots. La planificacin es lo
ms importante para estas arquitecturas, y para ello se valen de un planificador
llamado PRS-lite (Procedural Reasoning System-lite), el cual se apoya bastante
en el modelo virtual del mundo, que es considerado un proveedor de sensores
virtuales.
68
Fig. 3.4. Vista simplificada de la arquitectura Saphira. Tomado de (Murphy, 2000).

Arquitectura de control de tarea (TCA). Es una arquitectura bastante distinta,
pues no considera comportamientos como tales, adems de que utiliza estructuras
de sensado que le permiten generar un modelo distribuido del mundo; el flujo de
operaciones para conseguir la tarea es organizado por un Prodigy planner, el cual
interacta con el usuario y prioriza los objetivos; entonces se planifica la
trayectoria para luego utilizar un navegador basado en el proceso de decisin
parcialmente observable de Markov. Finalmente, una capa de evasin de
obstculos que coordina ms directamente a los sensores y actuadores.
69
Fig. 3.5. Configuracin de la arquitectura de control de tareas (TCA). Tomado de

(Murphy, 2000).
3.2. ARQUITECTURA DE CONTROL UTILIZADA EN DONAXI.
Como se ha hablado en el Captulo 1. , los robots de servicio desarrollados para la

categora @Home del torneo RoboCup, tienen ciertos requerimientos que deben cumplir en s
mismos. En primera instancia, deben ser robots mviles y completamente autnomos;
requieren interaccin lo ms natural posible con humanos, lo que implica reconocimiento y
sntesis de voz; capacidad para aprender y reconocer rostros humanos; capacidad de visin
artificial que le permitan comprender su entorno y encontrar y ubicar objetos especficos a su
alrededor; habilidad de tomar y manipular los objetos que requiera transportar de un lugar a
otro; capacidad de orientarse y cumplir tareas en entornos dinmicos; posibilidad de
seguimiento de personas en entornos completamente desconocidos; apoyo en la salvaguarda
de personas en situaciones de emergencia; entre algunas otras capacidades.
70
Resulta evidente que tantas capacidades en una misma plataforma requieren de una
arquitectura de control bastante compleja, pues las tareas que debe ser capaz de cumplir el
robot no dan pie a la posibilidad de arquitecturas basadas en el paradigma reactivo, debido a la
forzosa necesidad de planeacin por parte del robot para llevar a cabo sus funciones. Por el
contrario, una arquitectura fundamentada en el paradigma jerrquico resulta inviable por la
necesidad del robot de enfrentar entornos dinmicos y en algunos casos de entornos
completamente desconocidos. Esto deja en claro que la arquitectura utilizada debe estar
desarrollada con miras al paradigma hbrido.
De acuerdo con Vargas y otros (2013) y (Vargas, y otros, 2013), este esquema hbrido se
forma por un nmero de elementos llamados mdulos, los cuales estn agrupados en tres
capas:
En la primera capa, conocida como deliberativa, es donde se razona sobre una
representacin simblica y jerrquica del entorno, tal representacin del entorno
le permite al robot planificar sus acciones para cumplir una tarea a la par que
favorece la comunicacin con humanos.
La capa de ejecucin y control se encarga de la supervisin de las diferentes
etapas de los planes a travs de la informacin que provee la capa funcional, lo
que permite hacer los ajustes que los comportamientos requieran ante situaciones
inesperadas sin perder de vista los objetivos primordiales.
Por ltimo, la capa funcional se forma de una serie de grupos de capacidades que
son quienes ejecutan las acciones fsicas, para las cuales tiene distintos mtodos
que le permiten ejecutar un mismo objetivo, lo que ofrece tambin una gran
flexibilidad en el funcionamiento y as, poder adaptarse a las necesidades
especficas de la tarea.
Al recordar la seccin anterior, se tiene en cuenta que la arquitectura de Donaxi est

basada en las arquitecturas de estados jerrquicos, debido a la clara presencia de las tres capas
o grupos de mdulos caractersticos de este tipo de arquitecturas.
Estas capas pueden apreciarse tambin como una arquitectura distribuida de anillos
concntricos, los cuales representan cada una de las tres capas. La idea de la representacin de
tales anillos surge debido a que la interaccin entre los mdulos (o nodos) resulta ms clara si
71
se entiende como un conjunto de secciones que reciben instrucciones y que a la vez reportan
informacin a un mdulo central, conocido en esta arquitectura como nodo maestro (o
nodoMaestro).
Como se observa en la Fig. 3.6, el nodo maestro ocupa el anillo central o capa
deliberativa, y es por tanto quien toma las principales decisiones y realiza las planificaciones
para la solucin de las tareas.
As, en el siguiente anillo, o capa de ejecucin y control se encuentra un conjunto de
mdulos de control de nivel intermedio, los cuales dirigen a los mdulos del anillo ms
externo y al mismo tiempo tienen comunicacin directa con el nodo maestro.
Finalmente se aprecia el anillo externo o capa funcional, que es la encargada de ejecutar
los comportamientos y de recaudar la informacin del exterior, es decir las instrucciones de
ms bajo nivel, los cuales no tienen tal comunicacin directa con el nodo maestro si no es a
travs de su respectivo nodo en la capa intermedia.
Fig. 3.6. Representacin de anillos concntricos de la arquitectura de Donaxi.

Como se puede apreciar, dentro de tal arquitectura hay dos nodos referidos al brazo
manipulador: manipulacin de objetos e interfaz de brazo. Los cuales son particularmente
el motivo del presente trabajo.
72
3.3. VENTAJAS DEL USO DE ROS.
En la seccin anterior se observ la arquitectura de control de Donaxi, de la cual resulta

evidente la importancia de que existan mdulos separados para controlar los distintos
elementos del robot, y de la necesidad de vas de comunicacin eficientes y estables entre los
mdulos. Para tales efectos se volvi importante la eleccin de una plataforma que ofreciera la
posibilidad de ejecucin de programas separados de manera simultnea a la par que vas de
comunicacin estables y flexibles para los distintos tipos de datos que requieren intercambiar
los mdulos.
Para cumplir con lo exigido en el prrafo anterior se eligi ROS como plataforma de
desarrollo del sistema de control del robot. ROS, que son las siglas en ingls de Sistema
Operativo Robtico (Robot Operating System), es una plataforma, de cdigo abierto (ROS
Contributors, 2014), diseada para la programacin y control de robots y que est formada por
un conjunto de herramientas, libreras y convenciones con el propsito de simplificar la
creacin de robots complejos; adems fue creado con miras a un desarrollo colaborativo entre
distintos centros de investigacin en el mundo (About ROS, s.f.).
Una ventaja inicial de gran importancia es la licencia del software, pues al ser una
licencia BSD con muchos permisos para los usuarios (Is ROS For Me?, s.f.), es gratis para
cualquier persona que quiera desarrollar. Esto implica un ahorro econmico importante para el
logro de los objetivos del proyecto al no tener que pagar por la licencia de cada uno de los
equipos utilizados en las etapas de programacin. Esta economa en licencias no slo se limita
al uso de ROS, sino que el sistema operativo sobre el que se utiliza, Ubuntu, es una
distribucin de Linux, que tambin se distribuye libremente.
ROS fue diseado como un sistema modular con miras a ser un procesamiento lo ms
distribuido posible, lo que favorece el desarrollo contribuido entre la comunidad de usuarios
(Is ROS For Me?, s.f.). Los distintos mdulos requeridos por la arquitectura planteada, en
ROS, se consideran como nodos, que equivalen a programas que se ejecutan de manera
independiente y cuya ejecucin es gestionada por una serie de comandos propios de la
plataforma y que al mismo tiempo su sintaxis est generalizada independientemente de las
funciones cada nodo, as como de los tipos de datos que ste requiere.
73
Las vas de comunicacin que ROS ofrece son conocidas como tpicos, que consisten en
un sistema de publicacin/suscripcin annimo y asncrono, que puede ser fcilmente
consultado por un nodo o almacenado en un archivo, lo que favorece el desarrollo y
depuracin del software ms sencillo y rpido (ROS - Core Components, s.f.). Un tpico
puede ser analizado como un buzn, en el cual uno o varios nodos pueden dejar mensajes de
cierto tipo y cierto tamao, y que posteriormente puede ser ledo tambin por uno o varios
nodos. Este tipo de vas de comunicacin tiene una gran flexibilidad para los tipos de mensajes
y las frecuencias en las que stos pueden fluir, permitiendo que los nodos puedan funcionar a
las frecuencias que convengan, as como tener nodos de cierta forma en espera de que algn
mensaje sea publicado para poder comenzar su funcionamiento, o por el contrario, esperar a
que algn mensaje sea o deje de ser publicado para detener su ejecucin.
Adems de ofrecer una serie de libreras que aceleran el proceso de programacin. Tales
libreras implican tipos de mensajes estndares para robots, libreras geomtricas con las
herramientas tiles para robots, funciones de diagnstico, de localizacin, de mapeo,
navegacin entre otras (ROS - Core Components, s.f.), lo que resulta muy prctico porque
permite al programador ahorrar tiempo de desarrollo de tales herramientas que ya han sido
programadas y que, por tanto, no es necesario reprogramar.
ROS ofrece tambin herramientas propias que permiten desarrollar y depurar los
programas de una manera ms sencilla, pues permiten obtener informacin sobre el estado y
funcionamiento del robot y del sistema de control conforme ocurren. Entre estas herramientas
se pueden mencionar: herramientas de lnea de comando, rviz que es un visor grfico de
informacin de diversos sensores como lseres, cmaras, adems de imgenes en 3D del robot
y del entorno, o rqt que permite visualizar y gestionar los nodos y los tpicos utilizados,
informacin obtenida por encoders o cualquier otro valor numrico, por mencionar slo lo
principal (ROS - Core Components, s.f.).
Por ltimo es importante mencionar la capacidad de ROS de integrarse con otras
libreras externas que proveen de funcionalidades que el propio ROS no resuelve o que
resuelve de distintas maneras. Entre esas libreras se encuentra Gazebo que es un simulador
2D y 3D de cinemtica y dinmica; OpenCV que ofrece una gran cantidad de algoritmos tiles
para visin artificial; point cloud library que de igual manera contiene libreras para visin
74
artificial, pero enfocadas a manipulacin y datos en tres dimensiones; o MoveIt! Que es una
librera de planificacin de movimientos (ROS - Integration with Other Libraries, s.f.).
Como podr verse ms adelante en el presente trabajo, la funcionalidad modular, los
mensajes a travs de tpicos, las libreras enfocadas al desarrollo de robots (particularmente
KDL de OROCOS) y la integracin con otras herramientas (Gazebo) tienen una importancia
vital para el desarrollo y correcto funcionamiento de los mdulos de manipulacin de objetos.
3.4. PROGRAMACIN EN ROS.
En la seccin anterior se habl de las ventajas del uso de ROS, de la existencia de los
nodos, de los tpicos, de libreras y dems herramientas que favorecen el desarrollo de
arquitecturas robticas.
Es importante mencionar que ROS ofrece dos opciones primordiales en cuanto a
lenguajes de programacin. Es posible programar tanto en Python como en C++ pudiendo
aprovechar las ventajas propias de cada uno de esos lenguajes. Todas las funciones bsicas de
ROS estn disponibles para los mdulos programados en cualquiera de los dos lenguajes, sin
embargo, como es evidente, existen ciertas ventajas ms puntuales en alguno de ellos que en el
otro; por ejemplo, la existencia de ciertas libreras nicamente para alguno de los dos
lenguajes. Es por eso que el sistema de mensajes de ROS ofrece la ventaja de permitir la
comunicacin entre nodos sin importar el lenguaje con que hayan sido hechos, es decir, un
nodo en Python puede enviar mensajes a otro en C++ y viceversa. Estas dos opciones plantean
la posibilidad de la coexistencia de ambos lenguajes en el sistema de control, como termina
por ocurrir tanto en el robot Donaxi en general como en el presente trabajo (brazo
manipulador).
A continuacin se presentarn algunas caractersticas de ROS que permitirn explicar
cmo fue que se desarroll todo el software.
Todos los programas y libreras desarrolladas para correr sobre ROS, se organizan en
dos niveles segn (Navigating the ROS Filesystem, 2013):
75
Stacks. Los stacks son vistos como carpetas que almacenan diversos paquetes,
los cuales favorecen una mejor organizacin de los paquetes en proyectos
extensos.
Paquetes. Que son el nivel bajo donde se almacenan las libreras, ejecutables y
herramientas.
Cada uno de los dos niveles posee un archivo manifiesto el cual contiene la descripcin
del contenido, es decir, define los contenidos del paquete o stack y sus respectivas
dependencias de otros paquetes.
As cada mdulo o grupo de mdulos de Donaxi se organiza en paquetes a conveniencia
de acuerdo a sus respectivas dependencias y comodidad en las etapas de desarrollo con el
objetivo de no sacrificar la correcta organizacin de los archivos. En la Fig. 3.6 se observa de
manera sintetizada la estructura de los nodos que conforman el sistema de control de Donaxi.
Y como es posible observar, no es una estructura que termine siendo simple, pues cada nodo
tiene necesidades e incluso tipos de datos distintos de los dems. Esto condujo a que los nodos
implicados en la manipulacin de objetos, por comodidad, estn agrupados en un mismo
paquete debido a sus dependencias y libreras; para dicho paquete al momento de ser creado,
se consider utilizar dependencias de: libreras de mensajes estndares (std_msjs), libreras de
cliente de ROS para C++ (roscpp), libreras de cliente de ROS para Python (rospy) y KDL,
que es la librera en C++ de cinemtica y dinmica y que se explicar en el siguiente captulo.
Estas dependencias se pueden observar en el cdigo del archivo manifiesto del paquete
manipulacin que se muestra en el Anexo C- A.
Como explica la Seccin 3.3, un nodo implica un mdulo de la arquitectura, esto es un
programa ejecutable que funciona por s mismo, pero que a la vez se comunica con otros. De
esta forma, se plante la existencia de dos nodos encargados de la manipulacin: el
nodoManipulacion, el cual se encarga de comunicarse directamente con el nodoMaestro,
tomar las decisiones de alto nivel y enviarlas al programa de bajo nivel encargado de controlar
los servomotores; est programado en C++. El otro ejecutable es representado por el
nodoInterfaz; cuya funcin principal es recibir las instrucciones del nodoManipulacion y
con ellas hacer el control de los servomotores, y que por motivos de comodidad en el
desarrollo y pruebas, cumple algunas otras funciones como tener una interfaz grfica cmoda
para que el usuario pudiera verificar parmetros de los servomotores; est programado en
76
Python. En cuanto a los tpicos utilizados son: Manipulacion, que sirve para los reportes de
estado del nodoManipulacion hacia el nodoMaestro; ControlMaestro/Manipulacion, en el
cual el nodoMaestro publica las instrucciones para el nodoManipulacion, as como todos los
parmetros que requiere para su correcto funcionamiento; ManipData, que es donde el
nodoManipulacion publica las instrucciones que debe llevar a cabo InterfazManipulacion (o
nodoInterfaz); /brazo_donaxi_plugin/ArticulacionesGazebo es el tpico donde se publican
las instrucciones para la simulacin de los movimientos del brazo, lo cual ser explicado en la
seccin 3.6; y en cuanto a los tpicos rosout y rosout_agg son tpicos generales que ROS
requiere para su funcionamiento. La Fig. 3.7 muestra los nodos encerrados en elipses, y los
tpicos marcados por rectngulos, todos involucrados con la ejecucin de la manipulacin de
objetos.
Fig. 3.7. Nodos y tpicos involucrados con la manipulacin de objetos.

Los mensajes que son publicados en los tpicos, pueden ser de varios tipos para
ajustarse a los requerimientos de los datos que se transfieren de un nodo a otro, es decir, un
nodo puede publicar una cadena de caracteres, un float, un booleano, una matriz, etctera.
Incluso pueden utilizarse tipos de datos generados por el programador que le permiten
simplificar su labor (Understanding ROS Topics, 2014). Esto puede representar una
complicacin extra en el proceso de desarrollo debido a la gran cantidad de tipos de mensajes
y los problemas que puede representar la discordancia en estos detalles. Por tal razn existe un
77
conjunto de plantillas y una librera general de mensajes de nodos de Donaxi, los cuales
consideran todos los nodos que se comunican con el nodo maestro.
Estas plantillas consideran estructuras generales a adoptar por todos los nodos que
requieren de establecer comunicacin con el nodoMaestro, lo que implica el uso de nombres
normalizados para tanto los tpicos como los nodos, librera de estructuras de datos que
permiten normalizar todos los parmetros que deben comunicarse, librera de funciones que
permitan codificar y decodificar todos los mensajes en un tipo de dato especfico (String),
adems de funciones que simplifiquen y normalicen el envo y recepcin de datos con el
nodoMaestro. Todo esto est organizado en tres archivos de libreras de C y algunas funciones
cruciales en un cdigo para cada nodo.
Fuera de esto, cada nodo o grupo de nodos tiene la libertad para funcionar y organizarse
internamente como mejor le convenga, siempre y cuando se respeten las disposiciones de las
libreras y plantillas.
Por ltimo, es importante destacar los procesos de ejecucin de ROS. Como se ha
explicado, una arquitectura de robot programada en ROS implica varios ejecutables que deben
correr de manera simultnea. Es evidente que para Donaxi, con todas las funciones y
perifricos con que cuenta, se trata de una gran cantidad de programas que deben ser lanzados
para que el robot pueda ejecutar una tarea, y por motivos de reglamento en la competencia
RoboCup @Home, es necesario un mtodo sencillo y eficiente para poner en marcha todo el
sistema de control del robot. ROS tiene una importante funcionalidad que permite facilitar el
proceso, se trata de la elaboracin de sencillos scripts que enlistan los nodos que deben
ejecutarse, y algunos parmetros que se pudieran requerir para la ejecucin. Estos scripts son
llamados archivos launch y permiten ejecutar varios nodos con una sola lnea de comando.
Para esto fue que se cre un archivo launch que incluyera los nodos correspondientes a la
manipulacin; esto es el nodoInterfaz y el nodoManipulacion. Tal contenido se muestra en el
Anexo C- B.
78
3.5. PROGRAMACIN DE SECUENCIAS CON BRAZO DE DONAXI.
En la seccin anterior se explic de manera general cmo se llev a cabo la

programacin del sistema de control del robot Donaxi, y las disposiciones generales de toda la
arquitectura que afectan a la programacin de los nodos correspondientes al sistema de
manipulacin. En este punto se definirn ciertas consideraciones iniciales que plantearon las
decisiones importantes con respecto a la parte del control del brazo robtico, las cuales fueron
punto de partida para el comienzo de la programacin.
En primera instancia se ha mencionado que las libreras de cinemtica y dinmica de
OROCOS (KDL) estn en C++, adems de la capacidad que tiene C++ para la organizacin de
cdigo, resulta una potente y til opcin para la toma de decisiones de alto nivel, y si se toma
en cuenta que las libreras de funciones de codificacin y decodificacin de mensajes estn
escritas en C++, se crea la necesidad de que el nodoManipulacion est por fuerza escrito en
se lenguaje.
Por otro lado, la experiencia en la programacin, depuracin de cdigo, verificacin y
preparacin de las pruebas, caus la necesidad de un entorno grfico que permitiera el control
del brazo y el monitoreo de los parmetros de los servomotores. Para facilitar estos entornos,
Python ofrece diversas opciones, libreras y sencillez en la programacin. La necesidad de una
estable y robusta comunicacin serial tambin puede ser desarrollada en Python de manera
bastante sencilla gracias a sus libreras y la capacidad de estructuracin que ofrece el lenguaje.
Es importante que estas dos caractersticas aparezcan juntas en un mismo programa, pues se
evita as la transferencia innecesaria de toda la informacin a travs de los tpicos de ROS, lo
que representa un importante ahorro en procesamiento y recursos computacionales.
Finalmente, para que estos dos requerimientos puedan convivir en un mismo programa, resulta
conveniente que funcionen a travs de hilos, es decir que se generen tareas en paralelo dentro
de un mismo programa, de modo que por una parte, el programa se encuentra actualizando la
informacin en la interfaz, por otra parte sostiene la comunicacin serial con el controlador del
brazo para enviar las instrucciones y leer los parmetros de estado de cada uno de los
servomotores. Todo esto resulta conveniente que sea programado en Python, pues ofrece gran
cantidad de libreras compatibles con Linux y que, por su sencillez, aceleran el proceso de
programacin requerido.
79
Dadas las decisiones de programacin presentadas, a continuacin se explica el proceso
de escritura de cdigo inicial del nodoManipulacion, en una etapa en la que considera
nicamente programacin de secuencias fijas para el brazo.
El mensaje de instrucciones que el nodoMaestro enva al nodoManipulacion contiene
una estructura de datos que incluye cuatro variables numricas enteras. En primer lugar se
tiene la actividad a realizar, y en segundo, las tres coordenadas de X, Y y Z en centmetros
donde se encuentra el objeto especfico que debe tomar. El parmetro de la actividad a realizar
se vuelve entonces el nico nmero de inters para la ejecucin de secuencias, pues dado que
implica movimientos fijos que el brazo debe ejecutar, las coordenadas de ubicacin se vuelven
innecesarias. Para eso se decidi declarar una serie de valores constantes almacenados en la
librera de mensajes, los cuales identifican el nombre de la secuencia.
En cuanto al programa principal de manipulacin se decidi que fuera implementado
como una mquina de estados, en los cuales cada una de las secuencias tanto fijas como
flexibles para manipulacin sea representada por uno de los estados. El Algoritmo 3.1 muestra
de manera simplificada la estructura de la mquina de estados.
Algoritmo 3.1
Programa principal.
1. Inicializacin y configuracin del nodo.
2. Declaracin e inicializacin de variables, estructuras y mensajes.
3. Apertura del tpico de recepcin de mensajes del maestro.
4. Apertura del tpico de envo de mensajes al maestro.
5. Inicializacin de tpicos para mensajes para simulador y para interfaz.
6. Repetir Mientras ROS est OK y actividad distinto a TERMINAR_EJECUCION:
6.1. Si hay nuevo mensaje recibido, entonces:
6.1.1. Enviar mensaje a maestro indicando que la instruccin fue
recibida.
6.1.2. Segn actividad hacer:
6.1.2.1. Caso MANIPULACION_EMERGENCIA:
6.1.2.1.1. Apagar torque de todos los motores.
6.1.2.1.2. Enviar instruccin a la interfaz.
6.1.2.2. Caso MANIPULACION_INICIO:
6.1.2.2.1. Configurar valores de inicializacin para todos los
servomotores y guardar en estructura brazo_obj.
6.1.2.2.2. Llamar a funcin FaseSecuencia con parmetros de
entrada brazo_actual, brazo_obj y 5 segundos; y guardar
resultados en brazo_actual.
6.1.2.3. Caso MANIPULACION_EJEMPLO_SECUENCIA:
6.1.2.3.1. Configurar valores de primera etapa de secuencia para
los servomotores de inters y guardarlas en brazo_obj.
entrada brazo_actual, brazo_obj y tiempo en segundos; y
guardar resultados en brazo_actual.
6.1.2.3.3. Configurar valores de segunda etapa de secuencia para
80
los servomotores de inters y guardarlas en brazo_obj.
entrada brazo_actual, brazo_obj y tiempo en segundos; y
guardar resultados en brazo_actual.
6.1.2.3.5. Repetir configuraciones de valores y llamadas a
funcin FaseSecuencia para cada una de las etapas
requeridas por la secuencia.
6.1.3. Fin de segn actividad.
6.1.4. Enviar mensaje al maestro indicando que se ha terminado la
ejecucin de la tarea.
6.2. Fin de Si.
6.3. Verificar el tpico recepcin de mensajes nuevos.
7. Fin de repetir.
Como se observa, para construir una secuencia, es tan simple como repetir dos pasos las
veces que sean necesarias. Esos dos pasos son: primero la configuracin de valores para los
servomotores en la estructura brazo_obj (6.1.2.3.1 y 6.1.2.3.3), y segundo una llamada a la
funcin FaseSecuencia, cuyos parmetros de entrada son siempre los mismos (6.1.2.3.2 y
6.1.2.3.4). En cuanto a la configuracin de los valores, es tan simple como asignar el valor del
ngulo al que se desea llegar a la variable correspondiente al servomotor que se pretende
mover; para simplificar la programacin de esta etapa, las unidades de ngulos utilizadas son
grados, pues resulta ms sencillo de comprender para una persona.
Para el caso de la funcin FaseSecuencia lleva una mayor complejidad, pues
internamente itera durante el tiempo que se le solicita en los parmetros de entrada, y va
cambiando gradualmente a un ritmo constante los valores de los servomotores que se
pretendan mover. A la par que almacena en una variable el estado de los servomotores en cada
momento. El Algoritmo 3.2 Explica de manera sintetizada cmo trabaja esta funcin.
Algoritmo 3.2.
FaseSecuencia, con parmetros de entrada act de tipo InstruccionesInterfaz,
obj de tipo InstruccionesInterfaz y time de tipo double. Entrega como
resultado una estructura de tipo InstruccionesInterfaz.
1. Inicializar variable iteracion en el nmero de iteraciones que la funcin
debe dar al multiplicar time por la frecuencia de iteracin.
2. Repetir mientras iteracion sea mayor que 0:
2.1. Decrementar iteracion.
2.2. Llamar a funcin StepServo para cada uno de los servomotores, con
parmetros de entrada valor correspondiente del servomotor de la
estructura act y obj, adems de la constante STEP. Y guardar
resultados en act.
2.3. Definir VELOCIDAD_CONSTANTE para cada uno de los servomotores.
2.4. Verificar recepcin de mensajes del maestro.
2.5. Si el mensaje recibido es EMERGENCIA entonces:
2.5.1. Apagar el torque de todos los motores.
2.6. Fin de Si.
81
2.7. Enviar datos de servomotores al nodoInterfaz.
2.8. Enviar datos de servomotores al simulador Gazebo.
3. Fin de Repetir.
4. Regresar act.
Se puede observar en el paso 2.2 la presencia de una funcin llamada StepServo. Esta
funcin, que se encuentra en la librera de funciones_secuencias.h, es una serie de
condiciones, en las cuales se modifica la posicin de un solo servomotor, considerando su
estado y el ngulo al que se pretende llegar, generando un cambio de incremento o decremento
de un valor constante definido previamente en relacin a la velocidad a la que se desea hacer
el movimiento. Por motivos de configuraciones y particularidad de cada articulacin, se
desarroll una funcin distinta para cada uno, las cuales toman en cuenta los lmites y el rango
de accin de cada servomotor.
Es importante recordar que una estructura de tipo InstruccionesInterfaz guarda
informacin de ngulo y velocidad para cada uno de los servomotores, y es la que se enva por
medio de los tpicos de ROS al nodoInterfaz. Esta estructura es codificada con la funcin
codInstruccionesInterfaz, la cual se encuentra en la librera mensajes_interfaz.h, y la cual se
encarga de convertir todos los valores numricos de los servomotores y las velocidades a
cadena y concatenarlos para que viajen en un mismo mensaje. Evidentemente, el nodoInterfaz
realiza la decodificacin para poder interpretar esas instrucciones, tal como se explica ms
adelante en el presente trabajo.
La idea de que los mensajes sean codificados en una cadena de caracteres es tomada de
la disposicin general de comunicacin de todos los nodos con el nodoMaestro, pues el
mecanismo es muy similar: todos los valores de una estructura se concatenan dentro de una
misma matriz para ser enviados, el receptor se encarga de separar tales valores y reconstruir la
estructura original. En el caso de estas funciones de codificacin y decodificacin, as como
las definiciones de las estructuras, se encuentran definidas dentro de las libreras
EnvioMsgaMaestro.h, EnvioMsgaNodo.h y mensajes.h.
Como breve conclusin de esta seccin, cabe destacar que aunque el desarrollo de
secuencia fijas no es el objetivo primordial del presente trabajo, esta estructura sirvi de base
para el desarrollo de las trayectorias donde la cinemtica inversa es utilizada. Recordar que la
planeacin de trayectorias para la cinemtica inversa consiste en una secuencia mezcla de
etapas fijas y etapas variables.
82
El Anexo D- A muestra un ejemplo de la programacin de una secuencia ya en C++. Se
observa que el uso de funciones de libreras desarrolladas para este producto simplifica
bastante el proceso de programacin de secuencias fijas, pues consiste en algunos pasos
sencillos y puntuales. Para la comprensin completa de las funciones como FaseSecuencia,
StepServo o las de codificacin y decodificacin de mensajes, se recomienda revisar el cdigo
fuente adjunto a este documento.
3.6. SIMULACIN DE UN BRAZO EN GAZEBO.
Gazebo es una herramienta libre de simulacin de robots en tres dimensiones, que

permite probar de manera rpida algoritmos, disear robots y hacer pruebas de regresin
utilizando escenarios realistas. Es posible el uso de mltiples robots, entornos complejos,
cuenta con un motor de fsica para simular fuerzas e interacciones de los robots con objetos o
superficies (Gazebo, 2014).
La principal ventaja para el uso de Gazebo es la integracin con ROS, lo que lo ha
convertido en una de las principales herramientas de simulacin utilizada por desarrolladores
de robots (Gazebo, 2014). Con el apoyo de la documentacin de Gazebo en el portal de ROS
(Gazebo - Package Summary, 2013), as como de los tutoriales de la pgina web de Gazebo
(Gazebo - Tutorials, s.f.), se tom la decisin de actualizar la versin de Gazebo instalada,
pues la versin que se incluye dentro de ROS Fuerte era una versin muy anticuada, y se opt
por tomar la versin 1.5, que era la ms actual en el momento de desarrollo de esta etapa
(mediados del 2013).
La simulacin del brazo de Donaxi en Gazebo result bastante simple, pues nicamente
se pretenda mostrar un modelo simplificado en 3D de las distintas configuraciones de
posicin del brazo en los distintos instantes de tiempo. Para lograr esto, era preciso resolver
dos problemas: primero, generar un modelo en 3D que imitara las dimensiones del brazo y que
fuera visible en la interfaz de Gazebo; y segundo, establecer las vas de comunicacin entre
ROS y Gazebo para que el propio control de Donaxi pudiera controlar el brazo virtual cual si
fuera el brazo real.
83
Para la primer etapa, se escribi un cdigo en XML (eXtensible Markup Language)
propio de Gazebo, para lo cual se utiliz el apoyo de la documentacin propia de Gazebo y los
tutoriales para creacin de modelos (Make a Model, s.f.), particularmente el de creacin de un
gripper (Make a Simple Gripper, 2014). Tal modelo se gener a partir de los parmetros de
Denavit-Hartenberg obtenidos matemticamente (Ver Seccin 2.3), lo que permiti un modelo
como se observa en la Fig. 3.8 que es una captura de pantalla del modelo del brazo siendo
simulado en Gazebo. Como se puede ver no es como el modelo real, sin embargo las
dimensiones de los distintos elementos de la cadena cinemtica son coincidentes con el
modelo real.
Fig. 3.8. Captura de pantalla del modelo virtual del brazo de Donaxi para efectos de
pruebas en simulacin previas a la ejecucin de los cdigos en el brazo real.
Como se observa, en el modelo virtual se utilizaron cilindros para representar
articulaciones y prismas cuadrangulares para simular los eslabones rgidos, lo que aument
ligeramente la complejidad del modelo al requerir la programacin del doble de elementos,
pero que sin duda ayuda considerablemente a la comprensin de la cadena cinemtica. Luego
de representada tanto la parte visual como la parte con propiedades fsicas, se hizo el desglose
84
de articulaciones dentro del mismo cdigo, las cuales obedecen a las posiciones de cada uno
de los servomotores del robot. Para ver el XML generado consultar el Anexo D- B.
Dentro del funcionamiento de Gazebo, un modelo no puede ser representado por s
mismo si no est contenido dentro de un entorno o mundo. Para esto fue necesario generar
un XML ms para crear y configurar el mundo donde estar el modelo del brazo. Este mundo
se dise completamente vaco, y se agreg nicamente el modelo del brazo, pues para los
alcances de la simulacin buscada no era necesaria ninguna otra interaccin con otros
elementos. Para generar tal cdigo se recurri al apoyo de los tutoriales propios de Gazebo, en
particular: (Make a Simple Gripper, 2014).
Para la segunda etapa del desarrollo de la simulacin se gener un plugin para Gazebo,
el cual tiene la capacidad de suscribirse a un tpico de ROS. En (Tutorials/1.3/ros enabled
model plugin, 2012) hay documentacin suficiente para el desarrollo de tal plugin. Como se
menciona en la seccin anterior, el nodoManipulacin publica las instrucciones para el brazo
en dos tpicos al mismo tiempo, un tpico destinado a ser ledo por el nodoInterfaz, el cual
incluye tanto los ngulos como las velocidades a las que deben moverse los motores, y un
segundo tpico que es ledo por el plugin de Gazebo, que contiene nicamente las posiciones
puntuales en cada instante de tiempo. De manera general, el desarrollo del complemento que
permite la comunicacin fue como se explica a continuacin.
Una vez que se tena creado el XML del modelo del brazo y el mundo para poder
visualizarlo, se gener el archivo de plugin de ejemplo del tutorial (Tutorials/1.3/ros enabled
model plugin, 2012), el cual fue compilado e instalado. Tras la verificacin de su
funcionamiento, se procedi con una revisin de la documentacin de la librera
physics/physics.hh propia de Gazebo (gazebo::physics::JointController Class Reference, s.f.),
en donde se hallaron las funciones requeridas para que el plugin fuera capaz de manipular las
posiciones de cada una de las articulaciones.
Para facilitar la programacin del plugin con las funciones de la librera
physics/physics.hh, se complement con funciones de codificacin y decodificacin de los
mensajes, estas ltimas programadas en la librera mensajes_gazebo.h, de este modo, se utiliz
el mismo principio de envo y recepcin de mensajes que se utiliza entre el resto de los nodos.
As nodoManipulacion es capaz de publicar los mensajes codificados a Gazebo, el cual al
mismo tiempo es capaz de decodificarlos.
85
El cdigo del plugin, con la ayuda de la librera mensajes_gazebo.h, se limita a unas
pocas lneas de cdigo, como se puede observar en el Anexo D- C.
Finalmente, es importante mencionar que para el correcto funcionamiento del simulador,
una vez lanzado se requiere de un ajuste de la frecuencia de iteracin para que coincida con la
frecuencia del nodoManipulacion, es decir 10 hertz.
En este punto del desarrollo es posible programar secuencias de cdigo, las cuales
pueden ser visualizadas en el simulador sin la necesidad de conectar el brazo. De la misma
forma, se prev que para cuando se haga la ejecucin con el brazo en una prueba real, con el
fin de reducir el coste computacional, el simulador puede no ser ejecutado sin necesidad de
cambios en el cdigo, pues al no incluir un control que asegure la conexin entre nodos, el
nico efecto sera que los mensajes publicados en el tpico de Gazebo no sern ledos por
ningn nodo.
Las mquinas deben trabajar. La gente debe pensar.

(Richard Hamming)
CAPTULO 4. IMPLEMENTACIN DE ALGORITMOS.
En este cuarto captulo se presentan las libreras de cinemtica y dinmica (KDL) de

OROCOS, las ventajas que estas ofrecen y cmo es que simplifican en gran parte la
programacin de los clculos propios del brazo. Por extensin se explicar cmo se puso en
cdigo el modelo cinemtico encontrado en el segundo captulo y se mostrar la cinemtica
directa. Seguidamente se dar una breve explicacin de la programacin de la propuesta del
algoritmo de trazado de rutas. De esta manera se pretende llegar a la aclaracin de la funcin
que desempea el cdigo de este captulo en conjunto con el del anterior para la simulacin de
un movimiento.
86
87
4.1. LIBRERA DE CINEMTICA Y DINMICA (KDL) DE
OROCOS.
En el Captulo 2. se desarrollan los fundamentos matemticos que permiten hacer los

clculos de cinemtica del brazo robtico. Sin embargo, el proceso de programacin en ROS
resulta ms sencillo gracias a las libreras disponibles para el control de robots.
OROCOS es una iniciativa de software libre para el desarrollo de software para robots.
OROCOS significa Software de Control Libre para Robots (Open RObot COntrol Software).
El cual proporciona varias libreras y herramientas para el desarrollo de software de control
tanto para robots como aplicaciones en tiempo real, las cuales son distribuidas bajo licencias
GPL o LGPL (Orocos Licenses, s.f.), tales herramientas actualmente tienen aplicaciones
variadas como automviles autnomos, capacidades de seguimiento en robots, e incluso
ciruga a distancia (Applications, s.f.).
Las principales herramientas que ofrece OROCOS son: Kinematics and Dynamics
Library (KDL), Bayesian Filtering Library, y OROCOS Toolchain (The Orocos Project, s.f.);
aunque tambin se mencionan: Real-Time Toolkit, Orocos Component Library (que son parte
de OROCOS Toolchain), Geometric relations semantics, iTaSC, herramientas dedicadas a
robots KUKA y algunas otras. Entre las cuales hay soporte para generar componentes en
tiempo real en C++, algoritmos de visin artificial, libreras de cinemtica y dinmica,
libreras de herramientas matemticas de verificacin. De todo esto, ROS Fuerte incluye por
defecto las libreras KDL, que son un conjunto de herramientas matemticas en C++ que
permiten implementar de manera sencilla clculos de cinemtica y dinmica.
De acuerdo con (Orocos Kinematics and Dynamics, s.f.) KDL desarrolla un sistema de
aplicacin independiente para modelar y calcular cadenas cinemticas, contiene una serie de
libreras de clases de objetos geomtricos, cadenas cinemticas y sus respectivos movimientos.
Las clases de primitivas geomtricas que ofrece OROCOS, segn (Geometric
Primitives, s.f.) son:
Vectores, que son bsicamente matrices de 3x1, los cuales tienen sus respectivas
funciones de declaracin, asignacin, operaciones matemticas y comparacin
88
Rotaciones, representadas por matrices de 3x3 que equivalen a una matriz de
rotacin (ver Captulo 2. ), y que de igual manera ofrecen funciones para realizar
operaciones matemticas con ellas, entre otras.
Frames, (sistema de referencia), que equivalen a matrices de 4x4 que
representan la posicin de un objeto con respecto a un sistema de referencia.
Contiene una matriz de rotacin que representa la orientacin del objeto y un
vector que contiene la ubicacin del objeto. Se debe observar que por tanto un
Frame contiene a una clase vector y a una clase rotacin, adems de funciones
para operaciones matemticas y dems que permiten manipular Frames
completos.
Giro (Twist), que consiste en una matriz de 6x1 que representa la velocidad de
un Frame, tal matriz se forma de la unin de un vector de tres dimensiones que
equivale a la velocidad de traslacin en los tres ejes, y otro vector igualmente de
tres dimensiones que representa las velocidades de rotacin. Adems esta clase,
al igual que todas las dems, incluye funciones de declaracin, definicin y
operaciones matemticas.
Torsin (wrench), el cual es tambin una matriz de 6x1 que representa la unin
de un vector de tres dimensiones que representa fuerzas paralelas a los distintos
ejes cartesianos, y otro vector de tres dimensiones que representa momentos
alrededor de cada uno de los ejes cartesianos. Tambin contiene funciones de
declaracin, definicin y operaciones matemticas.
Como se puede observar, hay dos niveles de clases. En el nivel ms bajo se encuentran
los vectores y las rotaciones mientras que en el siguiente estn los Frames, giros y torsiones.
Se dice que estos tres ltimos son de nivel superior porque contienen elementos de nivel
inferior, es decir vectores y rotaciones.
De la misma manera existen otros cuatro niveles de clases, que permiten explotar an
ms las capacidades de la librera. Estas clases estn explicadas en (Kinematic Trees - KDL
1.1.x, s.f.), y son las siguientes:
89
Articulacin. Es la clase que representa a un grado de libertad y puede estar
formada por una rotacin o una traslacin entre dos segmentos. Pueden ser
aplicadas a Frames o a giros.
Segmento. Un segmento se puede explicar como un cuerpo rgido que contiene
una articulacin y un Frame. Es decir, es la representacin matemtica de un
eslabn de la cadena cinemtica como los explicados en el Captulo 2.
Cadena. Es sencillo de deducir que representa a una cadena cinemtica; es decir
que se construye como un conjunto de segmentos o de otras cadenas dadas,
siendo la representacin matemtica de un conjunto de slidos unidos por
articulaciones, es decir un brazo.
rbol. Un rbol es tambin un conjunto de cuerpos slidos unidos por
articulaciones, pero representan una clase de nivel mayor que las cadenas porque
pueden estar formados por segmentos, cadenas u otros rboles dados. Adems de
que su construccin implica la declaracin de una raz para cada nuevo
elemento; esto permite ramificaciones y conexiones entre cadenas, ofreciendo
cuerpos mucho ms complejos.
Como se puede observar, con estas clases es posible realizar modelados matemticos y
clculos dinmicos desde simples robots mviles, brazos de n grados de libertad tanto lineales
como paralelos, e incluso robots humanoides, insectoides y dems, pues los modelos pueden
ser tan simples o complejos como se requiera.
Por ltimo, KDL ofrece solvers de cinemtica directa (Kinematic and Dynamic Solvers,
s.f.), los cuales son clases capaces de encontrar soluciones de velocidad o de posicin para
arreglos de articulaciones dados. Por el momento KDL slo cuenta con solvers de cinemtica
directa, an no estn desarrollados los de cinemtica inversa; sin embargo, para el presente
proyecto, y recordando el Captulo 2. , el contar con un solver para cinemtica directa es
bastante ayuda, ya que el de cinemtica inversa es resultado de la presente investigacin.
90
4.2. PROGRAMACIN DE CINEMTICA DIRECTA.
Recordando los clculos de cinemtica directa explicados en la seccin 2.4, que

implican la extraccin de los parmetros X, Y y Z, as como los de roll-pitch-yaw de la matriz
que se obtiene de la evaluacin de la matriz de transformacin homognea T con respecto a
los valores articulares que tiene el brazo en un momento determinado. Tal matriz de
transformacin homognea que se calcula en la seccin 2.3 tomando como punto de partida
los parmetros Denavit-Hartenberg, es determinada automticamente por el solver de
cinemtica directa propio de KDL, lo que simplifica mucho el problema de programacin,
limitndose nicamente a facilitarle al solver la informacin requerida para calcular tal matriz.
Lo explicado en el prrafo anterior implica que el problema inicial, por tanto, es la
declaracin y construccin de la cadena cinemtica que represente al brazo del Donaxi. Para
esto las clases de KDL, explicadas en la seccin anterior toman un papel muy importante en
esta etapa. Es importante recordar que en el presente trabajo se efectan los clculos para el
brazo del robot Donaxi, de modo que el brazo se estudia como un elemento independiente. Por
tanto, no es necesario construir un rbol cinemtico, pues una cadena cinemtica es suficiente
para la descripcin del objeto de estudio.
Para la construccin de la cadena cinemtica fue necesario nicamente recurrir a los
parmetros Denavit-Hartenberg (seccin 2.3), as como el algoritmo de obtencin de tales
parmetros (Anexo A). As, la definicin de la cadena cinemtica qued como el Cdigo 4.1,
que consiste en tan solo la declaracin de la cadena cinemtica (lnea 1), y luego una serie
finita donde se aaden uno a uno los segmentos que van formando la cadena (lneas 2 a 7).
Cdigo 4.1
1. KDL::Chain chain;
2. chain.addSegment( Segment( Joint(Joint::RotZ),Frame( Rotation::RPY(M_PI/2
,0.0,0.0), Vector(0.0,0.0,18.5) ) ) );
,0.0,0.0), Vector(22.44,0.0,0.0) ) ) );
4. chain.addSegment( Segment( Joint(Joint::RotZ),Frame( Rotation::RPY(0.0,0.
0,0.0), Vector(3.1,0.0,0.0) ) ) );
5. chain.addSegment( Segment( Joint(Joint::None),Frame( Rotation::RPY(-
(M_PI/2),0.0,((7*M_PI)/4)), Vector(0.0,0.0,0.0) ) ) );
,0.0,0.0), Vector(0.0,0.0,15.67) ) ) );
7. chain.addSegment( Segment( Joint(Joint::RotZ),Frame( Rotation::RPY(-
(M_PI/2),0.0,0.0), Vector(15.0,0.0,0.0) ) ) );
91
Como se puede observar, la cadena cinemtica est formada por seis segmentos que
representan cada una de las partes rgidas del brazo de Donaxi. Recordando la seccin
anterior, un segmento se forma de la unin de una articulacin y un Frame, el cual se
encuentra unido a otro segmento precisamente por la articulacin.
Podra resultar visiblemente ms claro si cada segmento, articulacin o Frame se
declarara y definiera por separado antes de aadirlo a la cadena cinemtica, sin embargo, no
hay ms partes del cdigo donde se requiera acceso a modificar o leer tales clases, lo que
implica que si de declararan por separado, sera un consumo innecesario en memoria
computacional, por lo que se decidi que se declararan constantes desde el principio y que se
aadieran a la cadena de manera directa.
Como se mencion, para la construccin de la cadena fue necesario recurrir a los
parmetros Denavit-Hartenberg, de los cuales se recuerda del Anexo A, que es necesario que
todas las articulaciones se encuentren en el eje Z, es decir, si son articulaciones lineales, el
desplazamiento es a lo largo del eje Z, y si son articulaciones rotativas, las rotaciones deben
ser alrededor del eje Z. Por tanto, las articulaciones de todos los segmentos estn indicadas
para ser alrededor del eje Z, a excepcin de la lnea 5, donde no tiene ninguna. Esto representa
una articulacin rgida, que equivale a la articulacin del brazo que une dos secciones rgidas
con un ngulo rgido de 135.
En cuanto a los Frames que forman parte de los segmentos vienen dados en este caso por
los cuatro parmetros Denavit-Hartenberg, que de acuerdo a lo explicado en 2.3, donde
representa una rotacin alrededor del eje X, este valor debe ser incluido en el valor del eje X
de la rotacin correspondiente al Frame; equivale a una distancia medida a lo largo del eje
Z, por tanto ocupa el respectivo lugar en el vector de desplazamiento del Frame; el parmetro
representa un desplazamiento a lo largo del eje X, es por eso que ese parmetro ocupa el
primer lugar de las tres componentes de los vectores de los Frames; finalmente, representa
un ngulo alrededor del eje Z, es decir el valor del ngulo de la articulacin, de los cuales
todos se quedan en cero, pues sus valores no son constantes, excepto en el caso de la lnea 5,
donde se declar una articulacin rgida, pues ah se vuelve necesaria la colocacin de una
rotacin en Z que ser constante y equivale a . Resulta conveniente evitar ngulos negativos
en el programa para simplificar la conversin de los datos a los valores de los servomotores.
92
Como se observa que casi todos los parmetros de rotacin en Z estn en cero, se
obtiene una configuracin del brazo que no puede alcanzar en la realidad, como se observa en
la Fig. 4.1.A. Sin embargo, dentro de otras secciones del cdigo se ajustan los ngulos para
coincidir con los parmetros de Denavit-Hartenberg en cuanto a los ngulos de las
articulaciones, lo que permiten llevar al brazo a la posicin inicial tras los primeros clculos;
la posicin de inicializacin del brazo queda como muestra la Fig. 4.1.B.
Fig. 4.1. A) Izquierda. Configuracin del brazo con todas las articulaciones en cero. B)
Derecha. Configuracin del brazo en la posicin inicial que implica los valores Denavit-
Hartenberg.
En este punto ya se tiene la cadena cinemtica declarada para poder crear el solver que
calcule la cinemtica directa, por tanto las siguientes lneas de cdigo son las que se muestran
en el Cdigo 4.2 y representan la declaracin del solver, la declaracin del arreglo de
articulaciones que sern los parmetros para cada vez que se desee obtener la solucin de la
cinemtica directa de cierta configuracin de los servomotores, y finalmente la extraccin de
los valores de inters de la matriz de transformacin obtenida (Frame).
Cdigo 4.2
8. // Create solver based on kinematic chain :
9. ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
10. // Create joint array
11. unsigned int nj = chain.getNrOfJoints();
12. //arreglo de joints de posicin presente (constante durante algoritmo):
13. KDL::JntArray presentpos = JntArray(nj);
14. //arreglo de joints de posicin de la iteracin:
15. KDL::JntArray iterapos = JntArray(nj);
16. //arreglo de joints de prueba para definir cul conviene:
17. KDL::JntArray trialpos = JntArray(nj);
93
18. //inicializar Joint Array con valores de los servos hasta el momento:
19. presentpos(0)=degtorad(previous.servo1);
24. // Calculate forward position kinematics
25. bool kinematics_status;
26. kinematics_status = fksolver.JntToCart(presentpos,present);
27. present.M.GetRPY(presentrpy.roll, presentrpy.pitch, presentrpy.yaw);
28. if(kinematics_status>=0)
29. {
29.1. printf("present: pos: x: %f y: %f z: %f ", present.p.x(),
present.p.y(), present.p.z());
29.2. printf("rot: x: %f y: %f z: %f\n",radtodeg(presentrpy.roll),
radtodeg(presentrpy.pitch), radtodeg(presentrpy.yaw));
30. }
31. else
32. {
32.1. printf("%s\n","Error: could not calculate forward kinematics);
32.2. return previous;
33. }
34. //calcular el roll-pitch-yaw de la rotacin de goal
35. goal.M.GetRPY(goalrpy.roll, goalrpy.pitch, goalrpy.yaw);
En el Cdigo 4.2 se puede observar, en las lneas 8 y 9, que se encuentra la declaracin

del solver que contiene a la cadena chain, es decir que fksolver va a encontrar soluciones
de cinemtica directa para chain y para ninguna otra cadena ms, cada vez que se le llame.
En las lneas 10 a 23 se tiene la declaracin de distintos arreglos articulares. Primero se
obtiene el nmero de articulaciones con que cuenta la cadena cinemtica, que, aunque se sabe
que son cinco (una por cada grado de libertad), es preferible realizarlo de manera automtica
para tener la certeza de que las dimensiones de los arreglos de articulaciones son las correctas,
luego vienen las declaraciones de presentpos, iterapos y trialpos que son estructuras de
datos que contienen los valores angulares de las configuraciones de cada uno de los
servomotores en distintas partes del cdigo.
Y finalmente, en las lneas 24 a 35 se encuentran las instrucciones para hacer el clculo
automtico de la matriz de transformacin que indica la posicin presente del gripper y
posteriormente la extraccin de los ngulos roll-pitch-yaw en la estructura convenida para tal
fin. En cuanto a las coordenadas de X, Y y Z pueden ser ledas directamente del vector p
contenido en el Frame present.
La ayuda de la librera de KDL, como se puede observar simplific en gran parte la
programacin de los clculos de cinemtica directa. Estos valores fueron corroborados por los
94
clculos realizados en la seccin 2.4 y algunas evaluaciones realizadas por ajustes realizados
al cdigo del Anexo B.
4.3. PROGRAMACIN DE CINEMTICA INVERSA.
El objetivo del programa de control del brazo robtico es el clculo de la cinemtica

inversa, pues el clculo de la cinemtica directa no tiene utilidad en s mismo, es decir, que su
nica utilidad es por ser parte crucial en el camino al clculo de la cinemtica inversa, por
tanto, la librera cinematica.h, que fue programada para el presente proyecto, ofrece el
solver de cinemtica inversa como su principal aportacin.
La librera cinematica.h, como se observa en los algoritmos descritos en la seccin 2.7
contiene diversas funciones y estructuras que ayudan a simplificar los clculos de cinemtica
inversa. En primera instancia est la definicin de las estructuras de tipo angles que
contienen los parmetros de roll-pitch-yaw, para que, aprovechando las ventajas del lenguaje
C++, los tres puedan ser manipulados como un solo objeto.
Se tienen funciones de conversin de tipos de ngulos de grados a radianes, pues en gran
parte del cdigo los ngulos se gestionan en grados por ser ms simple, pero KDL utiliza
ngulos en radianes, por tanto estas simples funciones de conversin dieron muy buenos
resultados en las etapas de programacin pues permitan una comprensin ms sencilla de los
ngulos a la par que una alta precisin en los clculos.
Adems contiene funciones para evaluar los errores que recordemos que en el Algoritmo
2.1 tienen una gran importancia; as como las funciones de evaluacin y aplicacin de casos,
que son vitales para la correcta aproximacin de las articulaciones en busca de la posicin
deseada; la funcin de incremento de casos y la de evaluacin de costos. De los algoritmos:
Algoritmo 2.1, Algoritmo 2.2 y Algoritmo 2.3 se observa que no hay grandes complicaciones
para su correcta y estable programacin en C++, pues con las estructuras principales del
lenguaje, en conjunto con las clases de KDL explicadas hasta ahora, resulta sencillo de
programar.
Durante la programacin del algoritmo de cinemtica inversa, se obtuvo la conclusin de
que es muy probable encontrar situaciones en que tras hacer el cambio correspondiente de un
95
caso, el algoritmo evaluara un caso en el que la accin consista en la operacin contraria a la
recientemente aplicada, por tanto podra ciclarse en hacer esos dos casos una y otra vez y no
alcanzar nunca la posicin deseada; es por eso que se implement una serie de condicionales
que evitan que una nueva seleccin de caso sea equivalente a la contraria al caso que se realiz
en la iteracin anterior.
En cuanto a la funcin de aplicacin de casos, result ser una funcin muy delicada y
que requiri de diversas modificaciones y pruebas en la etapa de desarrollo. En primera
instancia, resulta delicada por el hecho de que cada servomotor tiene un rango de actuacin
distinto, lo que ocasion que la etapa de programacin inicial de la funcin fuera ms tardada
de lo esperado por lo tedioso de vigilar los distintos lmites de cada servomotor. Y en segundo
lugar, cuando se hicieron las primeras pruebas del algoritmo completo, se observ que haba
ocasiones en que la seleccin de casos haca operaciones complementarias a pesar de las
limitantes implementadas en la funcin de incremento de casos; esto se deba a que existen
algunos casos que implican cambios en dos articulaciones de manera simultnea, por tanto sus
complementarios estn formados por dos casos de una sola articulacin. Estos casos resultan
ms difciles de prever, por tanto se implementaron dos constantes distintas en la aplicacin de
casos; es decir, que si se utiliza un incremento de un valor y un decremento en otro valor
ligeramente distinto, el resultado en la articulacin cuando se ejecutan casos complementarios
va a ser distinto al original. Esta sencilla medida ha demostrado ofrecer una mayor velocidad
en la bsqueda del arreglo de articulaciones que coloca al brazo en el punto y la orientacin
deseada. El cdigo puede ser consultado en el Anexo E- A.
Para la funcin de evaluacin de costos, como se explica en la seccin 2.7, tuvo la
complicacin de probarse ineficiencia por regresar al brazo a estados anteriores; por lo que las
lneas que incluan la evaluacin de la distancia del punto inicial al punto de la iteracin
fueron suprimidas, quedando nicamente las instrucciones para la evaluacin del error restante
con el punto objetivo. De esta forma se obtuvo una funcin ms sencilla y funcional para la
valoracin de los distintos casos a aplicar en cada iteracin.
Y por ltimo, la funcin clave de la librera cinematica.h, el solver para cinemtica
inversa, result ser un algoritmo de poco ms de 500 lneas incluyendo comentarios. Es una
funcin cclica bastante compleja, con una gran cantidad de condicionales anidados, pues son
muchas las cosas que deben ser evaluadas y comparadas; contiene una gran cantidad de
96
variables debido tambin a la complejidad del algoritmo, y a que requiere de espacio en
memoria para poder almacenar en tiempo de ejecucin los distintos casos y sus respectivos
errores, as como las variables correspondientes a los arreglos de articulaciones y Frames
inicial, final y de iteracin en curso.
La funcin de cinemtica inversa ha mostrado ser funcional y estable para los casos en
que ha sido probada, adems de ser capaz de encontrar una solucin y enviarla a los
servomotores dentro del tiempo estipulado para la comunicacin con el controlador del brazo,
que es de 0.1 segundos debido a la frecuencia elegida de 10 Hertz.
4.4. PROGRAMACIN DE ALGORITMO DE TRAZADO DE RUTAS.
En la seccin 2.9 se detalla el algoritmo de trazado de rutas, el cual es visualizado como

una secuencia que implica dos tipos de etapas: las etapas de secuencia constantes y etapas
flexibles.
Tales etapas de secuencia constantes son exactamente iguales a las descritas en la
seccin 3.5, que son formuladas con sumo cuidado evitando su interferencia negativa en las
posiciones alcanzadas por las etapas flexibles, as como en la ruta que se trata de seguir. El
Anexo E- B, que muestra en lenguaje C++ el cdigo de control del trazado de rutas, deja en
manifiesto las etapas rgidas de la secuencia, pues son bastante similares en funciones y
estructura a las que se observan en el Anexo D- A. Es importante destacar cmo tales etapas se
limitan nicamente a poner una posicin inicial o a cambiar el estado del gripper, de modo que
se asegura la no-interferencia con las posiciones alcanzadas por las etapas flexibles del
algoritmo de trazado de rutas.
Es importante destacar que las etapas flexibles son considerablemente ms complejas,
pues tienen en cuenta clculos matemticos vectoriales para encontrar cada uno de los puntos
por los que el brazo debe pasar, y as poder trazar la ruta que tiene planeada. Para esto resultan
nuevamente de gran utilidad las funciones de clases que KDL ofrece, simplificando en gran
parte la programacin de tales operaciones matemticas. Tambin son observables en el
Anexo E- B, las secciones que implican la asignacin de nuevos puntos, clculo de vectores
directores y las operaciones dentro del ciclo donde se calcula el nuevo punto para cada
97
iteracin, se manda a llamar a la funcin de cinemtica inversa, y se envan las respectivas
soluciones a los servomotores para que ejecuten el movimiento.
Se trata de un trazado de rutas en un ciclo abierto, donde no se obtiene retroalimentacin
y no admite que el objetivo o la plataforma robtica cambien de posicin una vez comenzado
el movimiento. Adems de ser un trazado de ruta en tiempo real, es decir que se va calculando
uno a uno los puntos por los que el brazo debe pasar, lo que se hace en tiempo de ejecucin,
justamente a la frecuencia de comunicacin con el brazo robtico.
El hecho de que sea en tiempo real presenta una importante ventaja computacional, y es
que no requiere de un espacio de memoria dedicado al almacenamiento de los puntos en los
distintos instantes de tiempo, pues slo es necesario almacenar un arreglo de articulaciones a
la vez, es decir las posiciones que tiene o debe tener el brazo en el mismo instante de tiempo
en que se encuentre.
Las pruebas han mostrado un correcto funcionamiento de este algoritmo de trazado de
rutas, sin embargo se ha observado en la ejecucin que existen algunos puntos en que la
funcin de cinemtica inversa no logra hallar solucin, son pocos los casos, y son puntuales,
es decir, por ejemplo, en el punto inicial se hall una solucin correcta y el brazo se desplaza a
ese lugar, y en la siguiente iteracin, el nuevo punto resulta imposible de calcular por el
solver, por lo que el brazo se queda inmvil en esa fraccin de segundo; as en tras calculado
el tercer punto de la ruta, el solver ya encuentra una solucin correcta, por tanto el brazo
reanuda su movimiento. Esto resulta desfavorable, pues afecta a la continuidad del
movimiento; por esta razn se consider hacer una modificacin al algoritmo y no hacer que
calcule los puntos en tiempo real, sino que se calculen de manera previa y que una simple
interpolacin permita encontrar los puntos especficos que no hayan sido calculados. As se
asegurara tener la respectiva configuracin de articulaciones para todos y cada uno de los
instantes de tiempo en que el brazo realiza el movimiento. Esto sin embargo no fue posible de
implementar por limitantes de tiempo para el proyecto, pero es importante considerarse para
un trabajo futuro en el robot.
98
4.5. SIMULACIN DE CINEMTICA INVERSA.
En las secciones anteriores se ha explicado cmo se han puesto en lenguaje C++ los
clculos y los algoritmos desarrollados, as como los recursos utilizados en el presente
proyecto tanto para el uso de ROS, como de la simulacin de secuencias fijas del brazo en
Gazebo.
La simulacin en Gazebo fue planeada con miras a poder simular cualquier tipo de
movimiento controlado por el nodoManipulacion. Para el caso de las secuencias, que se
explicaron en el Captulo 3. , seccin 3.5, se mencion la existencia de la funcin
FaseSecuencia y se explic su funcionamiento en el Algoritmo 3.2. Es justamente la lnea
2.8 del Algoritmo 3.2 donde se menciona la existencia de las instrucciones de envo de datos a
Gazebo. FaseSecuencia ya incluye, por tanto, en su funcionamiento, las instrucciones
necesarias para el envo de los mensajes a Gazebo; a diferencia del caso de trazado de rutas,
donde se utiliza la cinemtica inversa para hacer los movimientos flexibles, que son secciones
que no utilizan una funcin dada que se encargue de gestionar este flujo de la informacin, y
que por tanto se deben incluir las instrucciones para el envo de datos a Gazebo.
Se ha venido mencionando a lo largo del presente trabajo que se desarrollaron libreras
de codificacin y decodificacin de mensajes. Esto se debe a que para facilitar y estandarizar
el envo y recepcin de mensajes, se utilizan los mensajes de tipo cadena de caracteres; de
modo que todos los datos a enviar se convierten en tipo String para luego concatenar todas las
variables en una misma. En este caso, las funciones correspondientes a codificacin y
decodificacin de los mensajes que se envan a Gazebo se encuentran en la librera
mensajes_gazebo.h, esto se puede apreciar en las lneas 31.14, 51.11, 70.11 y 84.14 del
Anexo E- B, que son especficamente los llamados a la funcin de codificacin del mensaje,
es decir que se prepara la cadena de caracteres con los valores angulares de todas las
articulaciones del brazo, esto para la posterior publicacin de los mismos.
En cuanto a la publicacin, sta se lleva a cabo en la lnea siguiente a la codificacin,
esto es en las lneas 31.15, 51.12, 70.12 y 84,15, tambin del Anexo E- B, las cuales
simplemente publican el mensaje en el tpico al cual se encuentra suscrito el plugin de
Gazebo.
99
De esta manera, si se garantiza el tener las publicaciones al tpico cada vez que se
requiera que el brazo haga un movimiento, se asegura que el simulador va a responder ante los
ngulos requeridos.
Finalmente, si se analizan los algoritmos presentados en la seccin 2.9, particularmente
el Algoritmo 2.4, se puede comprobar la existencia de una serie finita de movimientos
claramente definidos. Los cuales, de una manera ms grfica, se aprecian tal como la Fig. 4.2
muestra en la siguiente pgina.
100
Fig. 4.2. Rutina de Cinemtica inversa. 1) Manipulador en posicin de descanso; 2)

posicin conocida inicial de rutina; 3) movimiento vertical hasta altura de objeto; 4) apertura
de gripper; 5) movimiento en lnea recta sobre el plano horizontal hasta objeto; 6) cerrado de
gripper; 7) movimiento vertical para levantar objeto; 8) movimiento de retraccin sobre el
plano horizontal.
A ningn hombre debe obligrsele a hacer el trabajo

que puede hacer una mquina.
(Henry Ford)
CAPTULO 5. EL BRAZO ROBTICO DE DONAXI.
Este ltimo captulo describe a grandes rasgos el diseo del brazo, el funcionamiento de
los servomotores que lo hacen moverse y la electrnica que lo compone. Se explican entonces
los requerimientos del cdigo de la interfaz que se comunica con la computadora central de
Donaxi y cmo se hizo este programa. Tambin es tema del presente captulo el diseo del
entorno de usuario y de las ventajas que este ofrece, y finalmente la integracin general del
programa.
101
102
5.1. SERVOMOTORES DYNAMIXEL.
En captulos anteriores se ha repetido en numerosas ocasiones que un brazo robtico est

constituido por una cadena cinemtica, la cual consiste en una serie de elementos rgidos
unidos por articulaciones. En las articulaciones, la mayora de las veces se encuentra acoplado
algn tipo de actuador, el cual ofrece al robot la capacidad de ejecutar acciones por medio de
tales articulaciones. Tambin se ha mencionado la existencia de dos tipos de articulaciones
principales: prismticas y rotativas. Las prismticas implican movimiento lineal a lo largo de
un eje, mientras que las rotativas son capaces de generar un movimiento alrededor de un eje.
En el caso del brazo robtico de Donaxi, cuenta nicamente con articulaciones de tipo
rotativo, las cuales deben estar actuadas por servomotores. Para ello, desde las etapas de
diseo y construccin del brazo se eligieron servomotores Dynamixel por las altas
prestaciones que estos son capaces de ofrecer teniendo muy en cuenta su bajo costo y reducido
tamao.
Robotis es una empresa dedicada a la produccin de elementos para robots y kits de
robots educativos y de hobbistas. Dentro de los mltiples productos que Robotis ofrece, se
encuentra una familia de servomotores conocida como Dynamixel, que consiste en varios
modelos de distintos tamaos, capacidades de carga y por ende distintos precios, lo que
favorece la eleccin del servomotor que mejor se ajuste a las necesidades particulares del
proyecto.
Es por eso que se decidi utilizar tres modelos distintos de servomotores en el brazo:
RX-28, RX-64, EX-106+. El brazo de Donaxi cuenta con seis articulaciones: cinco grados de
libertad ms el actuador para abrir y cerrar el gripper, con evidentes diferencias de
requerimientos de cada uno.
Tomando como punto de partida el hecho de que los servomotores ms alejados del
gripper requieren de un torque mayor, debido a la longitud del brazo, en comparacin con la
fuerza que debe tener el actuador del gripper o de la mueca, donde la longitud de su brazo es
mucho menor, se tom la decisin de utilizar dos servomotores EX-106+ en las dos
articulaciones del hombro; en la articulacin del codo y en la siguiente articulacin que es la
103
primera de la mueca, se opt por un RX-64 para cada uno; y finalmente dos RX-28, uno para
la segunda articulacin de la mueca y el sobrante para abrir y cerrar el gripper.
Por el momento se dejar de lado la explicacin de las diferencias entre cada uno de los
modelos de servomotores elegidos, y se explicarn las principales caractersticas en comn de
los mismos.
Se trata de servomotores de altas prestaciones en relacin con su reducido tamao y
peso; cuentan con engranes metlicos que reducen considerablemente el juego mecnico en
comparacin con otros servomotores educativos; resoluciones muy altas y una gran capacidad
de configuracin.
En primera instancia es importante tener en cuenta el protocolo de comunicacin. La
mayor parte de servomotores educativos y para hobbistas son controlados por medio de una
seal PCM (Modulacin por impulsos codificados), es decir que el ngulo que se desea que el
servomotor alcance est dado por una medida de ancho de un pulso a una frecuencia
especfica. En contraparte, los servomotores Dynamixel hacen uso de comunicacin serial
para recibir instrucciones y enviar informacin al controlador, en particular, el protocolo RS-
485 es el que utilizan los elegidos para el brazo de Donaxi.
Por medio de este protocolo de comunicacin es posible dar instrucciones al servomotor
o leer variables de estado. Esto se logra escribiendo mensajes en determinadas direcciones de
memoria, tales como un registro para la posicin o la velocidad de movimiento deseada, o en
su defecto, pidiendo respuesta de la informacin contenida en ciertas direcciones de memoria,
esto significa que pueden ofrecer retroalimentacin de alta precisin en cuanto a la posicin en
que se encuentra, la carga que sostiene, el voltaje de alimentacin, la temperatura, la
velocidad, entre otros parmetros tiles; admiten tambin conexin en una red por medio de
un bus, pues cada servomotor tiene un ID nico; cuentan con la presencia de un LED de
alarma o el envo de mensajes en caso de que sean superados los niveles permitidos de
esfuerzo o de corriente del servomotor; tambin permite configurar rangos de sumisin, en los
que se establecen mrgenes alrededor de las posiciones deseadas, en los cuales se establece un
torque menor al torque mximo especificado; es posible tambin configurar lmites de torque,
adems de la posibilidad de habilitar o deshabilitar el torque sin necesidad de cortar la
alimentacin del actuador (Dynamixel, 2010).
104
Adems son servomotores que no cuentan con topes mecnicos, es decir que, aunque su
control de posicin tenga un cierto rango muerto donde no puede llegar, ni controlar su
posicin, es posible por medio del comando correcto cambiar el servomotor a un modo de
rueda, en el cual se pierde el control de posicin, pero es posible hacer vueltas de 360 por
medio del control nicamente de velocidad. Esto sin duda resulta muy prctico en diversas
aplicaciones, pues no requiere de modificaciones internas al dispositivo si se desea utilizar
como actuador para aplicaciones con giros continuos.
Cada servomotor cuenta con dos terminales para conectarlo, ambas puenteadas entre s,
lo que facilita la interconexin del bus, pues se reduce la cantidad de cables y sus longitudes
que se requieren para conectar todos los motores de un robot (RX-Series, 2010), (EX-Series,
2010).
Revisando las especificaciones de cada uno de los modelos de servomotores resulta
evidente una compatibilidad entre todos de modo que aunque tengan caractersticas y opciones
distintas, utilizan las mismas direcciones de memoria para las mismas funcionalidades, lo que
simplifica la programacin del controlador, lo que se ver en la seccin 5.2.
Retomando lo explicado al principio de la presente seccin, los tres modelos diferentes
tienen una diferencia primordial que es lo que motiv a la decisin de que sean tres modelos y
no uno; tal diferencia atiende a los requerimientos de torque, como se mencionaba, los dos
actuadores del hombro (EX-106+) requieren un torque mucho mayor, por eso se eligieron
servomotores con un torque mximo de 106 Kg-cm (EX-106+, 2010); de modo que los
siguientes, con un brazo menor (RX-64), ofrecen un torque de 64 Kg-cm (RX-64, 2010); y
finalmente los ltimos dos, que van en la mueca y en el gripper (RX-28), cuentan con un
torque de 28 Kg-cm (RX-28, 2010). Como una ventaja secundaria de esta distribucin de
torques se tiene el peso de cada servomotor, que entre menor sea el torque, menor es su masa,
por tanto, es posible reducir el esfuerzo que deben realizar los servomotores del hombro que
son los que tienen mayor esfuerzo.
Debido a la gran cantidad de opciones que ofrecen estos motores, el control puede ser
tan complejo o tan sencillo como lo demande el proyecto en particular. Debido a los alcances
de la presente investigacin, se hizo una configuracin inicial de algunos parmetros de los
servomotores, como el ID de cada uno para que pudieran estar organizados y estandarizados,
el mximo torque permitido, lmites angulares que van acordes con los lmites de movimiento
105
propios del brazo, as como niveles de sumisin para evitar esfuerzos innecesarios en los
motores.
Luego de eso, el control en tiempo real implica escribir ciertas variables a todos los
servomotores, entre tales parmetros se encuentran un estatus del torque ya sea habilitado o
deshabilitado, un parmetro de ngulo que debe alcanzar y la velocidad angular esperada. Por
otra parte, lo que se pretende leer del motor es nicamente el valor del encoder de posicin y
del sensor de carga, para monitorear los niveles de esfuerzo que est recibiendo el actuador.
Para todo esto se dise un protocolo de comunicacin que reduce la cantidad de caracteres
basura que se transmiten y mantiene estable el sistema de envo y recepcin de mensajes.
Para el control de todos los servomotores se utiliz la tarjeta CM-700 tambin de
Robotis, que funciona justamente como la interfaz de comunicacin con el ordenador por
medio de comunicacin serial, y a la vez se comunica con todos los servomotores. Su
implementacin es descrita en la siguiente seccin.
Como se pudo observar en los prrafos anteriores, los manuales en lnea de cada
servomotor han sido suficientemente claros y completos para su correcta instalacin,
implementacin y control.
5.2. CM700
La tarjeta CM-700 es un mdulo de control con un microcontrolador ATMEL, el cual

permite la comunicacin TTL o, como en el presente trabajo, comunicacin RS-485 con
servomotores Dynamixel. Para su correcto funcionamiento requiere de dos partes:
a) Tarjeta CM-700.
b) Subtarjeta CM-700.
La primera es la encargada de gestionar el control general, mientras que la segunda se
encarga de las etapas de potencia y de las terminales de conexin con la fuente de
alimentacin, con el ordenador y con los servomotores.
Es importante mencionar que la tarjeta no puede comunicarse por s misma con el
ordenador, pues requiere de un protocolo serial con conectores exclusivos de Robotis (CM-
106
700, 2010). Para esto es que existe el mdulo LN-101 (USB Downloader (LN-101), 2010),
que es un convertidor de USB al protocolo serial utilizado por la tarjeta CM-700.
La tarjeta CM-700 (junto con la subtarjeta), cuenta con cinco puertos de comunicacin
RS-485 y cuatro RS-232 diseados para llevar datos y alimentacin a los respectivos
servomotores Dynamixel; puertos de cinco pines para la conexin de sensores que pudiera
requerir el robot; push buttons de control; LED's de notificaciones; terminal para batera y
fusible de proteccin (CM-700, 2010). Estas caractersticas resultan ser ms que suficientes
para la aplicacin que se desarroll en el presente proyecto.
La tarjeta fue diseada para cuatro tipos de uso de acuerdo con (CM-700, 2010):
Uso con RoboPlus Manager. Es un software propio de Robotis diseado para la
gestin manual de los registros de los servomotores. Es simple, pero requiere de
la intervencin de un operador en todo momento, por tanto, resulta prctico en
etapas de desarrollo para configuracin y verificacin de funcionamiento de los
servomotores (RoboPlus Manager, 2010).
Uso con RoboPlus Motion. Otro software de Robotis que tiene como propsito la
generacin simple de secuencias de movimiento de varios servomotores; permite
controlar y almacenar directamente en la tarjeta secuencias fijas de movimiento,
que pueden ser utilizadas en modo offline si son descargadas a la CM-700
(RoboPlus Motion, 2010).
Uso con RoboPlus Task. Tambin un software de Robotis, el cual permite
desarrollo de cdigos ms complejos que incluyen condicionales, lecturas de
sensores y dems en la generacin de las secuencias, los cuales tambin pueden
ser descargados a la tarjeta para su uso offline (RoboPlus Task, 2010).
Programacin de la tarjeta en Lenguaje C embebido. Es una modalidad que
permite una gran libertad para el programador, pues es posible la generacin de
cdigos para el uso offline u online de control de servomotores, ofrece acceso a
los sensores, a los mdulos de comunicacin serial con el ordenador y adems
toda la capacidad de programacin del lenguaje C (Embedded C, 2010).
107
Resulta evidente que de las cuatro modalidades nicamente dos fueron utilizadas.
Primero la modalidad con RoboPlus Manager para verificar la funcionalidad de los
servomotores y su respectiva configuracin de torques, lmites de movimiento, ID's y dems.
Y en segundo lugar, la modalidad de programacin en C embebido, que fue el recurso
ms explotado de la tarjeta.
1. En primera instancia se descargaron e instalaron las libreras requeridas para la
programacin del microcontrolador ATMEL (CM510/CM700, 2010).
2. Sigui la instalacin de WinAVR (Installing WinAVR, 2010).
3. Luego la instalacin de Atmel Studio (Installing Atmel Studio, 2010).
4. Y la preparacin del entorno y la plataforma Atmel Studio (Setting Environment,
2010).
Todos los pasos mencionados, a diferencia del resto del proyecto, han tenido que ser
sobre el sistema operativo Windows por requerimiento de Robotis.
En este punto se procedi a la revisin exhaustiva de la documentacin sobre la
programacin de la tarjeta (Basic Programming, 2010), as como los ejemplos de programas
para la comprensin de la correcta utilizacin de los comandos y las libreras (Example, 2010).
La programacin de C embebido de la tarjeta CM-700 implica modificaciones al
firmware que ocasionan que ste deba ser reemplazado por el firmware nuevo generado en
Atmel Studio, y que por tanto la tarjeta no pueda ser utilizada con el software de Robotis como
RoboPlus Manager; sin embargo, el firmware puede ser restaurado sin problemas a los valores
de fbrica en cuanto sea necesario (Restoring RoboPlus, 2010).
Para descargar el archivo .hex generado por Atmel Studio con la compilacin del cdigo
de C embebido, se requiere del uso de otra aplicacin de Robotis que se llama RoboPlus
Terminal (RoboPlus Terminal, 2010), la cual es meramente una consola de comandos que
permite una interaccin directa y a ms bajo nivel con la tarjeta CM-700. La terminal cuenta
con una herramienta llamada Boot Loader, cuya funcin es precisamente apoyar en la
descarga e instalacin del archivo compilado .hex al microcontrolador de la tarjeta (Boot
Loader, 2010). Una vez que se ha instalado el firmware deseado en la tarjeta, es posible correr
una prueba de la ejecucin dentro de la terminal misma, lo cual es muy sencillo, pero
dependiendo de la complejidad de la comunicacin, puede resultar inviable este tipo de
108
pruebas, como es el caso del presente proyecto, debido a los datos que se envan y se reciben
entre el ordenador y la tarjeta.
En cuanto a la programacin de la tarjeta CM-700 en el lenguaje C embebido, se
desarroll el cdigo que muestra el Anexo F- A. Se observa en primer lugar la inclusin de las
libreras Dynamixel.h y serial.h que son propias de Robotis diseadas especficamente
para la programacin de la tarjeta controladora.
En seguida se encuentra una tabla de constantes, las cuales hacen referencia a las
distintas direcciones de memoria de los servomotores. Esto con la finalidad de simplificar el
proceso de programacin hacindolo ms claro para el programador. Ya en la funcin main
del cdigo, luego de las declaraciones de variables, se encuentran las instrucciones de
inicializacin de comunicacin con los servomotores y la configuracin inicial de los mismos.
El ciclo while principal se encarga de mantener iterando indefinidamente el programa de
la tarjeta, el cual establece, de manera general, los pasos:
1. Primero se hace la lectura de los encoders de posicin de los servomotores.
2. Luego, la lectura del registro de carga de cada uno de los actuadores.
3. En seguida espera recibir las instrucciones del ordenador en el protocolo
establecido, es decir, lee la cadena de caracteres e interpreta todos los parmetros
que debe enviar a los servomotores.
4. Justo despus enva de igual forma a la computadora una cadena de caracteres en
el protocolo establecido que contiene toda la informacin de los encoders y las
cargas de los servomotores.
5. Y finalmente, efecta los comandos de escritura de instrucciones de posicin y
velocidad a los servomotores, dependiendo del estado de torque recibido del
ordenador.
109
5.3. PROGRAMACIN DE LA INTERFAZ.
En la seccin anterior se dio un panorama general del programa utilizado en la tarjeta

CM-700. En esta seccin se presentar con mayor detalle el protocolo de comunicacin
utilizado, por un lado, entre la tarjeta de control y los servomotores, y por otro lado, entre el
ordenador y la tarjeta. As como tambin se establecern los principios de la parte del
nodoInterfaz, que se encargan de la comunicacin serial.
En la librera dynamixel.h se presentan las funciones: dxl_write_word() y
dxl_read_word(), las cuales se encargan de gestionar el protocolo de comunicacin propio
de los servomotores. Para la primera de las funciones mencionadas, es evidente que su
propsito es el envo de datos a los servomotores, y es tan sencillo como que sus parmetros
de entrada son tres: el ID del servomotor, la direccin de memoria donde se desea almacenar
el dato a enviar, y finalmente el valor que queremos que tenga el parmetro al que se escribe.
Por otra parte, la funcin de lectura de datos: dxl_read_word() devuelve el valor numrico
del registro que se requiera; sus parmetros de entrada son el ID del servomotor y la direccin
de memoria del registro requerido. Estas dos funciones simplifican la programacin, adems
de que ofrecen la libertad de organizar las instrucciones de escritura y lectura de datos de la
manera que resulte ms conveniente, pues no requiere de una sincrona para la correcta
comunicacin.
Por parte de la comunicacin con el ordenador, se disearon dos cadenas, una para los
datos que la CM700 recibe del ordenador, y la otra para los datos que enva. Para reducir la
cantidad de datos basura, se defini arbitrariamente un orden para los datos, y se mandaban
crudos, sin ningn tipo de etiqueta que indicara el tipo de dato transmitido. As, la tarjeta
recibe primero todos los datos del servomotor 1, luego del servomotor 2, y as sucesivamente;
dado que es un orden fijo y que siempre se va a recibir la cadena completa, terminan siendo
prescindibles las etiquetas de los datos, dando por sentado que el programa del ordenador
siempre manda la cadena con los datos en el mismo orden.
Ocurre lo mismo en el sentido contrario, el programa de la tarjeta CM-700 enva una
cadena continua con los datos de todos los servomotores ordenados pero sin etiquetas de
ningn tipo. Como se observa en el cdigo del anexo F-A las funciones de envo de datos a la
110
computadora son fijas, no cambian bajo ninguna circunstancia; de modo que el programa del
ordenador tampoco cambia sus funciones de interpretacin de la cadena bajo ninguna
circunstancia.
En cuanto a la comunicacin desde el punto de vista del nodoInterfaz, se tuvieron
algunas consideraciones iniciales antes del comienzo de la escritura del cdigo. Es evidente la
necesidad de la comunicacin con ROS de la que se ha hablado en captulos anteriores, pues el
programa requiere forzosamente estar funcionando como un nodo de ROS que pueda
suscribirse a un tpico donde el nodoManipulacion coloca la informacin que debe interpretar
este nodo y enviarlo a los servomotores. En segundo lugar tambin necesaria la capacidad de
controlar un puerto serial para el envo y recepcin de datos de la tarjeta CM-700. Y por
ltimo se decidi que sera de gran utilidad para las etapas de preparacin de pruebas que se
realizan en las competencias, o incluso durante el tiempo de programacin y depuracin, el
desarrollo de un entorno grafico para que el usuario pudiera monitorear el estado de los
servomotores, as como manipularlos para verificacin de configuracin y funcionalidad.
Se decidi que estas tres caractersticas se centraran en un mismo programa, que bien
poda ser programado en Python por las ventajas que ofrece este lenguaje en cuanto a libreras
de funciones, adems de la sencillez que representa la implementacin de programas debido a
las propias caractersticas de estructuracin del lenguaje.
En la seccin 3.4 se menciona la disponibilidad de funciones de comunicacin dentro de
ROS para programas en Python, los cuales tienen una estructura similar a la de C++, y su
funcionamiento es igual. Se declara la suscripcin al tpico y cada que hay un mensaje ocurre
un llamado a una funcin de lectura y almacenaje del mensaje. Adems de que es ah donde
ocurre la decodificacin del mensaje.
Para la comunicacin serial se determin el uso de la librera serial de Python, la cual
ha sido probada en otros proyectos y ha demostrado ser funcional. Sin embargo, para
optimizar el funcionamiento, se determin que las operaciones de envo y recepcin de datos
por el puerto serial se encuentren en un hilo paralelo del programa principal y que los datos
por escribir o los parmetros ledos estn almacenados en variables globales, a las cuales se
tenga acceso en todo momento por las distintas partes del programa. Para la colocacin de
operaciones en hilos paralelos se utiliz la librera threading, la cual permite generar la
cantidad de hilos que sean necesarios en un programa. Dados los tiempos de espera de envo y
111
recepcin de mensajes va serial, la interfaz podra sufrir retrasos en su actualizacin, por lo
que al lanzarse una funcin en paralelo que gestione tales tiempos de espera y que sea
eliminada una vez que termine su ejecucin result ser una elegante y funcional solucin para
tal problema. Los tiempos de las operaciones de comunicacin fueron calculados para que a
una frecuencia de 10 Hz tenga suficiente tiempo para lanzar el hilo, cumplir su propsito y
terminar antes de que la siguiente iteracin lanzase un hilo nuevo.
La programacin con hilos es un tanto delicada, sobre todo cuando stos son lanzados en
numerosas ocasiones a lo largo de la ejecucin, debido a que si los hilos lanzados no tienen un
tiempo de vida bien definido, podran durar ms de lo conveniente ocurriendo una demanda
muy alta de recursos computacionales, los cuales pueden incluso llegar a no ser suficientes. Es
por eso que en el hilo del presente trabajo, dado que es llamado cada 100 milisegundos, se
cuid mucho su estructura interna para asegurar que haya terminado de ejecutarse en un
tiempo menor a los 100 milisegundos, por ejemplo, el timeout de espera de datos del puerto
serial es de 0.08 segundos, lo que lo convierte en un tiempo muerto muy alto, pero sin duda
suficiente para evitar que el hilo se contine ejecutando.
Esto ha demostrado en las pruebas una gran fluidez en la comunicacin tanto serial con
la tarjeta controladora, como en ROS con el nodoManipulacion, sin olvidar el estable
funcionamiento de la ventana que genera el entorno grfico.
El programa en Python result demasiado extenso para ser incluido en un anexo, sin
embargo, es posible consultarlo en los archivos que acompaan al presente documento. Que es
donde se puede verificar la estructuracin del cdigo completo.
5.4. ENTORNO DE USUARIO.
Para la programacin del nodoInterfaz, que fue programado en Python, result

conveniente la consulta de un libro que explicara los fundamentos del lenguaje de
programacin: (Gonzlez Duque, s.f.), del cual se revisaron y estudiaron los paradigmas
bsicos de la programacin en Python, lo que serva como punto de partida para el inicio de la
estructuracin del cdigo.
112
Antes del comienzo de la programacin del proyecto, en la etapa de toma de decisiones
de programacin del proyecto, se realiz una breve e informal investigacin acerca de las
distintas libreras de generacin de entornos grficos en Python. Era un punto de partida, y si
resultaba necesario, se desarrollara una investigacin ms extensa, sin embargo no se
encontraron en primera instancia grandes diferencias entre ellas, salvo ventajas o desventajas
que otros programadores comentaban en foros en cuanto a portabilidad o la cantidad de
herramientas de que dispone la librera; as como asuntos de compatibilidad con otras libreras.
Tras algunos comentarios observados, se determin que no hara gran diferencia la
librera elegida, y se opt por wxPython debido a que era de las presentadas como ms
completas en cuanto a cantidad de herramientas que ofrece y que a la vez cuenta con una
buena portabilidad en el caso de que el programa requiera ser mudado de plataforma, o con
miras a que los conocimientos del uso de la librera pudieran ser utilizados en alguna otra
ocasin con algn proyecto distinto que requiera de otra plataforma.
Al ser wxPython una plataforma muy difundida para la creacin de entornos de usuario
con Python, existe una gran cantidad de informacin, cursos en lnea y tutoriales para conocer
desde lo ms sencillo a lo ms complicado de programar con esas libreras. El tutorial
utilizado para el desarrollo del presente trabajo es el (Bodnar, 2014), el cual es explicado de
manera muy clara y concisa y con ejemplos sencillos y funcionales.
Se fij el objetivo de construir una ventana que mostrara de una forma ordenada los
parmetros de todos los servomotores, esto es, que muestre las lecturas del encoder de
posicin y de carga de cada servomotor, controles individuales de velocidad, posicin y
habilitar/deshabilitar torque que permitan hacer pruebas rpidas de los motores. Esto implica
que el programa pueda estar en cualquiera de dos modos distintos, que se les llam modo
ROS y modo test. El primero, como su nombre lo indica existe para que el control de los
motores obedezca las instrucciones publicadas en el respectivo tpico de ROS, y por tanto
ignora las instrucciones de los controles en pantalla; el modo test es precisamente para hacer
que los motores obedezcan a lo indicado en pantalla, y por tanto se ignorar lo publicado en el
tpico de ROS. Y por ltimo, ya en las etapas de desarrollo se tom la decisin de incluir una
caracterstica extra, que consiste en campos de introduccin de los datos que el nodoMaestro
escribe al nodoManipulacion; esto con el propsito de que el nodoInterfaz pueda publicar
mensajes a peticin de un operador en el tpico correspondiente, hacindose pasar por el
113
nodoMaestro, y que el nodoManipulacion pudiera recibir tales instrucciones para ejecutar las
distintas tareas del brazo de una manera sencilla; esta caracterstica resulta til en la
verificacin de funcionalidad en etapas de preparacin de pruebas, sin la necesidad de lanzar
todo el sistema de control completo del robot.
Tras una revisin profunda de las herramientas de wxPython (Bodnar, 2014) se lleg a la
conclusin de que sera sencillo y claro para el usuario si se organizaban todos los elementos
(etiquetas, indicadores, controles y dems) en una matriz donde cada servomotor tuviera una
fila asignada, y por otra parte, que las columnas sean utilizadas para organizar los controles o
indicadores. Fue as que la ventana qued como muestra la Fig. 5.1.
Fig. 5.1. Entorno de usuario para el control del brazo.

Como se puede ver, los datos y controles estn organizados de una manera clara y
concisa. La primera columna muestra las etiquetas de los servomotores ordenados en su fila
respectiva. En la segunda se muestran las lecturas del encoder en grados que representan el
ngulo correspondiente a los parmetros Denavit-Hartenberg. La carga fue representada con
barras que se iluminan proporcionalmente al esfuerzo que realiza el servomotor
114
correspondiente. El control de posicin est dado por un slider que simplemente debe ser
arrastrado por el mouse mientras el torque est habilitado. La velocidad est dada por los
cuadros de texto donde se coloca el valor de la velocidad en unidades de 0.111 rpm. Cada
servomotor cuenta con un botn para habilitar o deshabilitar el torque, el cual cambia su color
dependiendo del estado seleccionado, adems de un botn de torque general que permite,
mediante condiciones en el cdigo, que se puedan apagar los torques de todos los
servomotores en situaciones de emergencia, por tanto, ningn servomotor se puede poner a
funcionar si tal control est deshabilitado. Y por ltimo se encuentra la columna actividad, que
es independiente del control individual de los servomotores. El propsito de esa columna es
que el usuario llene los cuatro campos que requiere el mensaje normalizado de ROS:
actividad, coordenada X, coordenada Y y coordenada Z; y que una vez llenados los datos, un
click en el botn publicar lanza una funcin en que se codifica el mensaje y se publica en el
tpico del cual el nodoManipulacion lee las instrucciones del nodoMaestro.
Por la gran cantidad de datos que es necesario manipular, gran parte del cdigo consiste
en asignaciones de valores de variables, lecturas de datos del tpico, del puerto serial y de los
controles grficos, as como escritura por el puerto serial, despliegue de datos, y dems. Sin
embargo, en cuanto a su estructura principal consiste en un ciclo indeterminado sin ninguna
accin interna, dentro de la cual ocurren mltiples llamados a funciones: a) un llamado a la
funcin de inicializacin del entorno grfico, b) un llamado a la funcin principal, la cual tiene
un llamado interno a si misma cada 100 milisegundos, generando as la frecuencia de iteracin
de 10 Hz del nodo; c) dentro de la funcin principal ocurren las revisiones del tpico de ROS,
que por tanto llaman a la funcin de lectura de datos si es que se encuentra algn mensaje; d)
por otro lado, cada accin del usuario a alguno de los controles de la interfaz dispara un
evento, el cual llama a una funcin que ejecuta las acciones correspondientes a la instruccin
indicada; y finalmente, e) se lanza tambin dentro de la funcin principal el hilo MiThread
que es quien contiene las operaciones de envo y recepcin de datos a travs del puerto serial.
Como se explic, el nodo funciona como una serie de funciones e hilos independientes
que se llaman entre si y que comparten informacin por medio de variables globales; se trata
de una programacin que sale de las secuencias usuales de lnea a lnea, pero que ofrece una
gran cantidad de funcionalidades optimizando los recursos computacionales adems de una
gran estabilidad probada en las comunicaciones.
115
El cdigo, tal como se explica en la seccin 5.3, debido a su extensin, no fue incluido
en los anexos, pero puede ser consultado en los archivos que se presentan junto al presente
documento.
5.5. IMPLEMENTACIN CON ROS DE LA INTERFAZ.
Se ha hablado mucho, tanto en este captulo como en los anteriores, de los medios por
los cuales se comunican las distintas partes del control de la manipulacin, as como de la
transferencia de datos entre el ordenador y la tarjeta controladora de los servomotores. Por
tanto, al ser tres programas simultneos que se encargan de la manipulacin y varios otros para
el resto del control completo del robot, parecera que ni el lanzamiento ni el uso de los
programas son sencillos. Es lo que se tratar de explicar en la presente seccin.
Desde el principio de las etapas de desarrollo se observ la complejidad que implicaba el
lanzamiento de los nodos, pues eran varios comandos que deban ser recordados, rutinas de
encendido y apagado que deban ser respetadas y dems. La experiencia del uso y depuracin
del sistema de control del brazo fue la propia fuente de ideas para la optimizacin del proceso.
El proceso, de manera general, para lanzar las pruebas quedaba como se explica a
continuacin:
1. Primero deba conectarse y encenderse la tarjeta CM-700 para preparar el puerto
serial y evitar la recepcin de datos basura del nodoInterfaz al no haber un
elemento conectado.
2. Luego el ncleo de ROS, seguido del nodoManipulacin se inicializa para que
prepare los tpicos de ROS, y quede en espera de instrucciones.
3. Le segua el nodoInterfaz, hecho as para que pueda inicializar el puerto serial y
comenzar la comunicacin con la CM-700, as como quedar el canal de ROS en
espera de que el nodoManipulacin comience a publicar.
4. Es entonces cuando debe comenzar a ejecutarse el nodoMaestro para que enve
las respectivas instrucciones al nodoManipulacion y toda la cadena pueda
funcionar.
116
De la misma forma, para detener la ejecucin, el orden no deba ser tan estricto, pero
deban detenerse forzosamente todos los nodos y apagarse la tarjeta CM-700, dejando el
ncleo de ROS para el final. Era necesario que todo fuera detenido para evitar problemas en la
comunicacin, pues era muy probable que si se volva a ejecutar el sistema sin reiniciar todos
los mdulos ocurriera un error en la sincrona, ocasionando que los datos enviados y recibidos
por el puerto serial fueran incorrectos.
Sin duda tal proceso resultaba engorroso para el usuario, sobre todo en etapas de
calibraciones de pruebas, porque usualmente se requieren mltiples ejecuciones de los
programas. Por eso fue que se tomaron medidas para simplificar un poco este proceso. Tales
medidas fueron implementadas sobre el nodoInterfaz, pues por ocupar un punto intermedio en
la comunicacin ofreca el suficiente campo de accin para tales mejoras.
En primer lugar, ya se ha explicado en la seccin 5.4 la inclusin de un bloque de cdigo
que le permite al nodoInterfaz publicar mensajes para el nodoManipulacion cual si fuera el
nodoMaestro. Tal caracterstica permite una mejor integracin, pues permite que el usuario
pueda controlar el brazo como lo hace el nodoMaestro desde un cmodo e intuitivo entorno
grfico. Ver Fig. 3.7 de la seccin 3.4, donde se muestra en esquemtico los nodos y los
tpicos por los cuales se comunican.
En segundo lugar, est la ejecucin de los dos nodos de ROS que implican el control de
la manipulacin. Si bien el orden en que se ejecutan no es riguroso, es muy importante que
ambas aplicaciones sean lanzadas correctamente. Para eso se gener un archivo launch como
se explica tambin en la seccin 3.4, el cual especifica que ambos nodos deben ser ejecutados.
Esto representa una nueva ventaja, pues los dos comandos para ejecutar cada uno de los nodos
se redujeron a una sola instruccin.
Lo ms delicado en la ejecucin es el hecho de que la tarjeta CM-700 debe estar
encendida antes de comenzar con la ejecucin de los nodos, y se es un factor que no puede
ser modificado debido al propio funcionamiento electrnico de la tarjeta CM-700 y el
adaptador LN-101; pues se generan seales basura que interfieren con el funcionamiento del
programa si se ejecuta primero el nodo interfaz y en segundo lugar se enciende la tarjeta
controladora. Sin embargo, s fue posible el desarrollo de una optimizacin en sa rea. Se
observ que los problemas de sincrona que ocurran se deban a que si uno de los dos
programas cesaba su ejecucin, ya sea la CM-700 o el nodoInterfaz, pero el otro no, la
117
comunicacin serial se quedaba en alguna etapa de espera que no necesariamente coincide con
la etapa con que comenzara el otro programa nuevamente su ejecucin. Dado que las
funciones de comunicacin serial de la tarjeta CM-700 no cuentan con un timeout, sino que se
quedan en espera todo el tiempo que sea necesario hasta recibir el dato, la solucin propuesta
fue sencilla: El botn salir del entorno grfico no tiene como nica funcin el terminar la
ejecucin, sino que su tarea se vuelve tambin el ejecutar una funcin de finalizacin de la
conexin; de modo que si al ser activado, la seccin de comunicacin serial se encuentra en
alguna etapa de la transferencia de datos, se le da al hilo en curso la oportunidad de terminar
sus operaciones, es decir, terminar el ciclo completo de comunicacin, y entonces, con la
iteracin completada, ya resulta seguro detener la ejecucin del programa. Esta accin causa
que el programa de la CM-700 tambin complete una iteracin del programa y quede
nuevamente en espera del primero de los datos de la cadena, lo que permite recuperar de
inmediato la conexin con el nodoInterfaz en cuanto este se vuelva a ejecutar sin la necesidad
de reiniciar la tarjeta. Una importante ventaja de esto es el hecho de que si ROS debiera
detenerse por alguna razn, el brazo no requiere ser reiniciado, pues puede reanudar la
comunicacin en cuanto ROS se reestablezca. En cambio, si durante la ejecucin la tarjeta
controladora sufriera un reinicio imprevisto, s ser necesario reiniciar la ejecucin del
nodoInterfaz, pues no se implementaron medidas para la recuperacin de la conexin ante tal
situacin.
Recordando que si fuera necesario hacer un reinicio del sistema de control, es
conveniente detener todo para volverlo a lanzar despus, se tom una medida ms para
simplificar los procesos. Dado que en este punto se vuelve necesario el terminar la ejecucin
del nodoInterfaz por medio de un click en el botn salir y no de ninguna otra manera, se
aprovecha tal accin para aadir un mensaje extraordinario a la librera mensajes.h. Este
mensaje es enviado, durante la ejecucin rutina de finalizacin, por el nodoInterfaz al
nodoManipulacin, el cual representa la instruccin de terminar su ejecucin. Esto hace que
un solo click del operador cause la finalizacin de los dos programas de control.
En este punto la ejecucin de los nodos correspondientes a la manipulacin y la
correspondiente preparacin de pruebas se ve simplificado y optimizado de manera
importante, pues puede decirse que los pasos requeridos son en resumen:
118
1. Encender tarjeta controladora de brazo. Alimentacin por bateras, conexin de
los motores y encendido de interruptor fsico.
2. Ejecucin de archivo launch para manipulacin. El cual contiene instrucciones
para lanzar el ncleo de ROS, el nodoManipulacion y el nodoInterfaz. Esto con
el comando: roslaunch Manipulacion manipulacion.launch
3. Envo de instrucciones a travs del entorno grfico. Llenar Actividad,
coordenada en X, coordenada en Y y coordenada en Z y dar click en publicar.
Resulta considerablemente ms sencillo de recordar y ms intuitivo tambin de ejecutar.

Por otra parte la finalizacin de la ejecucin tambin se vio considerablemente simplificada:
1. Click en botn salir. El cual finaliza la conexin con el puerto serial de un
modo seguro e indica al nodoInterfaz que debe detener su ejecucin.
2. Apagar tarjeta CM-700. nicamente si no se va a seguir utilizando el control,
pues si se pretende utilizar de nuevo en un corto tiempo, no es necesario apagar
la tarjeta.
Con la optimizacin del proceso de ejecucin y detencin del sistema de control del
brazo se concluye toda una serie de etapas de desarrollo que, como se ha observado a lo largo
del presente trabajo, abarcan diversos aspectos cuidando estabilidad, funcionalidad,
optimizacin de uso de recursos, simplificacin de operacin, normalizacin de vas de
comunicacin y reduccin del cdigo utilizado en los programas.
Las que conducen y arrastran al mundo no son las mquinas,

sino las ideas.
(Victor Hugo)
CAPTULO 6. CONCLUSIONES Y PERSPECTIVAS
Este ltimo captulo presenta gran parte de las observaciones que se lograron con el
desarrollo del proyecto, teniendo en cuenta siempre la relacin con el cumplimiento de los
objetivos propuestos. Tambin es importante mencionar ciertos caminos a considerarse para
trabajos futuros en manipulacin tanto en Donaxi como en proyectos distintos.
119
120
6.1. CONCLUSIONES.
El presente trabajo muestra los resultados del desarrollo del sistema de control del brazo
manipulador del robot de servicio Donaxi. Para la ltima etapa se requiri de un trabajo de
varios meses, a pesar de que el tiempo de desarrollo total de este software fue de dos aos
aproximadamente, pues se haban programado otras propuestas de solucin del problema, las
cuales de una o de otra manera no cumplan con lo necesario para tener un correcto y verstil
control; sin embargo, ofrecan las posibilidades de probar y evaluar ciertos recursos de
programacin, a la par que probar el funcionamiento del brazo y analizar los movimientos que
ste puede hacer, as como los que no puede hacer.
Tales experiencias previas, sin duda fueron de gran ayuda para el desarrollo de lo
explicado a lo largo de todo este trabajo, pues una gran parte de los comportamientos del
brazo, as como de los requisitos del software y las posibilidades de programacin se pudieron
tener en cuenta desde una etapa previa a la escritura del cdigo, lo que aceler en gran parte
las etapas de depuracin del programa, pues se pudieron obtener mejores resultados gracias a
que el conocimiento previo sent las bases para el desarrollo que en versiones previas del
software de control no se tenan.
En primera instancia, los clculos de cinemtica directa fueron comparados con clculos
realizados por otras herramientas de software (como LabView) observndose valores, modelos
y resultados iguales en ambas plataformas, lo que dio la pauta para continuar con el trabajo.
El desarrollo del algoritmo de cinemtica inversa fue un proceso muy complejo, el cual
implic una considerable cantidad de tiempo de depuracin, pues como se explica en la
seccin 2.7, tras la realizacin de una prueba que consuma considerable tiempo para el
anlisis de resultados, fueron surgiendo distintos errores o imprecisiones del algoritmo, los
cuales deban ser corregidos, y por consiguiente nuevas pruebas deban efectuarse. A pesar de
estas complicaciones, como tambin se termin de explicar en el Captulo 2. , se obtuvieron
muy buenos resultados, pues se observ que en la mayora de los problemas que se le pedan,
lograba converger en una solucin, aunque an existan algunos casos en los que no hallaba la
solucin con ningn motivo aparente.
121
Esta propuesta de cinemtica inversa result ser un aporte muy importante para el
funcionamiento del sistema de control, pues en etapas previas del desarrollo del proyecto
Donaxi, no se haban tenido mtodos completos para realizar tales clculos. Pues aunque se
haban hecho algunas pruebas con solvers genricos, stos no ofrecan lo que el desarrollado
en el presente trabajo puede ofrecer al manipulador de Donaxi. Esto se debe a que se trata de
un algoritmo construido especial y especficamente para funcionar con el brazo de Donaxi,
teniendo en cuenta en todo momento las capacidades y limitantes del diseo mecnico, los
requerimientos para que el robot pudiera cumplir sus tareas, el estado actual del resto de
componentes del robot, y dems. En general, una solucin que favorece enormemente el
desarrollo del proyecto.
En cuanto a la planificacin de rutas, la simpleza con la que se dio solucin al problema,
represent una gran ventaja en la ejecucin, pues ofrece una gran estabilidad, precisin y
repetibilidad de los movimientos; adems de que partiendo de los puntos expresados en la
seccin 2.9, se puede garantizar que el objeto ser tomado y sin colisiones siempre y cuando
ste se encuentre dentro del espacio de trabajo del brazo. Adems de la gran compatibilidad
entre ste proceso con el algoritmo de cinemtica inversa.
En cuanto a la arquitectura de control implementada en ROS, ha demostrado, desde
principios del 2012 que se implement ROS oficialmente en el proyecto, que es una
arquitectura muy poderosa, verstil, econmica en recursos y principalmente se destaca su
estabilidad, que es algo muy importante en el control de un robot de servicio como Donaxi,
debido a que se enfrentan a entornos dinmicos y problemas complejos, de modo que si la
estabilidad se viera comprometida en algn momento, el resultado sera una tarea incompleta,
un riesgo para la integridad del robot, o incluso un riesgo para las personas que interactan
con l. Por eso es que la arquitectura implementada de esa forma permite gran fluidez en las
comunicaciones entre nodos, por lo que el nodoMaestro, que tiene acceso a toda la
informacin de alto nivel, puede tomar decisiones preventivas para correccin de errores, o
incluso evitar accidentes.
La simulacin en Gazebo de los movimientos del brazo ha sido una herramienta de gran
ayuda que ha permitido simular de una manera sencilla la ejecucin de los movimientos del
brazo en un ambiente ideal. La puesta en marcha, que aunque no fue fcil ni trivial, termin
siendo un proceso muy sencillo una vez lograda la simulacin por primera vez; lo que la
122
convirti en una herramienta que permiti ahorrar tiempo de depuracin, pues se hacan las
primeras pruebas en virtual para evitar el tiempo de conexin y verificacin de los motores
para cada prueba, adems eso permita al resto de los compaeros del equipo seguir trabajando
con el robot en el resto de sus funcionalidades, y limitando el uso del brazo para las pruebas
finales. Otra de las ventajas que se pudo apreciar fue el hecho de que para una gran parte de
las pruebas de depuracin no fue necesario el uso del brazo, pues alguna rutina nueva para el
brazo tena un factor de riesgo muy grande debido a la posibilidad del error humano; as, se
podan detectar estos errores a nivel simulacin para corregirlos antes de echarlos a andar con
el prototipo fsico.
Por ltimo, la interfaz de usuario provee al operador una herramienta invaluable que le
permite poner en marcha el control del brazo del robot, as como realizar pruebas de una
manera muy sencilla, sin la necesidad del conocimiento a fondo del cdigo, es decir, cualquier
persona, con un mnimo de informacin sobre el software de control, es capaz de gestionar el
sistema de control del brazo de Donaxi. Esto parte de la premisa de que para obtener un
software eficiente en el cumplimiento de su objetivo, resulta necesario que se simplifique lo
ms posible su interaccin con el usuario, pues finalmente, si ste no es capaz de comprender
la gran cantidad de informacin que el software despliega, o simplemente los comandos de
control resultan engorrosos, es poco probable que el sistema de control llegue a ser exitoso.
El uso de la interfaz en las etapas de depuracin y calibracin de los movimientos del
manipulador ha probado que realmente es un medio de gran ayuda para el operador, pues
permite que la interaccin hombre-mquina se lleve con mucha mayor fluidez, de una manera
ms natural y sencilla.
123
6.2. PERSPECTIVAS Y TRABAJO FUTURO.
Evidentemente, cualquier aportacin tecnolgica nunca est terminada, siempre existen

puntos a mejorar, optimizar, cambiar o incluso aadir. El presente trabajo no est exento, y
durante las etapas de prueba o incluso la redaccin del presente documento, se observaron
puntos que sera conveniente que fueran revisados en trabajos posteriores para mejorar el
funcionamiento del sistema.
En primer lugar est la mencin de la falta de grados de libertad. Al ser un manipulador
de cinco grados de libertad la capacidad de movimientos y orientaciones que el brazo puede
alcanzar se ve bastante limitado, principalmente teniendo en cuenta su desempeo en las
situaciones para las cuales fue diseado, esto es: la manipulacin de objetos. Se sugiere revisar
la posibilidad de aadir un grado de libertad justo arriba del codo, lo que permita al codo girar
alrededor del eje vertical, esto representara una gran ventaja para incrementar el espacio de
trabajo, a la par que facilitar los movimientos para tomar o colocar objetos. Sin embargo,
aunque este grado de libertad extra sera una gran ayuda en la posicin del gripper, no lo es
para corregir su orientacin; esto causa la generacin de otra propuesta: se trata de que el
grado de libertad aadido sea en la mueca, en una posicin donde permita controlar el pitch,
pues, segn las secciones 2.7 y 2.7, es un parmetro que hasta el momento no se puede
controlar de manera independiente. Una solucin de aadir ambos grados de libertad para
hacerlo antropomrfico (es decir, redundante al tener siete grados de libertad), sera lo ideal,
pero evidentemente es mucho ms costoso en todos los aspectos aadir los dos grados de
libertad. Resulta vital hacer un anlisis que determine cul opcin es la mejor.
Por otra parte, aunque la solucin del trazado de rutas ha demostrado ser eficiente y
estable con ayuda del ajuste de velocidades que se aprecia en las ecuaciones del final de la
funcin del solver (ver cdigo de librera cinematica.h), se han observado ciertas reas de
oportunidad en tal algoritmo.
En primera instancia, se ha mencionado que hay ciertos casos en los que el algoritmo de
cinemtica inversa no es capaz de converger en una solucin; en tales situaciones, lo que el
solver devuelve es la posicin original del brazo. Esto no representa gran problema, porque
124
como son casos aislados, el algoritmo calcula el siguiente punto y ah recupera la trayectoria
que estaba trazando.
Por otra parte, dado que las variaciones entre un punto y el anterior son muy pequeas,
hay momentos en que una corrida del solver de cinemtica inversa no genera cambios en todas
las articulaciones debido a que encuentra la nueva configuracin en unas pocas iteraciones.
Aunque esto pareciera una ventaja de ahorro de iteraciones, genera tambin complicaciones en
el control.
Los dos prrafos anteriores mencionan situaciones en las cuales hay etapas en las que
hay servomotores que deben detener el movimiento, para luego reanudarlo al doble de la
velocidad que llevaba, pues el movimiento esta vez debe ser mayor. Esto, como se puede
intuir, representa un problema en el control, pues implica aceleraciones y desaceleraciones
infinitas y que se repiten en distintas etapas del movimiento. El cual fue enfrentado por medio
del uso de constantes en los clculos de velocidades deseadas de movimiento. Algunas
constantes evitan que se llegue a velocidades de cero, as como otras evitan que las mismas
lleguen a ser muy altas; esto suaviza los cambios bruscos de velocidad generando un
movimiento ms fluido.
Una propuesta que podra ser evaluada en futuros trabajos es la del clculo de la
trayectoria de manera offline, para luego ejecutar el movimiento de una matriz calculada
previamente. Esto, a pesar de que incrementa los requerimientos de recursos computacionales,
tiene algunas ventajas que pudieran ser tiles:
Los puntos en que la cinemtica inversa no logre hallar una solucin, sta puede
ser interpolada con respecto al punto anterior y el punto siguiente; para que luego
de la interpolacin se corra nuevamente el algoritmo de cinemtica para ver si es
posible ajustar an ms. Esto tiene la ventaja de evitar que haya tales
discontinuidades en el movimiento.
Los instantes en que ciertas articulaciones deben dejar de moverse representa
tambin un problema que causa discontinuidades, y que podran disminuirse del
mismo modo a travs de interpolaciones entre el punto anterior y el siguiente.
Tales interpolaciones ofrecen por tanto la posibilidad de encontrar una velocidad
distinta de cero para todos los puntos.
125
Al conocer posiciones y velocidades distintas para cada punto, algn algoritmo
de filtrado podra ser implementado, para reducir al mnimo posible los saltos o
los frenados bruscos.
Despus de tales clculos, podra hacerse una segunda corrida del algoritmo de
cinemtica para cada punto con un menor margen de error, lo que podra permitir
aumentar muchsimo la precisin del movimiento y de los puntos alcanzados.
Otro punto muy delicado y que no lleg a implementarse, es la interpretacin de la

retroalimentacin. La interfaz de comunicacin lee posiciones y cargas de los servomotores,
sin embargo, la nica utilidad de sa informacin es la de presentarla al usuario para que l
decida qu hacer con ella. Sin embargo, si se implementaran rutinas de interpretacin de la
informacin, se podran prevenir sobre cargas, y enviar un mensaje de alerta cuando un
servomotor supere cierto lmite, tras lo cual se podra comenzar alguna accin de emergencia
como apagar los motores o notificar al maestro del error. As como tambin, aunque
improbable, el diseo y programacin de algn mtodo que permita al software recuperarse de
una desconexin y reconexin, mecanismo que hasta el momento no existe debido a que no se
han presentado tales casos de errores en la conexin durante el tiempo de ejecucin.
Como se puede ver, el solo manipulador del robot Donaxi tiene muchas reas de
oportunidad, por lo que es importante que se siga fomentando la participacin de estudiantes
en el proyecto para que no se pierda el trabajo logrado y que, por el contrario este se fortalezca
con un prototipo cada vez ms robusto.
126
ANEXOS.
ANEXO A. ALGORITMO DENAVIT-HARTENBERG.
[] para que la matriz [] relacione los sistemas y {S}, es necesario que los
sistemas se hayan escogido de acuerdo a unas determinadas normas. stas, junto con la
definicin de los 4 parmetros de Denavit Hartenberg, conforman el siguiente algoritmo para
la resolucin del problema cinemtico directo (Barrientos, Pen, Balaguer, & Aracil, 2007):
DH 1. Numerar los eslabones comenzando con 1 (primer eslabn mvil de la cadena) y
acabando con n (ltimo eslabn mvil). Se numerar como eslabn 0 a la base
fija del robot.
DH 2. Numerar cada articulacin comenzando por 1 (la correspondiente al primer grado
de libertad) y acabando en n.
DH 3. Localizar el eje de cada articulacin. Si sta es rotativa, el eje ser su propio eje
de giro. Si es prismtica, ser el eje a lo largo del cual se produce el
desplazamiento.
DH 4. Para i de 0 a n-1 situar el eje sobre el eje de la articulacin i+1.
DH 5. Situar el origen del sistema de la base en cualquier punto del eje . Los ejes
e se situarn de modo que formen un sistema dextrgiro con .
DH 6. Para i de 1 a n-1, situar el origen del sistema (solidario al eslabn i) en la
interseccin del eje con la lnea normal comn a y . Si ambos ejes se
cortasen se situara en el punto de corte. Si fuesen paralelos se situara
en la articulacin i+1.
DH 7. Situar en la lnea normal comn a y .
DH 8. Situar de modo que forme un sistema dextrgiro con y .
DH 9. Situar el sistema en el extremo del robot de modo que coincida con la
direccin de y sea normal a y .
DH 10. Obtener como el ngulo que hay que girar en torno a para que y
queden paralelos.
127
DH 11. Obtener como la distancia, medida a lo largo de , que habra que
desplazar para que y quedasen alineados.
DH 12. Obtener como la distancia medida a lo largo de (que ahora coincidira con
) que habra que desplazar el nuevo para que su origen coincidiese
con .
DH 13. Obtener como el ngulo que habra que girar entorno [sic] a , para que el
nuevo coincidiese totalmente con .
DH 14. Obtener las matrices de transformacin definidas en [4.10].
DH 15. Obtener la matriz de transformacin que relaciona el sistema de la base con el
del extremo del robot
DH 16. La matriz T define la orientacin (submatriz de rotacin) y posicin (submatriz
de traslacin) del extremo referido a la base, en funcin de las n coordenadas
articulares.
ANEXO B. CDIGO DE MATRIZ DE TRANSFORMACIN PARA

WXMAXIMA.
El siguiente cdigo representa las instrucciones de wxMaxima para calcular la matriz

de transformacin T. Se recomienda que se copie tal cual el siguiente cdigo en un archivo de
texto con el nombre que se desee y la extensin .wxm. Una vez hecho eso, desde
wxMaxima se puede abrir tal archivo como si hubiera sido creado en l mismo.
/* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/

/* [ Created with wxMaxima version 11.08.0 ] */
/* [wxMaxima: input start ] */

A01: matrix(
[cos(T1),0,sin(T1),0],
[sin(T1),0,-cos(T1),0],
[0,1,0,18.5],
[0,0,0,1]
);
/* [wxMaxima: input end ] */

128
A12: matrix(
[cos(T2),0,sin(T2),22.44*cos(T2)],
[sin(T2),0,-cos(T2),22.44*sin(T2)],
[0,1,0,0],
[0,0,0,1]
);

A23: matrix(
[cos(T3),-sin(T3),0,3.1*cos(T3)],
[sin(T3),cos(T3),0,3.1*sin(T3)],
[0,0,1,0],
[0,0,0,1]
);

A34: matrix(
[sqrt(2)/2,0,sqrt(2)/2,0],
[-sqrt(2)/2,0,sqrt(2)/2,0],
[0,-1,0,0],
[0,0,0,1]
);

A45: matrix(
[cos(T5),0,sin(T5),0],
[sin(T5),0,-cos(T5),0],
[0,1,0,15.67],
[0,0,0,1]
);

A56: matrix(
[cos(T6),0,-sin(T6),15*cos(T6)],
[sin(T6),0,cos(T6),15*sin(T6)],
[0,-1,0,0],
[0,0,0,1]
);

A67: matrix(
[1,0,0,0],
[0,1,0,0],
[0,0,1,0],
[0,0,0,1]
);

M1:A01.A12;
129
M2:M1.A23;

M3:M2.A34;

M4:M3.A45;

M5:M4.A56;

T:M5.A67;

elem11:T[1,1];

ratsimp(factor(elem11));

elem12:T[1,2];


elem13:T[1,3];


elem14:T[1,4];


elem21:T[2,1];
130

elem22:T[2,2];


elem23:T[2,3];


elem24:T[2,4];


elem31:T[3,1];


elem32:T[3,2];


elem33:T[3,3];


elem34:T[3,4];
131

elem41:T[4,1];

elem42:T[4,2];

elem43:T[4,3];

elem44:T[4,4];
/* Maxima can't load/batch files which end with a comment! */

"Created with wxMaxima"$
Por conveniencia, dentro del cdigo los smbolos , , , y fueron

reemplazados por T1, T2, T3, T5 y T6 respectivamente, y en cuanto a los smbolos y
fueron evaluados previamente, pues sus valores son constantes.
ANEXO C. CDIGOS GENERALES.
Anexo C- A
El siguiente cdigo representa el contenido del archivo manifiesto del paquete de
manipulacin.
<package>
<description brief="Manipulacion">
Manipulacion
</description>
<author>Victor Poisot Martinez</author>
<license>BSD</license>
<review status="developing" notes=""/>
<url>http://ros.org/wiki/brazo_donaxi</url>
<depend package="std_msgs"/>
<depend package="rospy"/>
132
<depend package="roscpp"/>
<depend package="kdl"/>
</package>
Anexo C- B
El archivo manipulacion.launch, que permite ejecutar los dos nodos correspondientes a
manipulacin de objetos con una sola lnea de comando, contiene lo siguiente:
<launch>
<node pkg="Manipulacion" name="NodoManipulacion"
type="nodoManipulacion" output="screen">
</node>
<node pkg="Manipulacion" name="interfaz" type="interfaz.py">
</node>
</launch>
ANEXO D. CDIGOS UTILIZADOS EN LA PROGRAMACIN DE

SECUENCIAS.
Anexo D- A
El siguiente cdigo es un ejemplo de programacin de una secuencia fija. En este caso
es una secuencia utilizada para realizar el movimiento de acercar el gripper a una persona,
quien le entrega al robot un objeto, el robot lo toma y lo acerca a su cuerpo para transportarlo:
case MANIPULACION_OPEN_TOMAOBJ:
brazo_obj.servo1=40;
brazo_obj.servo6=0;
brazo_actual=TorqueON(brazo_actual);
brazo_actual=FaseSecuencia(brazo_actual, brazo_obj, 5);
//hasta aqu sube el brazo y lo mantiene retraido
//se acerca el brazo a quien le va a dar la botella
133
//abre gripper
brazo_obj.servo6=20; //ajustar valor para la botella
ros::Duration(4.0).sleep();
//cierra gripper
//se retrae nuevamente
Break;
Se observa que este cdigo es una secuencia de seis etapas de duraciones distintas. Cada
etapa resulta ser muy sencilla de programar gracias al respaldo de las slidas libreras de
funciones que permiten simplificar la programacin de esta etapa a nicamente la asignacin
de ngulos de los servomotores de inters, activacin del torque y el llamado a la funcin
FaseSecuencia que es quien se encarga del resto.
Anexo D- B
El siguiente cdigo es un tanto extenso, pero muestra la creacin del modelo para
Gazebo, separando claramente los distintos eslabones (links) empezando por una base y
terminando en el gripper. Cada eslabn est formado por ms de una geometra y sus
respectivas propiedades fsicas:
<?xml version='1.0'?>
<sdf version="1.3">
<model name="brazo">
<link name="base">
<pose>0 0.0 0.25 0 0 0</pose>
<inertial>
<pose>0 0 -0.15 0 0 0</pose>
<inertia>
<ixx>4</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>4</iyy>
<iyz>0</iyz>
<izz>4</izz>
</inertia>
<mass>20.0</mass>
</inertial>
<collision name="collision-base">
<geometry>
<box>
<size>0.5 0.5 0.5</size>
134
</box>
</geometry>
</collision>
<visual name="visual-base">
<geometry>
<box>
<size>0.5 0.5 0.5</size>
</box>
</geometry>
<material>
<script>Gazebo/Purple</script>
</material>
</visual>
<collision name="collision-torso">
<pose>0 0.0 0.8 0 0 0</pose>
<geometry>
<box>
<size>0.03 0.03 1.1</size>
</box>
</geometry>
</collision>
<visual name="visual-torso">
<pose>0 0.0 0.8 0 0 0</pose>
<geometry>
<box>
<size>0.03 0.03 1.1</size>
</box>
</geometry>
<material>
</material>
</visual>
</link>
<link name="hombro">
<gravity>false</gravity>
<pose>0.0 0.0 1.5 1.570796327 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision-torso-hombro">
<geometry>
<cylinder>
<radius>.03</radius>
<length>.035</length>
</cylinder>
</geometry>
</collision>
135
<visual name="visual-torso-hombro">
<geometry>
<cylinder>
</cylinder>
</geometry>
<material>
<script>Gazebo/Red</script>
</material>
</visual>
<collision name="collision-hombro">
<pose>0 0 0.0925 0 0 0</pose>
<geometry>
<box>
<size>0.03 0.03 0.185</size>
</box>
</geometry>
</collision>
<visual name="visual-hombro">
<pose>0 0 0.0925 0 0 0</pose>
<geometry>
<box>
<size>0.03 0.03 0.185</size>
</box>
</geometry>
<material>
</material>
</visual>
</link>
<link name="brazo">
<pose>0.0 -0.185 1.5 3.141592654 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision-hombro-brazo">
<geometry>
<cylinder>
</cylinder>
</geometry>
</collision>
<visual name="visual-hombro-brazo">
<geometry>
136
<cylinder>
</cylinder>
</geometry>
<material>
</material>
</visual>
<collision name="collision-brazo">
<pose>0.11 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.22 0.03 0.03</size>
</box>
</geometry>
</collision>
<visual name="visual-brazo">
<pose>0.11 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.22 0.03 0.03</size>
</box>
</geometry>
<material>
</material>
</visual>
</link>
<link name="codo">
<pose>0.22 -0.185 1.5 -1.570796327 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision-brazo-codo">
<geometry>
<cylinder>
</cylinder>
</geometry>
</collision>
<visual name="visual-brazo-codo">
<geometry>
<cylinder>
137
</cylinder>
</geometry>
<material>
</material>
</visual>
<collision name="collision-codo">
<pose>0.035 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.07 0.03 0.03</size>
</box>
</geometry>
</collision>
<visual name="visual-codo">
<pose>0.035 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.07 0.03 0.03</size>
</box>
</geometry>
<material>
</material>
</visual>
</link>
<link name="codo-antebrazo">
<pose>0.29 -0.185 1.5 3.141592654 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision">
<geometry>
<cylinder>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
</cylinder>
138
</geometry>
<material>
</material>
</visual>
</link>
<link name="antebrazo">
<pose>0.29 -0.185 1.42 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.03 0.03 0.16</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.03 0.03 0.16</size>
</box>
</geometry>
<material>
</material>
</visual>
</link>
<link name="muneca">
<pose>0.29 -0.185 1.34 -1.570796327 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision-antebrazo-muneca">
<geometry>
139
<cylinder>
</cylinder>
</geometry>
</collision>
<visual name="visual-antebrazo-muneca">
<geometry>
<cylinder>
</cylinder>
</geometry>
<material>
</material>
</visual>
<collision name="collision-muneca">
<pose>0.045 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.09 0.03 0.03</size>
</box>
</geometry>
</collision>
<visual name="visual-muneca">
<pose>0.045 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.09 0.03 0.03</size>
</box>
</geometry>
<material>
</material>
</visual>
</link>
<link name="gripper">
<pose>0.38 -0.185 1.34 3.141592654 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
<mass>0.05</mass>
</inertial>
<collision name="collision-gripper">
<geometry>
<cylinder>
140
</cylinder>
</geometry>
</collision>
<visual name="visual-gripper">
<geometry>
<cylinder>
</cylinder>
</geometry>
<material>
</material>
</visual>
<collision name="collision-1-1">
<pose>0.03 -0.017320581 0 0 0 -0.5235957756</pose>
<geometry>
<box>
<size>0.0692820323 0.015 0.015</size>
</box>
</geometry>
</collision>
<visual name="visual-1-1">
<pose>0.03 -0.017320581 0 0 0 -0.5235957756</pose>
<geometry>
<box>
<size>0.0692820323 0.015 0.015</size>
</box>
</geometry>
<material>
</material>
</visual>
<pose>0.09 -0.017320581 0 0 0 0.5235957756</pose>
<geometry>
<box>
<size>0.0692820323 0.015 0.015</size>
</box>
</geometry>
</collision>
<pose>0.09 -0.017320581 0 0 0 0.5235957756</pose>
<geometry>
<box>
<size>0.0692820323 0.015 0.015</size>
</box>
</geometry>
<material>
</material>
</visual>
</link>
<link name="gripper2-1">
141
<pose>0.41 -0.202320581 1.34 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
<mass>0.04</mass>
</inertial>
<pose>0 0 0 0 0 -0.5235957756</pose>
<geometry>
<box>
<size>0.0692820323 0.015 0.015</size>
</box>
</geometry>
</collision>
<pose>0 0 0 0 0 -0.5235957756</pose>
<geometry>
<box>
<size>0.0692820323 0.015 0.015</size>
</box>
</geometry>
<material>
</material>
</visual>
<pose>0.06 0 0 0 0 0.5235957756</pose>
<geometry>
<box>
<size>0.0692820323 0.015 0.015</size>
</box>
</geometry>
</collision>
<pose>0.06 0 0 0 0 0.5235957756</pose>
<geometry>
<box>
<size>0.0692820323 0.015 0.015</size>
</box>
</geometry>
<material>
</material>
</visual>
</link>
<static>false</static>
<joint name="joint1" type="revolute">
<pose>0 0 0 1.570796327 0 0</pose>
<child>hombro</child>
142
<parent>base</parent>
<axis>
<limit>
<lower>0.0</lower>
<upper>6.243185307</upper>
</limit>
<xyz>0 -1 0</xyz>
</axis>
</joint>
<pose>0 0 0 1.570796327 0 0</pose>
<child>brazo</child>
<parent>hombro</parent>
<axis>
<limit>
<lower>0.0</lower>
<upper>6.243185307</upper>
</limit>
<xyz>0 0 -1</xyz>
</axis>
</joint>
<pose>0 0 0 0 3.141592654 0</pose>
<child>codo</child>
<parent>brazo</parent>
<axis>
<limit>
<lower>0.0</lower>
<upper>6.243185307</upper>
</limit>
<xyz>0 1 0</xyz>
</axis>
</joint>
<joint name="joint3-2" type="revolute">
<pose>0 0 0 -1.570796327 0 0</pose>
<child>codo-antebrazo</child>
<parent>codo</parent>
<axis>
<limit>
<lower>0.0</lower>
<upper>6.243185307</upper>
</limit>
<xyz>0 -1 0</xyz>
</axis>
</joint>
<pose>0 0 0 1.570796327 0 0</pose>
<child>antebrazo</child>
<parent>codo-antebrazo</parent>
<axis>
<limit>
<lower>0.0</lower>
<upper>6.243185307</upper>
</limit>
<xyz>0 0 -1</xyz>
</axis>
143
</joint>
<pose>0 0 0 -1.570796327 0 0</pose>
<child>muneca</child>
<parent>antebrazo</parent>
<axis>
<limit>
<lower>0.0</lower>
<upper>6.243185307</upper>
</limit>
<xyz>0 1 0</xyz>
</axis>
</joint>
<child>gripper</child>
<parent>muneca</parent>
<axis>
<limit>
<lower>0.0</lower>
<upper>6.243185307</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="joint6-2" type="revolute">
<pose>-0.03 0.01732050808 0 0 0 0</pose>
<child>gripper2-1</child>
<parent>muneca</parent>
<axis>
<limit>
<lower>0.0</lower>
<upper>6.243185307</upper>
</limit>
<xyz>0 0 -1</xyz>
</axis>
</joint>
<plugin name="ControlGazebo" filename="libControlGazebo.so"/>
</model>
</sdf>
Este modelo simplemente fue insertado tal cual dentro de un mundo de Gazebo. No es
necesaria ninguna compilacin o similar previa, Gazebo lo dibuja al momento.
144
Anexo D- C
Cdigo del plugin de Gazebo para su comunicacin con ROS. El siguiente cdigo fuente
contiene las instrucciones adecuadas que permiten a Gazebo leer el tpico de mensajes de
ROS; una vez ledos los mensajes los decodifica y ajusta las posiciones de las articulaciones
de modo que coincidan con las indicadas.
//Estos primeros encabezados son requerimientos de Gazebo para sus
plugins
#include <boost/bind.hpp>
#include <gazebo.hh>
#include <physics/physics.hh>
#include <common/common.hh>
#include <stdio.h>
//estos siguientes son los requerimientos de ROS para los mensajes
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "mensajes_gazebo.h" //hecho por el usuario, contiene estructuras
para mensajes y sus codificaciones
#include "std_msgs/String.h"
namespace gazebo
{
//clase que hereda de ModelPlugin porque pretendemos manipular un
modelo con este plugin
class ROSModelPlugin : public ModelPlugin
{
public: ROSModelPlugin()
{
// inicializacin de ROS y nombre para el nodo
std::string name = "brazo_donaxi_plugin";
int argc = 0;
ros::init(argc, NULL, name);
}
public: ~ROSModelPlugin()
{
delete this->node;
}
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr
/*_sdf*/)
{
// indicacin del apuntador al modelo
this->model = _parent;
// ROS NodeHandle
this->node = new ros::NodeHandle("~");
// ROS Subscriber para el tpico "ArticulacionesGazebo"
// y llamada a la funcion cuando se dispare.
this->sub = this->node-
>subscribe<std_msgs::String>("ArticulacionesGazebo", 1,
&ROSModelPlugin::ROSCallback, this );
//Suscripcin al evento de actualizacin del "mundo" en
Gazebo, esto es lo que define el ritmo de iteraciones
this->updateConnection =
event::Events::ConnectWorldUpdateBegin(
boost::bind(&ROSModelPlugin::OnUpdate, this, _1));
145
//inicializacion de la estructura de escucha
escuchado=InitBrazo(escuchado);
//apuntadores a cada una de las articulaciones
joint_1 = model->GetJoint("joint1");
joint_3_2 = model->GetJoint("joint3-2");
joint_6_2 = model->GetJoint("joint6-2");
}
//esta funcin se llama cuando hay una actualizacin del mundo
public: void OnUpdate(const common::UpdateInfo & /*_info*/)
{
ros::spinOnce();
//declaramos una clase para controlar el model llamada
"controlin"
physics::JointController controlin(model);
//llamamos al mtodo miembro de controlin llamada
SetJointPosition,
//el cual pondr en el modelo la posicin que pidamos
controlin.SetJointPosition(joint_1,escuchado.servo1);
controlin.SetJointPosition(joint_3_2,0.785398163);
controlin.SetJointPosition(joint_6_2,escuchado.servo6);
}
//interrupcin de mensaje recibido
void ROSCallback(const std_msgs::String::ConstPtr& msgRecibido)
{
//guardamos el mensaje en "escuchado"
escuchado=decEstructuraBrazo(msgRecibido->data);
ROS_INFO("leido: s1: %f s2: %f s3: %f s4: %f s5: %f
s6: %f", escuchado.servo1, escuchado.servo2, escuchado.servo3,
escuchado.servo4, escuchado.servo5, escuchado.servo6);
escuchado.servo1=(escuchado.servo1*3.141592654)/180;
}
// declaracin de un apuntador al modelo
private: physics::ModelPtr model;
// declaracin de apuntador al evento de actualizacin del mundo
private: event::ConnectionPtr updateConnection;
// ROS Nodehandle
private: ros::NodeHandle* node;
// ROS Subscriber
ros::Subscriber sub;
//Declaracin de la estructura donde guardaremos el mensaje
private: EstructuraBrazoDonaxi escuchado;
146
//declaramos apuntadores a cada una de las articulaciones
physics::JointPtr joint_1;
physics::JointPtr joint_3_2;
physics::JointPtr joint_6_2;
};
// Registro del plugin en el simulador
GZ_REGISTER_MODEL_PLUGIN(ROSModelPlugin)
}
Como se observa se utilizan apuntadores a las respectivas articulaciones para poder
manipularlas asignndoles los valores ledos del mensaje de ROS.
ANEXO E. CDIGOS DE CINEMTICA.
Anexo E- A
Cdigo de funcin apply_case, que ejecuta los cambios correspondientes a las
articulaciones dependiendo el caso que se desee aplicar, considerando los lmites de los
servomotores y las dos constantes distintas para incrementos y decrementos.
KDL::JntArray apply_case(KDL::JntArray jointconfig, int case_index)
{
switch (case_index)
{
case 1:
jointconfig(0)=jointconfig(0)+MIN_CHANGE;
if( radtodeg(jointconfig(0)) > 156.0 )
{
jointconfig(0)=jointconfig(0)-MIN_CHANGE;
}
break;
case 2:
jointconfig(0)=jointconfig(0)-MAX_CHANGE;
if( radtodeg(jointconfig(0)) < 6.0 )
{
jointconfig(0)=jointconfig(0)+MAX_CHANGE;
}
break;
case 3:
{
}
147
break;
case 4:
{
}
break;
case 5:
{
jointconfig(2)=jointconfig(2)-(2*M_PI);
}
if( (radtodeg(jointconfig(2)) > 116.0) &&
(radtodeg(jointconfig(2)) < 265.0) )
{
}
break;
case 6:
{
jointconfig(2)=(2*M_PI)+jointconfig(2);
}
if( (radtodeg(jointconfig(2)) < 270.0) &&
(radtodeg(jointconfig(2)) > 120.0) )
{
}
break;
case 7:
{
}
{
}
break;
case 8:
{
jointconfig(3)=jointconfig(3)+(2*M_PI);
}
{
}
break;
148
case 9:
{
}
break;
case 10:
{
}
break;
case 11:
{
}
{
}
{
}
break;
case 12:
{
}
{
}
{
}
break;
case 13:
{
}
149
{
}
{
}
break;
case 14:
{
}
{
}
{
}
break;
case 15:
{
}
{
}
{
}
break;
case 16:
{
}
{
}
150
{
}
break;
case 17:
{
}
{
}
{
}
break;
case 18:
{
}
{
}
{
}
break;
case 19:
{
}
{
}
{
}
break;
case 20:
151
{
}
{
}
{
}
break;
case 21:
{
}
{
}
{
}
break;
case 22:
{
}
{
}
{
}
break;
default:
printf("Unknown case...!!\n");
break;
}
return jointconfig;
}
152
Anexo E- B
Cdigo de algoritmo de trazado de Rutas:
case MANIPULACION_PRUEBAS_CINEMATICA:
1. printf("Pruebas con cinemtica para determinar posiciones!!! \n");
2. //Primer paso poner el brazo en una posicin conocida usando la funcin
"FaseSecuencia"
3. brazo_obj.servo1=60;
8. brazo_actual=TorqueON(brazo_actual);
9. brazo_actual=FaseSecuencia(brazo_actual, brazo_obj, 5);
10. //definimos el punto actual
11. frpresent.p.x(0.0);
12. frpresent.p.y(-37.0);
13. frpresent.p.z(18.5);
14. frpresent.M=Rotation::RPY(degtorad(-90.0), degtorad(0.0),
degtorad(0.0));
15. //definimos frgoal
16. frgoal.p.x(menMaestro.x);
17. frgoal.p.y(menMaestro.y);
18. frgoal.p.z(menMaestro.z);
19. frgoal.M=Rotation::RPY(degtorad(-90.0),degtorad(0.0),degtorad(0.0));
20. //definiremos uno a uno slo los pasos que queremos hacer
21. frstep=frpresent;
22. frstep.p.y(frgoal.p.y());
23. //calculamos el vector director
24. distance = frstep.p - frpresent.p;
25. magnitude=evalMagnitude(distance);
26. director.x(PASO_RUTA*(distance.x()/magnitude));
27. director.y(PASO_RUTA*(distance.y()/magnitude));
28. director.z(PASO_RUTA*(distance.z()/magnitude));
29. //inicia ciclo que tabular los puntos y mandar las instruccines a los
servos
30. while(magnitude>=PASO_RUTA)
31. {
31.1. //tabulamos nuevo punto
31.2. frpresent.p = frpresent.p + director;
31.3. brazo_actual=KinematicsSolverDonaxi(brazo_actual, frpresent,
FREQUENCY);
31.4. art_actual=CopyInstrucciones(brazo_actual, art_actual);
31.5. ros::spinOnce();
31.6. if(flag==true)
31.7. {
31.7.1. if(menMaestro.actividad==MANIPULACION_EMERGENCIA)
31.7.2. {
31.7.2.1. brazo_actual=TorqueOFF(brazo_actual);
31.7.3. }
31.8. }
31.9. //Manda datos a interfaz.py
31.10. msg2.data=codInstruccionesInterfaz(brazo_actual);
31.11. printf("Enviando dato\n");
31.12. chatter_pub2.publish(msg2);
153
31.13. //Manda datos a Gazebo
31.14. msg1.data=codEstructuraBrazo(art_actual);
31.16. //volvemos a evaluar la magnitud faltante
31.17. distance=frstep.p - frpresent.p;
31.18. magnitude=evalMagnitude(distance);
31.19. //mensaje vaco para el Maestro y
31.20. chat.publish(msg_vacio);
31.22. ros::Duration(1.0/FREQUENCY).sleep();
32. }
33. //abrimos el gripper
34. brazo_obj=brazo_actual;
38. //definiremos el siguiente paso que queremos hacer
40. frstep.p.x(frgoal.p.x());
41. frstep.p.z(frgoal.p.z());
servos
50. {
FREQUENCY);
50.7. {
50.7.2. {
50.7.3. }
50.8. }
154
51. }
52. //cierra gripper
53. brazo_obj=brazo_actual;
59. frstep.p.y(frgoal.p.y()+4);
servos
68. {
FREQUENCY);
68.7. {
68.7.2. {
68.7.3. }
68.8. }
69. }
72. frstep.p.x(0.0);
73. frstep.p.z(18.5);
155
servos
82. {
FREQUENCY);
82.7. {
82.7.2. {
82.7.3. }
82.8. }
83. }
84. break;
ANEXO F. CDIGOS DE INTERFAZ DE COMUNICACIN.
Anexo F- A
Cdigo de firmware de la tarjeta CM-700.
//cdigo de control de brazo derecho. Versin 2.0
//configuracin de servos 11, 14, 15, 16, 17, opciones para leer encoder
// de posicin, error, temperatura, carga, habilitar/deshabilitar el
// torque, corriente y moverse a una posicin "inicial".
// recibe goal_speed y goal_position.
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <util/delay.h>
156
#include "dynamixel.h"
#include "serial.h"
/// Control table address
#define TORQUE_ENABLE 24
#define CW_COMPLIANCE_MARGIN 26
#define CCW_COMPLIANCE_MARGIN 27
#define P_GOAL_POSITION_L 30
#define P_GOAL_POSITION_H 31
#define P_GOAL_SPEED_L 32
#define P_GOAL_SPEED_H 33
#define P_TORQUE_LIMIT_L 34
#define P_TORQUE_LIMIT_H 35
#define P_PRESENT_POSITION_L 36
#define P_PRESENT_POSITION_H 37
#define P_MOVING 46
#define PRESENT_LOAD_L 40
#define PRESENT_LOAD_H 41
// Defulat setting
#define DEFAULT_BAUDNUM 1 // 1Mbps
//funciones
int caL(int valor)
{
return valor%256;
}
int caH(int valor)
{
return valor/256;
}
int fil(int t)
{
if(t<=1023)
{
return t/10;
}
else
{
return ((t-1024)/10);
}
}
int main(void)
{
//posiciones leidas
int pos11=1001;
int pos12=1002;
int pos13=1003;
int pos14=1004;
int pos15=1005;
int pos16=1006;
int pos17=1007;
//cargas leidas
int load11=1001;
int load12=1002;
int load13=1003;//10
int load14=1004;
int load15=1005;
int load16=1006;
157
int load17=1007;
//estados recibidos
int state11=0;
int state12=0;
int state13=0;
int state14=0;
int state15=0;
int state16=0;
int state17=0;
//posiciones objetivo l y h
int oposl11=0;
int oposh11=0;//30
int oposl12=0;
int oposh12=0;
int oposl13=0;
int oposh13=0;
int oposl14=0;
int oposh14=0;
int oposl15=0;
int oposh15=0;
int oposl16=0;
int oposh16=0;//40
int oposl17=0;
int oposh17=0;
//velocidades objetivo l y h
int vel11=0;
int vel12=0;
int vel13=0;
int vel14=0;
int vel15=0;
int vel16=0;
int vel17=0;
//posiciones convertidas para enviar
int posl11=0;
int posh11=0;
int posl12=0;
int posh12=0;//60
int posl13=0;
int posh13=0;
int posl14=0;
int posh14=0;
int posl15=0;
int posh15=0;
int posl16=0;
int posh16=0;
int posl17=0;
int posh17=0;//70
//cargas convertidas para enviar
serial_initialize(57600);
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
sei(); // Interrupt Enable
//inicializacion de servos!!
dxl_write_word(11, TORQUE_ENABLE, 0); //11
//dxl_write_word(26, TORQUE_ENABLE, 0); //12 servo faltante
//AQU IRA LA CONFIG DEL COMPLIANCE SI SE CONSIGUE EL SERVO FALTANTE
158
while(1)
{
//lectura de posiciones de todos los servos
pos11 = dxl_read_word( 11, P_PRESENT_POSITION_L ); //11
//pos12 = dxl_read_word( 12, P_PRESENT_POSITION_L ); //12 servo
faltante
//conversiones de todos los servos
posl11=caL(pos11);
posh11=caH(pos11);
posl12=caL(pos12);
posh12=caH(pos12);
posl13=caL(pos13);
posh13=caH(pos13);
posl14=caL(pos14);
posh14=caH(pos14);
posl15=caL(pos15);
posh15=caH(pos15);
posl16=caL(pos16);
posh16=caH(pos16);
posl17=caL(pos17);
posh17=caH(pos17);
//lectura de carga de todos los servos
load11 = dxl_read_word( 11, PRESENT_LOAD_L ); //11
//load12 = dxl_read_word( 12, PRESENT_LOAD_L ); //12 SERVO
FALTANTE
//convetrsiones de todos los servos
load11=fil(load11);
load12=fil(load12);
load13=fil(load13);
load14=fil(load14);
load15=fil(load15);
load16=fil(load16);
load17=fil(load17);
//recepcion de datos para servo 11
state11=getchar();
oposl11=getchar();
oposh11=getchar();
vel11=getchar();
state12=getchar();
oposl12=getchar();
oposh12=getchar();
159
vel12=getchar();
state13=getchar();
oposl13=getchar();
oposh13=getchar();
vel13=getchar();
state14=getchar();
oposl14=getchar();
oposh14=getchar();
vel14=getchar();
state15=getchar();
oposl15=getchar();
oposh15=getchar();
vel15=getchar();
state16=getchar();
oposl16=getchar();
oposh16=getchar();
vel16=getchar();
state17=getchar();
oposl17=getchar();
oposh17=getchar();
vel17=getchar();
//Escritura de datos para servo 11
printf("%c%c%c",posl11,posh11,load11);
//Enviar velocidades a todos los servos
//dxl_write_word(26, P_GOAL_SPEED_L, (vel11*4)); //11
//dxl_write_word(26, P_GOAL_SPEED_L, (vel13*4)); //13 servo
faltante
//Enviar posiciones o desabilitar lo que corresponda a cada
servo
//servo 11
if (state11=='T')
{
dxl_write_word(11, P_GOAL_SPEED_L, (vel11*4)); //11
oposl11=oposl11+((oposh11-50)*256);
dxl_write_word(11, P_GOAL_POSITION_L, oposl11);
160
}
else
{
dxl_write_word(11, TORQUE_ENABLE, 0);
}
//servo 12
/*if (state12=='T')
{
oposl12=oposl12+(oposh12*256);
}
else
{
}*/
//servo 13
if (state13=='T')
{
oposl13=oposl13+((oposh13-50)*256);
}
else
{
}
//servo 14
if (state14=='T')
{
}
else
{
}
//servo 15
if (state15=='T')
{
}
else
{
}
//servo 16
if (state16=='T')
{
}
161
else
{
}
//servo 17
if (state17=='T')
{
}
else
{
}
}
return 0;
}
Se puede observar que hay algunas lneas con instrucciones convertidas en comentario
que hacen referencia a un servomotor 12. Esto se debe a que en un diseo original del sistema
se incluan siete servomotores, es decir, una de las articulaciones del hombro estaba formada
por dos EX-106+ acoplados para sumar sus torques y obtener una mayor fuerza, sin embargo,
uno de los motores experiment ciertos problemas y por cuestiones de recursos del proyecto
no pudo ser reemplazado. Se hicieron pruebas y se determin que un motor bastara para un
correcto funcionamiento del manipulador aunque el peso mximo que pudiera soportar se
viera ligeramente disminuido.
162
BIBLIOGRAFA
About ROS. (s.f.). Recuperado el 26 de Agosto de 2014, de www.ros.org/about-ros/
Aleny, G., & Tellez, R. (2013). The Reem@IRI 2013 RoboCup@Home Team Description.
Institut de Robtica i Informtica Industrial, CSIC-UPC, http://robocup.pal-
robotics.com/.
Applications. (s.f.). Recuperado el 05 de Septiembre de 2014, de The Orocos Project:
http://orocos.org/orocos/applications
Barinka, L., & Berka, R. (s.f.). Inverse Kinematics - Basic Methods. Dept. of Computer
Science & Engineering, Czech Technical University.
Barrientos, A., Pen, L. F., Balaguer, C., & Aracil, R. (2007). Fundamentos de robtica
(Segunda ed.). Madrid, Espaa: S.A. McGraw-Hill/Interamericana de Espaa.
Basic Programming. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-
Manual v1.23.00:
http://support.robotis.com/en/software/embeded_c/cm510_cm700/programming/basic_
programming.htm
Bharatheesha, M., Rudinac, M., Chandarr, A., Gaisser, F., Bruinink, M., Pons Rueda, S., . . .
Jonker, P. (2012). Delft Robotics RoboCup@Home 2012 Team Description Paper.
Delft University of Technology, Department of Bio-Mechanical Engineering,
http://www.delftrobotics.nl.
Bodnar, J. (25 de Mayo de 2014). wxPython tutorial. Recuperado el 09 de Septiembre de
2014, de ZetCode.com: http://zetcode.com/wxpython/
Boot Loader. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00:
http://support.robotis.com/en/software/embeded_c/cm510_cm700/programming/bootlo
ader.htm
Chen, X., Wang, F., Sun, H., Xie, J., Cheng, M., & Chen, K. (2013). KeJia: The Integrated
Intelligent Robot for RoboCup@Home 2013. Multi-Agent Systems Lab., Department
of Computer Science and Technology, University of Science and Technology of China,
http://wrighteagle.org/en/robocup/atHome.
163
CM510/CM700. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00: http://support.robotis.com/en/software/embeded_c/cm510_cm700.htm
CM-700. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual v1.23.00:
http://support.robotis.com/en/product/controller/cm700_manual.htm
Correa, M., Bernuy, F., Herrmann, D., Olave, G., Pavez, M., Tampier, C., . . . Ruiz-del-Solar,
J. (2013). UChile HomeBreakers 2013 Team Description Paper. Department of
Electrical Engineering - Advanced Mining Technology Center, Universidad de Chile,
http://www.robocup.cl/athome.htm.
Dwiputra, R., Fller, M., Hegger, F., Hochgeschwender, N., Paulus, J., Schneider, S., . . .
Kraetzschmar, G. K. (2013). The b-it-bots RoboCup@Home 2013 Team Description
Paper. Bonn-Rhein-Sieg University of Applied Sciences, Department od Computer
Science, http://www.b-it-bots.de.
Dynamixel. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00: http://support.robotis.com/en/product/dxl_main.htm
Embedded C. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00: http://support.robotis.com/en/software/embeded_c_main.htm
EX-106+. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual v1.23.00:
http://support.robotis.com/en/product/dynamixel/ex_series/ex-106.htm
Example. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual v1.23.00:
http://support.robotis.com/en/software/embeded_c/cm510_cm700/example.htm
EX-Series. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00: http://support.robotis.com/en/product/dynamixel/dxl_ex_main.htm
Fu, K. S., Gonzlez, R. C., & Lee, C. S. (1987). Robotics: Control, Sensing, Vision and
Intelligence (primera ed.). Madrid, Espaa: McGraw-Hill.
Gazebo - Package Summary. (28 de Junio de 2013). Recuperado el 05 de Septiembre de 2014,
de http://wiki.ros.org/gazebo?distro=fuerte
Gazebo - Tutorials. (s.f.). Recuperado el 05 de Septiembre de 2014, de
http://gazebosim.org/tutorials
Gazebo. (2014). Recuperado el 09 de Septiembre de 2014, de http://gazebosim.org/
gazebo::physics::JointController Class Reference. (s.f.). Recuperado el 05 de Septiembre de
2014, de Classes for physics and dynamics: http://osrf-
164
distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1physics_1_1JointCo
ntroller.html
Geometric Primitives. (s.f.). Recuperado el 05 de Septiembre de 2014, de Orocos Kinematics
and Dynamics: http://orocos.org/kdl/usermanual/geometric-primitives
Gonzlez Duque, R. (s.f.). Python para todos. Espaa: http://mundogeek.net/tutorial-python/.
Gonzlez, V. R. (2002-03). Estructura de un robot industrial. Recuperado el 16 de Noviembre
de 2013, de
http://platea.pntic.mec.es/vgonzale/cyr_0204/ctrl_rob/robotica/sistema/morfologia.htm
Grossman, S. I. (1992). lgebra Lineal (Cuarta (tercera edicin en espaol) ed.). (C. M.
Snchez Trujillo, Trad.) Naucalpan de Jurez, Mxico: MGraw-Hill.
Hubert, U., Stckler, J., & Behnke, S. (2012). Bayesian Calibration of the Hand-Eye
Kinematics of an Anthropomorphic Robot. In Proceedings of the IEEE-RAS
International Conference on Humanoid Robots, Autonomous Intelligent Systems
Group, Computer Science Institute VI, University of Bonn.
Installing Atmel Studio. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-
Manual v1.23.00:
http://support.robotis.com/en/software/embeded_c/cm510_cm700/quickstart/atmel_stu
dio_install.htm
Installing WinAVR. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00:
http://support.robotis.com/en/software/embeded_c/cm510_cm700/quickstart/winavr_in
stall.htm
Is ROS For Me? (s.f.). Recuperado el 26 de Agosto de 2014, de www.ros.org/is-ros-for-me/
Jazar, R. N. (2006). Theory of Applied Robotics: Kynematics, Dynamics and Control (Segunda
ed.). New York, USA: Springer.
Kinematic and Dynamic Solvers. (s.f.). Recuperado el 05 de Septiembre de 2014, de Orocos
Kinematics and Dynamics: http://orocos.org/kdl/UserManual/kinematic_solvers
Kinematic Trees - KDL 1.1.x. (s.f.). Recuperado el 05 de Septiembre de 2014, de Orocos Wiki:
http://orocos.org/wiki/main-page/kdl-wiki/user-manual/kinematic-chains/kinematic-
chains-kdl-11x
165
Lewis, F. L., Dawson, D. M., & Abdallah, C. T. (2004). Robot Manipulator Control. Theory
and Practice (segunda ed.). New York, USA: Marcel-Dekker.
Llarena, A., Boldt, J. F., Steinke, N. S., Engelmeyer, H., & Rojas, R. (2013).
BerlinUnited@Home 2013 Team Description Paper. Department of Mathematics and
Computer Science, Freie Universitt Berlin, http://athome.berlinunited.org.
Lunenburg, J. J., Coenen, S. A., van den Dries, S., Elfring, J., Janssen, R. J., Sandee, J. H., &
van de Molengraft, M. J. (2013). Tech United Eindhoven Team Description 2013.
Eindhoven University of Technology,, http://www.techunited.nl, techunited@tue.nl.
Mahmoudi, F., Fathzadeh, R., Hosseini, A., Namazifar, M. J., Nabavi, N., Abdollahi, F., . . .
Bagheri, H. (2013). MRL @Home 2013 Team Description Paper. Mechatronics
Research Laboratory, Qazvin Islamic Azad University, http://www.mrl.ir.
Make a Model. (s.f.). Recuperado el 05 de Septiembre de 2014, de
http://gazebosim.org/tutorials?tut=build_model&cat=build_robot
Make a Simple Gripper. (2014). Recuperado el 05 de Septiembre de 2014, de
http://gazebosim.org/tutorials?tut=simple_gripper&cat=build_robot
Maneewarn, T., Sooktip, T., Thungod, K., Suparat, N., & Aurmyou, J. (2013). Team TRCC
RoboCup 2013 RoboCup@Home League Team Description Paper. Thonburi Robot
Contest Club, King Mongkuts University of Technology Thonburi (KMUTT),
http://trccrobotathome.blogspot.com/.
Murphy, R. R. (2000). Introduction to AI Robotics (Primera ed.). Cambridge, Massachusetts,
USA: The MIT Press.
Navigating the ROS Filesystem. (19 de Diciembre de 2013). Recuperado el 31 de Agosto de
2014, de http://wiki.ros.org/ROS/Tutorials/NavigatingTheFilesystem
Nieuwenhuisen, M., Stckler, J., Berner, A., Klein, R., & Behnke, S. (2012). Shape-Primitive
Based Object Recognition and Grasping. Computer Science Institute, University of
Bonn, Germany, 7th German Conference on Robotics (ROBOTIK).
Orocos Kinematics and Dynamics. (s.f.). Recuperado el 05 de Septiembre de 2014, de
http://orocos.org/kdl
Orocos Licenses. (s.f.). Recuperado el 05 de Septiembre de 2014, de
http://orocos.org/content/orocos-licenses
166
Pineda, L. A. (2013). The Golem Team, RoboCup@Home 2013. Computer Sciences
Department, Instituto de Investigaciones en Matemticas Aplicadas y en Sistemas,
Universidad Nacional Autnoma de Mxico, http://golem.iimas.unam.mx.
Popirlan, C., & Dupac, M. (2009). An Optimal Path Algorithm for Autonomous Searching
Robots. Annals of University of Craiova, Math. Comp. Sci. Ser, 36(1), 37-48.
Qizhi, Z., Yali, Z., Xinxin, X., Ye, L., & Jun, Z. (2013). Sun - RoboCup@Home 2013 Team
Description Paper. School of Automation, Beijing Information Science & Technology
University, http://zt.bistu.edu.cn/RoboCupAtHome/.
Restoring RoboPlus. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-
Manual v1.23.00:
http://support.robotis.com/en/software/embeded_c/cm510_cm700/programming/firmw
are_recovery.htm
RoboPlus Manager. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00:
http://support.robotis.com/en/software/roboplus/roboplus_manager_main.htm
RoboPlus Motion. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00: http://support.robotis.com/en/software/roboplus/roboplus_motion_main.htm
RoboPlus Task. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00: http://support.robotis.com/en/software/roboplus/roboplus_task_main.htm
RoboPlus Terminal. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00:
http://support.robotis.com/en/software/roboplus/roboplus_terminal_main.htm
ROS - Core Components. (s.f.). Recuperado el 26 de Agosto de 2014, de www.ros.org/core-
components/
ROS - Integration with Other Libraries. (s.f.). Recuperado el 26 de Agsosto de 2014, de
http://www.ros.org/integration/
ROS Contributors. (14 de Julio de 2014). Recuperado el 26 de Agosto de 2014, de
www.ros.org/contributors/
RX-28. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual v1.23.00:
http://support.robotis.com/en/product/dynamixel/rx_series/rx-28.htm
167
RX-64. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual v1.23.00:
http://support.robotis.com/en/product/dynamixel/rx_series/rx-64.htm
RX-Series. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-Manual
v1.23.00: http://support.robotis.com/en/product/dynamixel/dxl_rx_main.htm
Savage, J., Matamoros, M., Negrete, M., Figueroa, I., Cruz, J., Contreras, L., . . . Mrquez, J.
(2013). Pumas@Home 2013 Team Description Paper. Bio-Robotics Laboratory,
UNAM, http://biorobotics.fi-p.unam.mx.
Seib, V., Kathe, F., McStay, D., Manthe, S., Peters, A., Benedikt, J., . . . Paulus, D. (08 de 02
de 2013). RoboCup 2013 - homer@UniKoblenz (Germany). Active Vision Group,
University of Koblenz and Landau, http://robots.uni-koblenz.de/.
Setting Environment. (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS e-
Manual v1.23.00:
http://support.robotis.com/en/software/embeded_c/cm510_cm700/quickstart/etc_prepa
ration.htm
Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning
and Control (primera ed.). (M. J. Grimble, & M. A. Johnson, Edits.) Roma-Npoles,
Italia: Springer-Verlag.
Smithmaitrie, P., Hiransoog, C., Wichakool, W., Sangsaikeaw, S., Chanchaichujit, P.,
Rajawana, A., . . . Saekow, P. (2013). Team Description Paper: Team Dong Yang in
2013 RoboCup @Home League. Department of Mechanical Engineering, Faculty of
Engineering, Prince of Songkla University, www.me.psu.ac.th/robocup.
Stckler, J., & Behnke, S. (2012). Benchmarking Mobile Manipulation in Everyday
Environments. Autonomous Intelligent Systems, University of Bonn, In Proc. of the
IEEE Workshop on Advanced Robotics and its Social Impacts.
Stckler, J., Droeschel, D., Grve, K., Holz, D., Schreiber, M., & Behnke, S. (2013).
NimbRo@Home 2013 Team Description. Rheinische Friedrich-Wilhelms-Universitt
Bonn, Computer Science Institute VI: Autonomous Intelligent Systems,
http://www.NimbRo.net/@Home.
Sucar, E., Morales, E., Heyer, P., Vasquez, I., Palacios-Alonso, M. A., Escalante, H. J., . . .
Estevez, C. (2012). Markovito's Team Description RoboCup@Home 2012. National
Institute for Astrophysics, Optics and Electronics, Computer Science Department;
168
Research Center in Mathematics, Computer Science Department; Guanajuato
University, http://ccc.inaoep.mx/~markovito.
Suthakorn, J., Onprasert, W., Nakdhamabhorn, S., Phuengsuk, R., Itsarachaiyot, Y.,
Moonjaita, C., . . . Patel, S. (2013). RoboCup@Home League 2013 <BART LAB
AssistBot (THAILAND)>. Center for Biomedical and Robotics Technology (BART
LAB), Faculty of Engineering, Mahidol University, http://www.bartlab.org.
The Orocos Project. (s.f.). Recuperado el 05 de Septiembre de 2014, de http://orocos.org/
Tutorials/1.3/ros enabled model plugin. (30 de November de 2012). Recuperado el 05 de
Septiembre de 2014, de
http://wiki.gazebosim.org/wiki/Tutorials/1.3/ros_enabled_model_plugin
Understanding ROS Topics. (05 de Marzo de 2014). Recuperado el 31 de Agosto de 2014, de
http://wiki.ros.org/ROS/Tutorials/UnderstandingTopics
USB Downloader (LN-101). (2010). Recuperado el 07 de Septiembre de 2014, de ROBOTIS
e-Manual v1.23.00:
http://support.robotis.com/en/product/auxdevice/interface/ln101_manual.htm
van Elteren, T., Neculoiu, P., Oost, C., Shantia, A., Snijders, R., van der Wal, E., & van der
Zant, T. (2013). BORG The RoboCup@Home team of the University of Groningen
Team Description Paper 2013. Faculty of Mathematics and Natural Sciences,
University of Groningen Dept. of Artificial Intelligence, http://www.ai.rug.nl.
Vargas M., H. S., Olmedo U., E., Martnez M., A. D., Poisot M., V., Perroni G., A. A.,
Rodrguez C., A., . . . Orozco E., A. (2013). "Project Donaxi@HOME Service Robot".
Applied Mechanics and Materials, 423-426,
doi:10.4028/www.scientific.net/AMM.423-426.2817.
Vargas, H. S., Olmedo, E., Martnez, D., Poisot, V., Perroni, A., Rodrguez, A., . . . Portillo,
A. (2013). Donaxi@HOME Project. Universidad Popular Autnoma del Estado de
Puebla, Department of Engineering.

Poisot Manipulacion Objetos Donaxi

Transféré par

Informations du document

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Poisot Manipulacion Objetos Donaxi

Transféré par

Droits d'auteur :

Formats disponibles

UNIVERSIDAD POPULAR AUTNOMA

DEL ESTADO DE PUEBLA

Puebla, Pue., Mxico. Septiembre de 2014.

CARTA DE APROBACIN DE OPCIN DE TITULACIN.

CARTA DE APROBACIN DE ASESOR.

Actualmente existen varios proyectos en desarrollo en el laboratorio de Control y

Carta de aprobacin de opcin de titulacin. ................................................................. II

Carta de aprobacin de asesor. ..................................................................................... III

Sumario. .................................................................................................................... VII

1.1. Antecedentes. ................................................................................................. 2

1.2. Justificacin. .................................................................................................. 6

1.3. Objetivos. ....................................................................................................... 7

1.3.1. Generales. .................................................................................................. 7

1.3.2. Especficos. ................................................................................................ 7

1.4. Pregunta de investigacin. .............................................................................. 8

1.5. Alcances y limitaciones. ................................................................................. 8

1.6. Viabilidad de la investigacin. ........................................................................ 9

1.7. Impacto esperado. .......................................................................................... 9

1.8. Estructuras de memoria de tesis. ................................................................... 10

Captulo 2. Modelos cinemticos. ................................................................................ 12

2.1. Brazos Robticos. ........................................................................................ 13

2.2. Modelos Cinemticos. .................................................................................. 19

2.3. Convencin Denavit-Hartenberg en manipulador de Donaxi......................... 25

2.4. Cinemtica Directa de Brazo de Donaxi. ...................................................... 34

2.5. Mtodos de Cinemtica Inversa. ................................................................... 36

2.5.1. Mtodos Geomtricos. ............................................................................. 37

2.5.2. Transformacin inversa. ........................................................................... 37

2.5.3. Desacoplo Cinemtico.............................................................................. 38

2.5.4. Tcnicas Iterativas. .................................................................................. 39

2.6. Viabilidad de Mtodos de Cinemtica Inversa en Robot de Servicio Donaxi. 41

2.7. Propuesta de Cinemtica Inversa para Manipulador de Donaxi. .................... 44

2.8. Planificacin de Rutas para Manipulacin. ................................................... 50

2.8.1. Tipos de Trayectorias. .............................................................................. 51

2.8.2. Generacin de Trayectorias. ..................................................................... 52

2.9. Propuesta de Trazado de Rutas para Manipulacin de Donaxi. ..................... 53

Captulo 3. ROS. .......................................................................................................... 60

3.1. Arquitecturas de Control para Robots. .......................................................... 61

3.2. Arquitectura de Control Utilizada en Donaxi. ............................................... 69

3.3. Ventajas del uso de ROS. ............................................................................. 72

3.4. Programacin en ROS. ................................................................................. 74

3.5. Programacin de Secuencias con Brazo de Donaxi. ...................................... 78

3.6. Simulacin de un Brazo en Gazebo. ............................................................. 82

Captulo 4. Implementacin de Algoritmos. ................................................................. 86

4.1. Librera de Cinemtica y Dinmica (KDL) de OROCOS. ............................. 87

4.2. Programacin de Cinemtica Directa. ........................................................... 90

4.3. Programacin de Cinemtica Inversa. ........................................................... 94

4.4. Programacin de Algoritmo de trazado de rutas. ........................................... 96

4.5. Simulacin de Cinemtica Inversa. ............................................................... 98

Captulo 5. El Brazo Robtico de Donaxi. .................................................................. 101

5.1. Servomotores Dynamixel. .......................................................................... 102

5.2. CM700 ....................................................................................................... 105

5.3. Programacin de la Interfaz. ....................................................................... 109

5.4. Entorno de Usuario..................................................................................... 111

5.5. Implementacin con ROS de la Interfaz. .................................................... 115

Captulo 6. Conclusiones y perspectivas ..................................................................... 119

6.1. Conclusiones. ............................................................................................. 120

6.2. Perspectivas y trabajo futuro....................................................................... 123

Anexo A. Algoritmo Denavit-Hartenberg. .............................................................. 126

Anexo B. Cdigo de Matriz de TRANSFORMACIN para wxMaxima. ............... 127

Anexo C. Cdigos Generales.................................................................................. 131

Anexo D. Cdigos Utilizados en la Programacin de Secuencias. .......................... 132

Anexo E. Cdigos de Cinemtica. .......................................................................... 146

Anexo F. Cdigos de Interfaz de Comunicacin. .................................................... 155

Bibliografa ................................................................................................................ 162