Argentina – Mendoza

Modelado cinemático de robot FANUC S-420I F, para

“Sistema de corte por plasma en vigas perfil H”

Año: 2020
Proyecto Final de Catedra

Autores:
IGLESIAS Alfredo Andres; Legajo: 10299
ORTEGA Pablo; Legajo: 10666

Catedra: Robótica I
Profesores: Diaz Carolina; Sánchez Eric
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

RESUMEN
Se presenta el siguiente informe del Proyecto Integrador para la cátedra Robótica 1, con el fin de integrar y evaluar los
conocimientos adquiridos a lo largo del dictado de catedra.
El proyecto tiene como objetivo la realización del modelado, simulación, y análisis a nivel cinemático, de un robot
comercial que se encuentra abocado a la realización de una tarea de corte de vigas de perfil H mediante corte por plasma. El
mismo se encuentra, actualmente en etapa de desarrollo, un Robot FANUC S-420I F propiedad de la empresa Log Metal.
Se presentan los desarrollos de modelado mediante la convención de Denavit- Hartenberg utilizada para el desarrollo de
algoritmos que nos permiten calcular y simular la cinemática directa e inversa, posteriormente analizar singularidades
mediante el desarrollo del Jacobiano y sus parámetros característicos, y finalmente la planificación, generación y simulación
de trayectorias, generando un análisis sobre el robot real y posibles tareas del robot enmarcadas en la aplicación mencionada,
analizando velocidades y aceleraciones tanto en el espacio articular como en el espacio cartesiano. Por último, se presenta
una sección con sensores y actuadores sugeridos para el sistema completo.
Se abordaron aspectos específicos del robot tratado, siendo que el mismo posee un mecanismo de cadena cinemática
cerrada, lo que lo convierte en un robot de tipo paralelo, es así, que se adoptaron simplificaciones y consideraciones
específicas para trabajar con el mismo.
Finalmente, mediante la herramienta de programación (Matlab) y el toolbox de Peter Corke, se obtienen simulaciónes
formidable del modelaje cinemático. Y con ellas lograron contrastar los conocimientos teóricos/prácticos adquiridos y
además prever posibles errores al momento de implementar la aplicación real.

Pág. 2 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

ÍNDICE
RESUMEN ........................................................................................................................................................................... 2
ÍNDICE ................................................................................................................................................................................. 3
A. INTRODUCCIÓN .................................................................................................................................................... 4
B. DESARROLLO ........................................................................................................................................................ 5
1. Descripción del sistema a modelar ............................................................................................................................ 5
2. Convención Denavit – Hartenberg ............................................................................................................................ 7
3. Cinemática Directa ................................................................................................................................................. 11
3.1. Función desarrollada ....................................................................................................................................... 12
4. Cinemática Inversa ................................................................................................................................................. 15
4.1. Función desarrollada ....................................................................................................................................... 17
5. Jacobiano ................................................................................................................................................................ 20
6. Trayectorias ............................................................................................................................................................ 26
7. Sensores y actuadores ............................................................................................................................................. 32
C. APORTE ................................................................................................................................................................. 35
D. CONCLUSIONES .................................................................................................................................................. 37
E. BIBLIOGRAFÍA .................................................................................................................................................... 38
F. ANEXOS ................................................................................................................................................................ 39

Pág. 3 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

A. INTRODUCCIÓN

En la actualidad, si se analizan los sistemas de corte por plasma automatizados, se pueden encontrar algunas variantes,
pero en su gran mayoría los que resaltan son los que usan control numérico computacional CNC, de diseño tipo pantógrafo.
Este tipo de maquinaria responde satisfactoriamente a las necesidades de corte por chapa para trabajar con 3 grados de
libertad. Pero estos suelen presentar en algunos casos limitaciones, al momento de intentar realizar cortes sobre materiales
con geometrías complejas, así también su área de trabajo queda delimitada en su mayoría a una altura pequeña. No obstante,
existen algunos pantógrafos de altos costos y complejidad que responden a estas necesidades.
Considerando lo recién mencionado y gracias al tener disponible un Robot FANUC S-420I F en la empresa Log Metal,
se plantea en la tesina del alumno Alfredo Iglesias, generar un sistema de corte por plasma para vigas perfil H de 12m con
el robot mencionado (actualmente en desarrollo). Se destaca la incorporación de Pablo Ortega en colaboración, en la segunda
etapa de esta, para generar su tesina. De esta manera, se obtiene con este robot los veneficios de robustez, precisión,
repetibilidad y capacidad de trabajar en los 6 grados de libertad, del gran espacio de trabajo que dispone el robot.
Teniendo en cuenta el marco aplicativo, se presenta concretamente este proyecto final, de la materia Robótica I, que tiene
como objetivo el modelado cinemático del robot FANUC, para la aplicación del “Sistema de corte por plasma en vigas perfil
H”. Teniendo como alcance, la posibilidad de generar un análisis previo, mediante el modelo cinemático de las limitaciones
y posibles tareas del robot enmarcadas en la aplicación mencionada.
En el desarrollo de la problemática planteada, se describirán los detalles del modelado del sistema. Donde se resalta que
el robot es de tipo paralelo y posee una cadena cinemática cerrada, no obstante, se procederá a trabajar con el mismo
descomponiéndolo en robots de tipo de serie, para poder resolver la problemática planteada en la guía de acuerdo con los
desarrollos aplicados a robots de tipo serie a lo largo del cursado de la cátedra. Luego de esta aclaración, la problemática del
modelaje cinemático se dividirá en 6 grandes etapas. Esto se logrará gracias a los conocimientos teóricos/prácticos obtenidos
mediante el cursado de la catedra, siendo estos desarrollados mediante trabajos prácticos.
Dentro de estas 6 grandes etapas, se encuentran desarrollados y aplicados los conceptos de la convención Denavit –
Hartenberg; la función y desarrollo validado de la Cinemática Directa del robot; la función y desarrollo validado de la
Cinemática Inversa del robot; análisis y verificación de puntos singulares hallados para el robot y la aplicación mediante las
graficas de elipsoides utilizando el Jacobiano del robot; planificación y generación de trayectoria relacionada con la
aplicación, analizando velocidades tanto articulares como cartesianas del entorno de trabajo; por ultimo se presentara una
sección con los sensores y actuadores sugeridos para todo el sistema completo.
Finalmente, mediante la herramienta de programación (Matlab) y el toolbox de Peter Corke, se espera obtener una
simulación formidable del modelaje cinemático. Y con ella poder contrastar los conocimientos teóricos/prácticos adquiridos
y además prever posibles errores al momento de implementar la aplicación real.

Pág. 4 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

B. DESARROLLO
1. Descripción del sistema a modelar

Para lograr describir el sistema en su totalidad, primeramente, se analiza el robot con sus características, luego las
limitaciones que impone el sistema de corte, enfocado al corte de vigas H.

Según [1], [2] y verificando correctamente el modelo exacto del robot debido a que presenta 3 variantes del modelo S-
420i F, se extraen las siguientes características principales del robot.
Características del Robot: Marca FANUC, Modelo: S-420I F, Año de fabricación 1996, Controlador: RJ2

Figura 1-1: Caracteristicas generales del robot

• Configuración mecánica:

Figura 1-2: Imagen izquierda: disposicion de motores. Imagen derecha: mecanismo de 4 barras 1 con lados igales, para transmitir el movimiento
del motor 3 al el eje 3

1
Se resalta la importancia de este mecanismo ya que es el punto de vinculación en los posteriores análisis del robot, al querer descomponerlo.

Pág. 5 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

• Espacio de trabajo del robot:

Figura 1-3: Entorno de trabajo que puede alcanar el robot. Imagen izquierda: vista lateral derecha. Imagen derecha: vista superior

Luego de hacer un análisis del robot con sus respectivos manuales, se presenta el espacio de trabajo del sistema completo.
El sistema en su totalidad está comprendido por, la mesa de apoyo de la viga perfil H, y el robot que incorpora en su
extremo de trabajo, una herramienta para el corte por plasma. La finalidad del robot, es lograr llevar con precisión a los
puntos memorizados, la torcha para luego mediante una trayectoria recta, generar un corte a velocidad constante, a lo largo
de la viga sobre el alma de la misma.

Figura 1-4: Entorno de trabajo, del sistema completo

Pág. 6 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

2. Convención Denavit – Hartenberg

Según [3] (U3 - Denavit – Hartenberg).

Se parte desde la primicia de que el robot Fanuc planteado es del tipo paralelo (cadena cinemática cerrada), para su
resolución, se plantea dividir la problemática en dos robots tipo series (cadena cinemática abierta).
Aplicación de la convención DH en ambos robots:

Figura 2-1: Robot 2 de 4GL

Figura 2-2: Robot 1, de 6 GL

Pág. 7 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

A partir de aplicar la convención y de la información geométrica en las Figura 1-3, se presentan las matrices DH
𝛳
ROBOT 1 d a α 𝛳
σ
(m) (m) (rad) ROBOT 2 d a α σ
(rad)
1 0 1 0.18 -π/2 0 (m) (m) (rad)
(rad)
2 -π/2 0 0.95 0 0
1 0 1 0.18 -π/2 0
3 0 0 0.24 -π/2 0
4 0 1.3 0 π/2 0 2 0 0 0.6 0 0
5 0 0 0 -π/2 0 3 π/2 0 0.95 0 0
6 π 0.2 0 0 0 4 0 0 0.6 0 0

Tabla 1: Matrices DH

Limites articulares

Según [2] se extraen los limites articulares reales del robot. Con estos se generar las siguientes funciones para el robot 1
y 2, parametrizadas por los limites articulares. En el caso de los límites para el robot 2, se hace un análisis simple geométrico,
para el mecanismo de 4 barras mostrado en la Figura 1-2 (Derecha). Encontrando que este mecanismo para trasladar el
movimiento del motor 3 a la articulación 3, no introduce cálculos complejos, debido a la simetría del sistema y sus
características.

Figura 2-3: Limites articualeres de hambos robots expresados mediante las funciones del toolbox de [4]

Offset

Para ambos robots, fue necesario incorporar un offset, para lograr que las articulaciones partieran desde un marco de
referencia adecuado, y que sea consistente con el robo real. Se invita en este inciso a ver la sección C más adelante, donde
se logró llevar al robot real, según sus marcar articulares de fábrica y utilizando la programación del mismo a su posición de
offset, para corroborar los datos ingresados en la simulación.

Figura 2-4: Offset , izquierda robot 1, derecha robot 2

Pág. 8 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Graficas de los robots

Utilizando las herramientas que nos provee [4], se obtienen las siguientes imágenes. Todos los códigos en Matlab,
necesarios a lo largo del informe, se encuentran adjuntos a este.

Figura 2-5: Robot 1, vista lateral derecha y vista en perspectiva isometrica

Figura 2-6: Robot 2, vista lateral derecha y vista en perspectiva isometrica

Matriz de base y Tool

Se trabajó con matrices de base y herramienta tentativas, esto porque dada la aplicación de nuestro robot, y siendo que
aún no se ha implementado la misma, se decidió utilizar valores tentativos. En el caso de la base se la desplazo 0.5 m en el
eje Zo, hacia arriba, pues sería interesante tener nuestro robot en una posición más alta dándole quizás, mayor facilidad en
la tarea ya que la mayoría de los movimientos que se espera realizar será en posiciones bajas. También esto genera una
ventaja, al analizar las articulaciones 2 y 3, ya que en los momentos que el robot pasa por su frente de offset, estas se acercan
demasiado a sus límites, y de esta manera se pueden alejarlas. En cuanto a la herramienta, aún no ha finalizado el proceso
de diseño de la misma, por lo que se decidió adoptar una barra recta de 60 cm, sobre la cual se colocaría el controlador de
altura, y en este último la antorcha de plasma.

Figura 2-7: Matriz base y herramienta

Pág. 9 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Robot 1 y Robot 2 completos

Una vez obtenido los robots 1, 2 con sus respectivos limites articulares, offsets, y las matrices de herramienta y tool Se
presenta el robot completo1.

Figura 2-8: Robot completo 1 y 2, vista lateral derecha y vista en perspectiva isometrica
Como se aprecia en las imágenes Figura 2-8, el sistema en el extremo final del robot 2, debe coincidir continuamente
sobre el sistema numero 2 o la articulación 3 del robot 1, esto se puede aprecia en las Figura 2-1, Figura 2-2.

Figura 2-9: Robot 1: matris DH, offset, base y tool

Figura 2-10: Robot 2: matris DH, offset.

1
Se aclara que en el robot 2, fue necesario solamente incluir la matriz base del robot 1 para lograr su coincidencia.

Pág. 10 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

3. Cinemática Directa

Para el estudio de nuestro robot, como se habló anteriormente, se procedió a la descomposición del mismo en dos robots
de tipo serie. Siendo que estos dos robots están relacionados, pues en el robot real, se trata de un mecanismo de cuatro barras
(de tipo paralelogramo), que vincula el movimiento del eje del motor de la articulación 3, con el respectivo punto de pívot
de dicha articulación generando el movimiento del antebrazo. Para encontrar las ecuaciones que vincularan las articulaciones
de ambos robots se procedió a analizar el mecanismo de cuatro barras, el punto de pívot y el punto donde se encuentra el eje
del motor 3, de la descomposición geométrica (debajo se explicita en una figura a mano alzada) se obtuvieron las siguientes
ecuaciones1:
• 𝑞21 = 𝑞11
• 𝑞22 = 𝑞13 + 𝑞12
• 𝑞23 = − 𝑞13
• 𝑞24 = 𝑞13

Una vez relacionados nuestros robots serie, según [3] (U3-Cinematica Directa) se procedió al desarrollo e
implementación de las respectivas funciones de cinemática directa, en las cuales se procedió a obtener la matriz de
transformación homogénea trabajando con las cuatro operaciones elementales, a partir de la información contenida en la
matriz de Denavit- Hartenberg y los vectores articulares elegidos.
A continuación, se presentan una serie de imágenes para ilustrar los resultados obtenidos:

Figura 3-1: Espacio de trabajo del robot modelado

1
Los indices 𝑞21 se refieren a la articulación 1 del robot 2, 𝑞11 la articulación 1 del robot 1.

Pág. 11 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Observando las Figura 3-1, se presenta los límites del espacio de trabajo del robot, utilizando solamente al robot 1,
debido a que la función teach de [4] ,no lo permite. Otra opción para generar esta gráfica, era la de generar todos los posibles
puntos en el espacio cartesiano que el robot puede acceder. Debido al trabajo que conlleva esta opción, se tomó el primer
camino, obteniendo resultados que se pueden constatar con la Figura 1-3.
Tanto en las imágenes mostradas como en los siguientes ejemplos, se aclara nuevamente que se trabajó con matrices de
base y herramienta tentativas, esto porque dada la aplicación de nuestro robot, y siendo que aún no se ha implementado la
misma, se decidió utilizar valores tentativos.

3.1. Función desarrollada

En el código desarrollado en Matlab, al comienzo del mismo, se dedica una línea donde se escribe el vector articular
deseado para el robot 1, posteriormente se calculan las articulaciones del robot 2, para luego llamar a las respectivas funciones
que calculan las matrices de transformación homogénea de cada uno. Se realiza una verificación de las mismas matrices
homogéneas T, mediante el uso de la función fkine de [4]. Por último, se realiza además una verificación de los limites
articulares.
A continuación, se presentan una serie de resultados obtenidos con sus respectivos datos de entrada:
Ejemplo 1:

Pág. 12 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 3-2: Robot en posicion ejemplo 1

Ejemplo 2:

Pág. 13 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Ejemplo 3:

Pág. 14 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

4. Cinemática Inversa

Para la resolución de la cinemática inversa se procedió mediante el empleo del método de Pieper según [3](U3-
Cinematica Inversa), ya que el robot trabajado presenta las características de muñeca esférica, por lo que podemos, como
plantea el método, descomponer la resolución en dos problemas separados, uno basado en la posición (articulaciones 1, 2 y
3) y el otro en la orientación (articulaciones 4, 5 y 6).
En primera instancia, según lo visto en clase y lo que se pudo profundizar en [5] (Apartad 4 – pág. 85), se procedió a
realizar un planteo geométrico para la obtención de las ecuaciones necesarias para las primeras tres articulaciones.
Posteriormente se planteó un problema algebraico analizando la orientación para obtener las ecuaciones de las últimas tres
articulaciones. Se aclara que estos planteos, tanto analíticos como los códigos de Matlab fueron desarrollados, en la sección
F-Anexos. Sin embargo, estos planteos no fueron exitosos, presentaron errores y problemas en la implementación; por las
limitaciones de tiempo que se han presentado en el actual semestre y que son de mutuo conocimiento, se decidió, para
cumplir adecuadamente con los plazos de entrega, adoptar un método presentado por el profesor de la cátedra, Eric Sanchez,
para el planteo de Pieper. Este método, se encuentra planteado mediante ecuaciones que se podían adaptar fácilmente a
robots de 6 g.d.l que presentaran características similares al presentado en este trabajo. El código con la implementación se
muestra al final de este trabajo.

Descripción breve del método utilizado:

Según Piper (desacople cinemático) y gracias a que el robot tiene una muñeca esférica, es posible simplificar el problema
dividiéndolo en un problema de cinemática inversa en la posición y otro de cinemática inversa para la orientación. El punto
donde se cruzan las coordenadas de los sistemas 4, 5 y 6, que es el origen de coordenadas de 𝑜4 ; 𝑥4 𝑦4 𝑧4 𝑦 𝑜5 ; 𝑥5 𝑦5 𝑧5 ,
será el punto central de la muñeca. Con este punto podemos dividir ambos problemas y le asignaremos el vector 𝑑c , el cual
queda independiente de la orientación de las últimas tres articulaciones, solo dependerá de la orientación de las tres primeras
articulaciones.
Según el problema general:
⃗ = 𝑓( 0𝑇6 )
𝑞 (Ec.B-1)

Siendo 𝑞 el vector de variables articulares y 0𝑇6 la matriz homogénea que representa posición y orientación del extremo
6 referida al sistema 0 fijo a la base.

Primer problema de Piper, planteamos:

0
(𝑞1 , 𝑞2 , 𝑞3 ) = 𝑓1 ( 0𝑥4 , 0𝑦4 , 𝑧4 ) (Ec.B-2)

Partiendo de una posición y orientación dada para el sistema de coordenadas 𝑜6 ; 𝑥6 𝑦6 𝑧6 , dado como una transformación
homogénea relativa a la base del robot.
0 0
0
𝑇6 = [ 𝑅6 𝑑6 ] (Ec.B-3)
0 1

Con esta, es posible calcular el punto central de la muñeca que coincide con el 𝑆4 𝑦 𝑆5 .
0 𝑟13
0 0
𝑑6 = 𝑑𝑐 + 𝑅6 [ 0 ] = 𝑑𝑐 + 𝑑6 [𝑟23 ] (Ec.B-4)
𝑑6 𝑟33
• Donde 𝑑6 = 0.2𝑚 es la distancia entre el 𝑆5 𝑦 𝑆6 a lo largo de 𝑧5 .
• Los r1,2,3;3 corresponden al verzor Z o tercera columna de la matriz de rotación dentro de la matriz de DH.

Despejando:
𝑟13
0 𝑟 (Ec.B-5)
𝑑𝑐 = 𝑑6 − 𝑑6 [ 23 ]
𝑟33

Pág. 15 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 4-1: Reprecentacion del robot en posicion articular 0, con el vector 𝑑𝑐 coincidente con el eje de la articulacion 4

Figura 4-2: Reprecentacion del robot en posicion articular 0, con la proyecion en el plano X,Y del vector 𝑑𝑐 , obteniendo 𝑋𝑐 , 𝑌𝑐

Teniendo la posición del punto central de la muñeca, se procede a calcular las funciones que determinan los valores de
las articulaciones 1,2 y 3.
Los procedimientos siguientes consisten en la obtención, mediante el método geométrico, las ecuaciones para las
articulaciones 1, 2 y 3, que como se comentó anteriormente, se adoptaron y adaptaron las obtenidas por el profesor.
Destacando aquí, que nuestro robot respecto al planteado por el profesor, tienen diferencias geométricas principalmente en
la articulación 3. En nuestro caso está articulación, no se encuentra desplazada respecto al eje 𝑦0 , pero si se encuentra
desplazada respecto a al eje 𝑧0 . Gracias al haber hecho nuestro primer planteo, esta adaptación fue rápidamente solucionada,
debido a que ya teníamos mejor comprendida la problemática geométrica. Haciendo uso de las gráficas del robot que
habíamos generado anteriormente en nuestro análisis en papel y lápiz.

Segundo problema de Piper, cálculo de la orientación, planteamos:

(𝑞4 , 𝑞5 , 𝑞6 ) = 𝑓2 (, 𝑞1 , 𝑞2 , 𝑞3 , 0𝑇6 ) (Ec.B-6)

Seguidamente se resolvió el problema de orientación para las articulaciones 4, 5 y 6, que inicialmente fue encarado de
manera algebraica, pero se adoptaron las ecuaciones obtenidas por el profesor, mediante un planteo geométrico.

Pág. 16 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Solución general 1

Del procedimiento utilizado se obtienen ocho soluciones posibles, pues del primer análisis geométrico surgen dos
soluciones para la articulación 𝑞1 , dos de la articulación 𝑞2 para cada una de las soluciones de 𝑞1 , e idem situación con la
tercera articulación 𝑞3 . Esto resulta, hasta el momento en cuatro vectores articulares posibles.
Luego, para las articulaciones 𝑞4 , 𝑞5 y 𝑞6 se obtienen ocho valores posibles para cada articulación, cuyo cálculo, además,
depende de las anteriores. Al terminar el procedimiento se tienen ocho vectores solución en total, de los cuales se busca el
más óptimo.
El método de selección consiste en comparar cada una de las ocho posibles soluciones con el vector de coordenadas
articulares actual, el cual es pasado como parámetro a la función, y de esta manera se selecciona el vector que implique un
movimiento menor (dado por la sumatoria absoluta, de los movimientos de todas las articulaciones), para llegar al punto
deseado y que a su vez verifique limites articulares y del espacio de trabajo.
Se resalta en el método de verificación que la solución encontrada, se encuentre dentro del espacio de trabajo articular,
descartando las que no se encuentren. Luego si esta se encuentra dentro del espacio de trabajo matemático. Y finalmente en
el caso de encontrarse en alguna singularidad, o esta sea una solución de la más cercana, pero no se encuentre dentro del
espacio físico articular o espacio de trabajo matemático, esta solución se recalcula, obteniendo una solución que no es la más
cercana de las 8 posibles, pero si consistente.

4.1. Función desarrollada

Las ecuaciones planteadas se incluyen en el código de Matlab en Anexos. En el código desarrollado, para facilitar la
interacción con el usuario, le permite ingresar la posición deseada de dos maneras. De manera directa, ingresando los valores
de posición y orientación del extremo, a través de la matriz de transformación homogénea correspondiente. Y de manera
indirecta (se usa este por defecto), ingresando los valores del vector articular, al cual seguidamente, se le calcula la cinemática
directa para obtener la matriz de transformación homogénea, la cual es pasada como parámetro a la función de cinemática
inversa.
A continuación, se presentan algunos resultados:
Ejemplo 1:
Ingreso de datos por método indirecto, vector articular deseado: [180,-45,50,60,32,0]

1
Se recuerda que al igual que en la cinemática directa se procedió a tratar los resultados del robot 2, como dependientes de los resultados del robot 1,
ya que la relación de uno con otro se da por el mecanismo de cuatro barras anteriormente nombrado. Esto nos permite utilizar las soluciones articulares
del robot1, para obtener los valores de las articulaciones del robot 2. Sin la necesidad de calcular la cinemática inversa del robot 2, ya que solamente se
precisa obtener el valor de la articulación 2 del robot 2, y esta tiene una relación lineal con la articulación 3 del robot 1.

Pág. 17 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 4-3: Posicion robot, ejemplo 1 CI

Ejemplo 2:
Ingreso de datos por método indirecto, vector articular deseado: [45,25,-25,180, -20,60]

Pág. 18 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 4-4: Posicion robot, ejemplo 2 CI

Pág. 19 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

5. Jacobiano

Según [3] (U3- Jacobiano).

Para proceder a encontrar el Jacobiano y su determinante, se procedió a implementarlo de manera simbólica en Matlab,
dada la complejidad del robot trabajado, pues resultaría poco práctico plantear dichas ecuaciones a mano. Aun así, resultó
ineficaz dicho análisis, ya que las ecuaciones que se obtuvieron resultaron muy extensas y su análisis de una complejidad
que precisaba de mucho tiempo, más del disponible para los plazos de entrega del presente informe. Dichos resultados se
aprecian en formato de comentario, en el código correspondiente en la sección Anexos.
Por lo descripto en el párrafo anterior es que se decidió por implementar un método de análisis numérico. Se trabajó con
las funciones del toolbox de Peter Corke [4] y [6], y se realizó una evaluación del Jacobiano, su determinante y otros
parámetros, en un conjunto de 10000 puntos, de valores articulares posibles para cada una de las seis articulaciones,
generados de manera aleatoria entre los respectivos límites articulares.
En primera instancia se procedió con el análisis del determinante y el número de condición, valores representativos e
indicadores de posibles singularidades en las posiciones generadas. Se realizaron gráficas de puntos dispersos para evidenciar
un poco mejor como resultó esta generación aleatoria de posiciones, a continuación, las gráficas:

Figura 5-1: Valores que toma el determinante, en funcion del nº de vector articular tomado

Pág. 20 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 5-2: Valores que toma el numero de condicion, en funcion del nº de vector articular tomado

Como puede verse en las Figura 5-1y Figura 5-2, se trabajó un conjunto de puntos bastante representativo de las diferentes
posiciones del robot, obteniendo posiciones que verificaban estar lejos de singularidades tanto como posiciones que estaban
prácticamente en una singularidad. Habiendo verificado esto se muestran luego, a modo de síntesis, los valores máximos y
mínimos obtenidos para los parámetros graficados.
Para la posterior selección de aquellos vectores articulares que no verificaban la condición de estar lejos de
singularidades, se decidió adoptar un criterio basado en el análisis del determinante del Jacobiano, donde se identificaron
como vectores problemáticos aquellos cuyo determinante resultaba estar por debajo de 0.001.
De esta selección se obtuvieron, dentro de los 10000 vectores, 21 vectores etiquetados como problemáticos. De esos 21
vectores se muestran, a continuación, se muestran 5 ejemplos, para evidenciar un poco mejor las causas y tipos de
singularidades que acusa el robot trabajado.
Ejemplo 1:
Vector articular (Grados °) = [144.1849 , -32.3836 , -32.7312 , -137.5946 , -86.3195 , -337.5901]

Figura 5-3: Posicion robot, ejemplo 1 , Jacobiano

Pág. 21 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Ejemplo 2:
Vector articular (Grados °) = [91.8342 , 25.0163 , 38.7205 , 51.8584 , 0.0259 , 152.0597]

Figura 5-4 Posicion robot, ejemplo 2 , Jacobiano

Ejemplo 3:
Vector articular (Grados °) = [-96.3100 , 71.4000 , 77.7230 , 24.3267 , 0.5471 , -345.2530]

Figura 5-5 : Posicion robot, ejemplo 3 , Jacobiano

Ejemplo 4:
Vector articular (Grados °) = [170.2180 , -75.2219 , 57.7760 , -272.0003 , 0.0603 , -313.2797]

Figura 5-6: Posicion robot, ejemplo 4 , Jacobiano

Pág. 22 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Ejemplo 5:
Vector articular (Grados °) = [-11.4479 , -71.0398 , 24.4448 , -47.9740 , 81.9701 , 342.4179]

Figura 5-7: Posicion robot, ejemplo 5 , Jacobiano

Análisis de los 5 ejemplos:
En el caso de los ejemplos 1 y 2 se evidencia que el robot está trabajando muy cerca del límite del espacio de trabajo, en
el primer caso, en la parte superior, y en la inferior para el segundo caso. Similar situación se aprecia en el ejemplo 4, donde
se está por llegar a un límite, pero en este caso, provocado por la capacidad del robot para plegarse sobre sí mismo, en base
a las articulaciones 2 y 3. En el caso del ejemplo 3 se evidencia que el robot está en el punto máximo del espacio de trabajo,
al impactar consigo mismo, es difícil apreciar esto último en la imagen pero prestando atención al vector, y teniendo el mente
el robot real, con el volumen de sus eslabones, es evidente la situación. En el ejemplo 5 se aprecia que la cercanía a la
singularidad es provocada por la interferencia entre eslabones que se genera gracias a la combinación articular, se detectan
como problemáticas, las articulaciones 2 y 3.
En conclusión, podemos ver que lo que provoca que la manipulabilidad se vea drásticamente reducida son aquellas
combinaciones articulares que posicionan el extremo en puntos que se encuentran en los límites del espacio de trabajo,
puntos que provocan interferencias entre los propios eslabones del robot, y puntos donde alguna de las articulaciones trabaja
en una posición límite, donde en dicho caso, la combinación articular tiende a una singularidad.
Como ejemplo de combinaciones articulares que causan problemas de interferencias se presenta a continuación,
información extraída del manual del fabricante:

Figura 5-8: Interferencia entre articulacion 2 y 3

Como puede apreciarse en la Figura 5-8 el robot presenta interferencia entre las articulaciones 2 y 3 cuando se trabaja
cerca de sus límites, y están determinados dichos rangos articulares, se determina una interferencia de 20° entre dichas
articulaciones, esto es provocado por el mecanismo de cuatro barras que realiza la vinculación del eje del motor 3 con el
punto de pivot del antebrazo. Esto último es difícil de apreciar bien en los modelos presentados en las gráficas de Matlab.

Pág. 23 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 5-9: Limites mecanicos de la articulacion 3. Hacen tope con el meacnismo de 4 barras
Dicha interferencia en el robot real, es tratada mediante 2 limites mecánicos, pero cabe resaltar que estos están colocados
sobre el eslabón dos del robot 1. Según el análisis hecho al robot real, en estas circunstancias, el controlador, no genera
ninguna alerta de “conflicto articular o interferencia”. Por ejemplo, al momento de ejecutar una trayectoria particular que
condicione a la articulación 3 a querer moverse estando muy cerca de alguno de los topes mecánicos mencionados, el robot
intenta generar la trayectoria, moviéndose lentamente y apenas colisiona, indica simplemente una alerta por exceso de
corriente en el motor. Pero no soluciona la problemática intentando mover la articulación 2 para que esta articulación 3 salga
de la interferencia.
Debido a todo esto, en los modelos planteados en Matlab, se modificó adecuadamente los valores de los límites de la
articulación 3, para impedir que ocurran estos casos de interferencia a la hora de hacer las simulaciones de las cinemáticas
directa e inversa, y de las trayectorias. Limitando levemente al robot en ciertas posiciones, el área de trabajo. Ya que por
tema de tiempos, no se logró generar de alguna manera, un código que permita simular en la articulación 3 el límite mecánico
impuesto sobre el eslabón 2 del robot 2 y sobre el eslabón 2 del robot 1. Ya que, este segundo limite se consideraría móvil
En la aplicación a trabajar solo se detectan como problemáticos aquellos puntos límites de la trayectoria, esto se verá
mejor en la próxima sección, ya que el objetivo es aprovechar el máximo alcance del robot, y dichos puntos pueden estar
cerca del límite del espacio de trabajo, por ello debe prestarse especial atención a la hora de planificar y programar la tarea
en este aspecto.
A continuación, se muestran las gráficas del elipsoide de manipulabilidad traslacional graficado con la función del
toolbox de Peter Corke, para las posiciones límite e intermedias de la aplicación a implementar.

Figura 5-10: Posible punto inicial de la trayectoria de la aplicación (cercano al extremo del espacio de trabajo)

Pág. 24 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 5-11: Posible punto medio de la trayectoria de la aplicación

Figura 5-12: Posible punto final de la trayectoria de la aplicación (cercano al extremo del espacio de trabajo)

Del análisis de las gráficas anteriores se concluye que, para la trayectoria planteada para efectuar la tarea, en todos los
puntos, tanto extremos como intermedios, se trabaja con una manipulabilidad traslacional adecuada, que permite realizar los
movimientos necesarios para mover la herramienta en línea recta sobre la pieza a trabajar. Se aprecia como en los extremos
la manipulabilidad es reducida en la dirección en que el robot podría estirarse, esto debido a que se está trabajando cerca de
los límites de trabajo del robot. Situación que es mucho más indulgente mientras nos aproximamos a la posición intermedia,
pues el robot tiene más capacidad para contraerse o estirarse. De ser difícil de apreciar la forma del elipsoide en las gráficas
mostradas se propone al lector ejecutar el código correspondiente (Jacobiano.m) incluido como anexo en este trabajo, para
poder realizar una mejor apreciación en el grafico tridimensional.

Pág. 25 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

6. Trayectorias

Según lo descripto en la Figura 1-4 al comienzo de este trabajo, donde se especifica la distribución del espacio de trabajo,
y se ve posicionado el robot frente al material a trabajar, desde una vista superior, podemos apreciar desde ya, la trayectoria
que el robot debe realizar. Se tratar un corte en línea recta, en el punto medio del alma de la viga, para realizar un
seccionamiento de la misma en partes iguales. A continuación, se presenta la imagen más detallada y especificando los
puntos de inicio y fin de la trayectoria de la tarea especificada.

Figura 6-1: Trayectoria de corte sobre el alma de la viga, desde el punto P1 hasta el punto P2

Dada la particularidad de esta experiencia, hemos tenido la oportunidad de trabajar sobre el robot real, y poder planificar,
programar y ejecutar dicha trayectoria in situ.
Los puntos utilizados para la misma fueron los siguientes: P1: [-49, 52, -10, 180, -46, -43] hasta P2: [49, 52, -10, 180, -
46, -43].
En las simulaciones en Matlab, con la intención de tener una representación de la posición y orientación, en las
respectivas matrices de transformación homogéneas, más fácil de interpretar y visualizar, se adoptaron los siguientes puntos:
P1: [45.0000 , 68.1881 , -33.0007 , 0 , 54.8126 , 135.0000] hasta P2: [-45.0000 , 68.1881 , -33.0007 , 0 , 54.8126 , 45.0000],
los cuales corresponden a las siguientes matrices de transformación homogéneas:

Establezca requisitos generales y requisitos específicos

Algunos de los requisitos que podemos contemplar, incluyen, en primer lugar, los puntos que comprenden la tarea a
realizar (corte con plasma). Los mismos se ubican sobre el alma de la viga, y la trayectoria que se realiza entre ellos es recta,
por ende, en la aplicación concreta se planificará la tarea de manera tal de obtener la mayor distancia entre ambos puntos y
así poder optimizar la tarea, siempre prestando atención a las consideraciones expuestas en el trabajo práctico anterior
(singularidades).

Pág. 26 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Puede verse en las matrices anteriores que hemos adoptado valores de posición bastante cerca del límite del espacio de
trabajo, de manera similar a la experiencia realiza in situ, y además en este caso utilizamos la misma submatriz de rotación,
pues para la aplicación mencionada, la herramienta es una torcha de plasma, la cual no tiene requisitos en cuanto al ángulo
de rotación respecto al eje Z, pero sí la rotación respecto a los otros dos ejes, x e y, ya que principalmente la exigencia en
cuanto a posición y orientación, es que debe mantenerse lo más perpendicular posible al material y a una distancia
determinada que depende de las características de la máquina de plasma empleada, todo esto para que una vez iniciado el
corte, la distancia entre la punta de la boquilla y el material sea la menor posible, ya que esto incide en la diferencia de
potencial generada en el arco de plasma, y a su vez esta diferencia de potencial es la que mide el controlador de altura y en
función de ella realiza las respectivas correcciones en la posición de la torcha en el eje z.
Por esto es que nos resultaron indiferentes los valores de rotación respecto a dicho eje. Aun así, es muy importante
considerar este aspecto, ya que para mantener la misma rotación respecto al eje Z, la articulación 6 ha debido girar, y es en
dicha articulación donde se encuentra montado el efector final, y en este último el controlador de altura que a su vez sujeta
la torcha. La torcha se encuentra conectada a cables de alimentación y a la manguera de aire comprimido. La importancia
de considerar el movimiento de la articulación 6 recae en el hecho de planificar una trayectoria que no permita que estos
cables y mangueras se enreden o choquen en el brazo del robot produciendo roturas en la herramienta y/o accesorios de la
misma, ni que se vean sometidas a la fatiga por tracción o flexión.
En la trayectoria actual, por su simplicidad, podríamos haber indicado un cambio de orientación en el punto final, de
manera que la articulación 6 no se hubiese movido en todo el recorrido evitando este problema, pero dada la simplicidad
de la tarea se verificó que este movimiento no produce ningún tipo de problemas en el sistema de amarre de cables y
mangueras, el cual además se encuentra en proceso de diseño y ya se han establecido las características que posibilitan un
amplio movimiento de los cables y mangueras para que no sucedan problemas como los descriptos anteriormente. Por ello
es que utilizamos la orientación descripta, para realizar el ejercicio mental de contemplar dicha problemática, la cual puede
ser de mayor preponderancia en alguna otra trayectoria más exigente desde el punto de vista articular.
Por otro lado, las velocidades, aceleraciones y tiempos de la tarea estarán muy por debajo de los límites alcanzables por
el robot, y dependerán enteramente del equipo utilizado para el corte por plasma y del material a cortar, ya que las
características de los mismos determinan la velocidad a la que se realiza el corte; el equipo de plasma a utilizar aún no ha
sido definido.
Implementación de dos soluciones diferentes para la trayectoria elegida

Figura 6-2: Izquierda: trayectoria con interpolacion articular. Derecha: trayectoria con interpolacion cartesiana

Pág. 27 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Posición, velocidad y aceleración de las 2 soluciones encontradas, a nivel articular y cartesiano del espacio de trabajo

- Interpolación articular e Interpolación Cartesiana en el Espacio Articular

Figura 6-3:Posición articular para: Izquierda interpolacion articular; derecha interpolacion carteciana

• Puede verse como la trayectoria curva de interpolación articular 3.a), el camino que utiliza describe un arco de
radio constante, y para esto simplemente mueve la articulación q1 que lo lleva a la posición cartesiana deseada y
q6 mantiene la orientación cartesiana
• En el caso de la trayectoria recta de la interpolación cartesiana 3.b) involucra más movimientos articulares ya
que para pasar del punto inicial de corte, al punto final, el extremo necesita acercarse al robot antes de volver a
alejarse continuando con su dirección.

Figura 6-4: Velocidad articular para: Izquierda interpolacion articular; derecha interpolacion carteciana

• En cuanto a las velocidades articulares vemos que la razón principal de la diferencia entre ambos casos yace en
el mismo motivo mencionado en el apartado anterior.
• En el caso de la trayectoria lineal es apreciable como las articulaciones que afectan a la posición del extremo son
las que trabajan con un perfil de velocidad de mayor magnitud, mientras que para las articulaciones que afectan
su orientación, es más leve.

Pág. 28 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 6-5: Aceleracion articular para: Izquierda interpolacion articular; derecha interpolacion cartesiana

• Del análisis de las aceleraciones articulares se comprende que la trayectoria lineal es mucho más exigente para
los actuadores, imponiendo en ciertas secciones de la misma, variaciones muy rápidas en la magnitud de la
aceleración.

- Interpolación articular e Interpolación Cartesiana en el Espacio de Trabajo

Figura 6-6: Posición cartesiana para: Izquierda interpolacion articular; derecha interpolacion cartesiana

• Podemos observar cómo, al trabajar en coordenadas cartesianas, e interpolando directamente sobre puntos del
espacio de trabajo, se consigue una trayectoria suave y precisa según lo definido en los requisitos de la
aplicación.

Pág. 29 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Figura 6-7: Posicion cartesiana X,Y para: Izquierda interpolacion articular; derecha interpolacion cartesiana

• Del análisis de posiciones del extremo en los ejes x vs y, se desprende una de las razones de lo que será nuestra
elección final, ello por el hecho de que la trayectoria en línea recta para nuestro espacio de trabajo implica
variaciones lineales de uno respecto al otro.

Figura 6-8: velocidad cartesiana para: Izquierda interpolacion articular; derecha interpolacion cartesiana

Figura 6-9: Aceleracion cartesiana para: Izquierda interpolacion articular; derecha interpolacion cartesiana

Pág. 30 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

• Analizando las velocidades y aceleraciones, vemos que, si bien la trayectoria curva presenta variaciones más
suaves, es la trayectoria recta la que se adapta a la tarea especificada, y por ello resulta importante prestarle
atención a los perfiles de velocidad y aceleración expuestos, típicos de la interpolación lineal con ajuste
parabólico empleada. Deberán tenerse en cuenta dichas variaciones rápidas de velocidad y aceleración en la
planificación de la tarea.

Selección de solución óptima para la tarea solicitada de corte.

Según lo analizado, se evidencia que la solución óptima es la que nos permite generar una trayectoria recta. Esta misma
está ligada a la solución que se genera mediante interpolación en el espacio cartesiano, con la técnica de interpolación lineal
con ajuste parabólico. Utilizando la ventaja de que la trayectoria final es recta entre ambos puntos, justo lo que necesitamos
en nuestro caso.
No obstante, se debe prestar atención a las gráficas de velocidades y aceleraciones, para generar una trayectoria en donde
dichos parámetros puedan ser resueltas por el robot y sus características de velocidades y aceleraciones máximas. En la
aplicación en cuestión dicho aspecto están pendiente de una evaluación minuciosa, pues los aspectos finales dependen del
equipo utilizado para el corte por plasma, equipo que aún no se ha definido, pero se tiene conocimiento de que suelen ser
velocidades más bajas de aquellos límites alcanzables por el robot.
En la etapa actual de desarrollo se plantea el uso de una máquina de corte por plasma, que puede trabajar con las siguientes
velocidades:

Figura 6-10: Velocidades maximas de corte para acero al carbono, manual plasma hypertherm

En el caso de la máxima velocidad de corte admisible para la herramienta, las velocidades cartesianas de traslación de la
herramienta no superarían los 0.093 m/s, valor que está notablemente por debajo de los calculados en la simulación a través
de los algoritmos de Peter Corke para la trayectoria solicitada.
Por ello se concluye que es muy factible la programación y ejecución de la trayectoria planificada con interpolación en
el espacio cartesiano utilizando un perfil de velocidad trapezoidal.

Pág. 31 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

7. Sensores y actuadores

Esta sección, se desglosa en tres grandes módulos, donde cada uno se describe desde las características de Sensores,
Actuadores y Fuente de alimentación, para su mejor comprensión. Encontrando dentro del módulo de Robot, que muchas
de estas características son extraídas de los manuales oficiales de los fabricantes. Se aclara que el segundo módulo de
Acoplamiento, abarca el acoplamiento entre el módulo de Robot y el tercer módulo de Entorno de Trabajo. En este último
módulo de Entorno de Trabajo, encontramos las características mencionadas que son externas al robot y al sistema de acople,
necesarias para la aplicación.

Módulo de Robot:

(El robot incorpora un sistema de control tipo RJ2: A05B-2350-B003, con cabina modelo B y tablero de operador)

Figura 7-1: Imagen 1)Cabina controlador RJ2 con placa driver de potencia. 2)Tablero de Operador. 3) Modulo I/O dentro del tablero de operador

• Sensores:
- Encoder: Todos los motores se proveen con encoders incorporados de la familia Alpha A64i, los
cuales son de 65536 pulsos por vuelta (360°), y pueden configurarse por software como absolutos o
incrementales. Cada modelo de dicha familia solo varía de acuerdo a las características físicas y
mecánicas del motor al que se acopla, pero mantienen la resolución.
- Tablero de Operador del sistema RJ2 con indicadores, conexión I/O, etc

• Actuadores:
- Motores 1,2,3: modelo A06B--0147--B675, tipo paso a paso, velocidad máxima 2000 rpm
- Motores 4: modelo A06B--0146--B675, tipo paso a paso, velocidad máxima 1500 rpm
- Motores 5: modelo A06B--0142--B675, tipo paso a paso, velocidad máxima 2000 rpm
- Motores 6: modelo A06B--0162--B675, tipo paso a paso, velocidad máxima 3000 rpm

- Todos los motores vienen con un sistema de frenos electromecánicos incorporado en la carcasa del
motor. Que actúan en caso de emergencia o ante cualquier parada del motor. Estos mismos son del
tipo normalmente accionados, lo que les da mayor confiabilidad ante alguna falla al robot.

- Todos los reductores nombrados a continuación son reductores planetarios, que además a parte de
estos reductores, incorporan un engrane para que el eje del motor trasmita al eje de entrada de los
reductores. La relación de reducción total de este sistema no se provee en el manual, solo el código
de producto, sin embargo, obtuvimos la misma del conocimiento de las velocidades nominales de los
motores y de las velocidades máximas de las articulaciones del robot (esto último si se presenta en el
manual). Obtuvimos una relación que se adecua también a nuestra aplicación de:
M1(A97L--0118--0939#250C—35)= 120:1, M2(A97L--0118--0940#250A—118)= 110:1,
M3(A97L--0118--0941#250A—129)= 120:1, M4(A97L--0118--0936#70C—36)= 43:1, M5(Esta
reducción se logra por medio del tren de engranajes que incorpora dicha articulación) =80:1,
M6(A97L--0118--0942#30A—81)= 86:1

- Según las características cinemáticas de los motores y las reducciones mencionados. Se encuentra
que la velocidad final obtenida en cada articulación, responde a las solicitaciones en velocidad de las
trayectorias rectas necesarias en el punto central de la herramienta (TCP). Cabe destacar que el
sistema de corte por plasma, utiliza en su trayectoria una velocidad máxima de corte (continuas, de
aceleración = 0): 760mm/min =0,0127 m/s. Comparando esta solicitación máxima de corte, respecto

Pág. 32 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

a las obtenidas en el informe de trayectorias, en la sección TF 4.2 , que obtuvimos una velocidad
máxima del TCP, en trayectoria recta con interpolación cartesiana de valor: 5 m/s. Utilizando estos
dos parámetros, se evidencia que el robot se encuentra sobredimensionado para la aplicación desde
la perspectiva cinemática. Dado que la empresa contaba con el mismos, se le dio utilidad.

• Fuente de alimentación:
- Alimentación del controlador RJ2: Transformador trifásico de 380v, +10% -15%, 50/60q1 Hz,
power 12kVA. Potencia media 2.5kW
- Drivers motores (A06B–6076–H001): El sistema RJ2 provee una placa de potencia driver, para el
control de los motores, esta es capaz de controlar 6 motores, pero la CPU principal, es capas de
manipular 16 motores en total. Contiene un display indicador de errores. Este mismo se alimenta
con 220v CA, 50/60Hz, con una terminal de conexión a una resistencia regenerativa.

Módulo de Acoplamiento:

• Sensores: Modulo I/O tipo “Model B I/O Fanuc”. Con su respectivo expansor, para obtener 16 I/O.

• Actuadores: Sistema de protección mediante relés, para las I/O, así poder aislar eléctricamente y proteger las
instalaciones.

• Fuente de alimentación: Las alimentaciones son extraídas de la misma fuente que otorga el controlador RJ2.

Módulo de Entorno de Trabajo:

• Sensores:
- Sistema de control de altura THC (Torch Height Control), que controlara independientemente a la
trocha, para mantener una distancia continua al corte, podemos encontrar, 2 fines de carrera N/A
superior e inferior, fin de carrera N/A anticolisión. Placa adaptadora driver de voltaje, esta captura la
tención de la máquina de plasma, la escala con una reducción de 50:1, que luego ingresa a la unidad
CPU del THC.

• Actuadores:
- Sistema de control de altura: Motor CC, con su respectivo driver incluido en la CPU del THC, que
otorga el fabricante del mismo. Interfaz HMI display con botoneras para la calibración e interacción
del THC.
- Tablero de control: botón de emergencia de parada (tipo industrial), llave de potencia para
alimentación trifásica 380v 25A.
- Maquina Plasma: Hypertherm Powermax 105, adecuada para el espesor solicitado de corte 25mm.
(incluye compresor).

• Fuente de alimentación:
- Tablero de control que al mismo tiempo alimenta al THC: se utilizará una fuente de 380v input, con
output 24v de 10A. Esto a su vez, se alimenta de la red 380v trifásica, 50/60 Hz
- Maquina Plasma: 380v trifásica, 50/60 Hz

Pág. 33 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Se presenta un primer croquis del diagrama general eléctrico genérico, que comprende alguno de estos módulos
mencionados, con las características presentadas

Figura 7-2: Diagrama conexion electria/ electornica sistema de corte por plasma

Pág. 34 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

C. APORTE
Se presenta dos aportes, relacionado con la sección 2-Convención Denavit – Hartenberg y 6-Trayectorias.

I. Luego de generar la convención, se procedió a corroborar mediante el robot real el ofsset del mismo, mediante
el terminal de enseñanza teach pendant. Mediante la observación en cada articulación, se encontraron las marcas
de fábrica para lograr posicionar todos los ejes a 0º. De esta manera se logró crear el vector de offset necesario
para que el robot parta de la posición de medición adecuada.

Se visualiza la información en el Teach Pendant verificando que hemos colocado todas las articulaciones en el cero
articular

Se visualizan las marcas que posee el robot en su estructura, de fábrica, para identificar visualmente los “ceros
articulares”.

Pág. 35 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Finalmente se visualizan el robot en posición de offset

II. Por otro lado, se presenta el mismo programa ejecutado en la simulación, pero con el robot real. Se aclara que
todavía no se pudo generar la herramienta para la tarea, pero se ha colocado un barilla sobre el soporte, para
simular la torcha de corte. Encontrando resultados satisfactorios para poder lograr la trayectoria con la
orientación necesaria perpendicular a la viga en todo momento. Se adjunta un video a este informe, mostrando
la trayectoria programada.

Pág. 36 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

D. CONCLUSIONES

Se evidencia a lo largo del informe el uso de una ¨cadena sistemática de razonamientos¨, con la cual se logró profundizar
y aplicar todos los temas estudiados en la catedra. Esta perspectiva de razonamiento introdujo a los alumnos, una metodología
de trabajo y estudio valiosas para el futuro ingeniero.
Gracias a esto se alcanzaron los objetivos planteados. Dentro de ellos se logró desarrollar el modelado mediante la
convención de Denavit- Hartenberg para la descomposición en los robots 1 y 2, utilizada para el desarrollo de algoritmos
que nos permitió calcular y simular la cinemática directa e inversa. Siendo validadas y corroboradas mediante herramientas
matemáticas y de programación. Posteriormente se lograron analizar las singularidades del robot mediante el desarrollo del
Jacobiano y sus parámetros característicos. Encontrando en esta las articulaciones problemáticas para posibles puntos del
robot. Como último objetivo, se logró planificar, generar y simular la trayectoria planteada, generando un análisis sobre el
robot real y posibles tareas del robot enmarcadas en la aplicación mencionada. También se hizo un análisis de velocidades
y aceleraciones tanto en el espacio articular como en el espacio cartesiano, dando resultados satisfactorios. Por último, se
presentó una sección con sensores y actuadores sugeridos para el sistema completo, el cual están por ser aplicados en la
aplicación real.
Al haber aplicado estas técnicas a la problemática desarrollada, se obtuvieron resultados destacables en las simulaciones
del modelaje cinemático. Y con ellas se lograron contrastar los conocimientos teóricos/prácticos adquiridos y además prever
posibles errores al momento de implementar la aplicación real.
No obstante, el trabajo realizado evidencia la posibilidad de introducir otras mejoras al modelaje y conocimiento
aplicado, para obtener resultados más precisos y próximos a la realidad, según las necesidades de la aplicación.
Para concluir, como agradecimiento a los múltiples autores citados en este trabajo, compartimos la siguiente frase de un
referente de esta hermosa área, que es la Robótica, Automatización y Control.

“All models are wrong but some are useful.”

George Box, Robustness in the strategy of scientific model building, in Launer, R. L; Wilkinson, G. N.,Robustness in Statis

Pág. 37 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

E. BIBLIOGRAFÍA

[1] FANUC, «S-420i_Data Sheet».

[2] FANUC, «S-420i_Maintenance-Installation».
[3] E. S. Carolina Diaz, «Apuntes de catedra Robotica 1 (2020) - Unidades: U1.U2,U3,U4,U5,U6,U7,» Mendoza -
Argentina.
[4] P. Corke, «Robotics Toolbox por MATLAB (Release 9),» 2013.
[5] S. H. a. M. V. Mark W. Spong, Robot Dynamics and Control (Second Edition), 2004.
[6] P. Corke, Robotics Vision & Control v2 - Corke.
[7] FANUC, «SYSTEM_R-J2_Controller_Maintenance».

*Los manuales del robot, se encuentra adjuntos a este informe.

Pág. 38 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

F. ANEXOS

- ANEXOS CINEMATICA INVERSA

Primer problema de Pieper:

Pág. 39 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Pág. 40 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Segundo problema de Piper, cálculo de la orientación, planteamos:

(𝑞4 , 𝑞5 , 𝑞6 ) = 𝑓2 (, 𝑞1 , 𝑞2 , 𝑞3 , 0𝑇6 ) (Ec.F-1)

Código utilizado, que NO se obtuvieron resultados satisfactorios:

%Matriz de D-H simbólica
syms q1 q2 q3 q4 q5 q6 double
syms d1 d2 d3 d4 d5 d6 double
syms a1 a2 a3 a4 a5 a6 double
syms alf1 alf2 alf3 alf4 alf5 alf6 double
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 double

d1=0.735; d4=1.3; d6=0.2;

a1=0.18; a2=0.95; a3=0.24;
%alf1=-pi/2; alf2=0; alf3=-pi/2; alf4=pi/2; alf5=-pi/2; alf6=0;
alf1=-90; alf2=0; alf3=-90; alf4=90; alf5=-90; alf6=0;

Pág. 41 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

dh = [0,d1,a1,alf1,0;
0, 0,a2, 0,0;
0, 0,a3,alf3,0;
0,d4, 0,alf4,0;
0, 0, 0,alf5,0;
0,d6, 0, 0,0];
%
R = SerialLink(dh);
fprintf('Robot:\n')
[Link]=eye(4,4);
[Link]=eye(4,4);
R
[Link]()
[Link]

Rzt1=[cos(q1), -sin(q1), 0;
sin(q1), cos(q1), 0;
0, 0, 1];
Rxa1=[1, 0, 0;
0, cosd(alf1), -sind(alf1);
0, sind(alf1), cosd(alf1)];
Rzt2=[cos(q2), -sin(q2), 0;
sin(q2), cos(q2), 0;
0, 0, 1];
Rxa2=[1, 0, 0;
0, cosd(alf2), -sind(alf2);
0, sind(alf2), cosd(alf2)];
Rzt3=[cos(q3), -sin(q3), 0;
sin(q3), cos(q3), 0;
0, 0, 1];
Rxa3=[1, 0, 0;
0, cosd(alf3), -sind(alf3);
0, sind(alf3), cosd(alf3)];

R03 = Rzt1*Rxa1*Rzt2*Rxa2*Rzt3*Rxa3

Rzt4=[cos(q4), -sin(q4), 0;
sin(q4), cos(q4), 0;
0, 0, 1];
Rxa4=[1, 0, 0;
0, cosd(alf4), -sind(alf4);
0, sind(alf4), cosd(alf4)];
Rzt5=[cos(q5), -sin(q5), 0;
sin(q5), cos(q5), 0;
0, 0, 1];
Rxa5=[1, 0, 0;
0, cosd(alf5), -sind(alf5);
0, sind(alf5), cosd(alf5)];
Rzt6=[cos(q6), -sin(q6), 0;
sin(q6), cos(q6), 0;
0, 0, 1];
Rxa6=[1, 0, 0;
0, cosd(alf6), -sind(alf6);
0, sind(alf6), cosd(alf6)];

R36 = Rzt4*Rxa4*Rzt5*Rxa5*Rzt6*Rxa6

Pág. 42 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

R06 = [r11, r12, r13;

r21, r22, r23
r31, r32, r33];

Resu_izq = ((R03')*R06)

ecuq5 = Resu_izq(3,3) == R36(3,3);

q5s=solve(ecuq5,q5);
fprintf('Primer solución para q5:\n')
q5s(1,1)
%q5(1,1) = acos(- T60(1,3)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) -
T60(2,3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) -
T60(3,3)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))
fprintf('Segunda solución para q5:\n')
q5s(2,1)
%q5(2,1) = -acos(- T60(1,3)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) -
T60(2,3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) -
T60(3,3)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))

ecuq4 = Resu_izq(1,3) == R36(1,3);

q4s=solve(ecuq4,q4);
fprintf('Primer solución para q4:\n')
q4s(1,1)
%q4(1,1) = pi - acos((T60(1,3)*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3)) + T60(2,3)*(cos(q2)*cos(q3)*sin(q1) -
sin(q1)*sin(q2)*sin(q3)) - T60(3,3)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)))/sin(q5))
fprintf('Segunda solución para q4:\n')
q4s(2,1)
%q4(2,1) = pi + acos((T60(1,3)*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3)) + T60(2,3)*(cos(q2)*cos(q3)*sin(q1) -
sin(q1)*sin(q2)*sin(q3)) - T60(3,3)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)))/sin(q5))

ecuq6 = Resu_izq(3,1) == R36(3,1);

q6s=solve(ecuq6,q6);
fprintf('Primer solución para q6:\n')
q6s(1,1)
%q6(1,1) = pi - acos((T60(1,1)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2)) + T60(2,1)*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)) + T60(3,1)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/sin(q5))
fprintf('Segunda solución para q6:\n')
q6s(2,1)
%q6(2,1) = pi + acos((T60(1,1)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2)) + T60(2,1)*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)) + T60(3,1)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/sin(q5))

%__________Caso particular (q5=0)_____________

%Tomo los terminos 11 de R36 y de Resu_izq

% cos(q4)*cos(q5)*cos(q6) - sin(q4)*sin(q6) = ...

% r11*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) ...
% + r21*(cos(q2)*cos(q3)*sin(q1) - sin(q1)*sin(q2)*sin(q3)) ...
% - r31*(cos(q2)*sin(q3) + cos(q3)*sin(q2))

%Puedo considerar cos(q5) = 1 porque siempre q = 0, y me queda:

% cos(q4 + q6) = ...

% r11*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) ...

Pág. 43 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

% + r21*(cos(q2)*cos(q3)*sin(q1) - sin(q1)*sin(q2)*sin(q3)) ...

% - r31*(cos(q2)*sin(q3) + cos(q3)*sin(q2))

%Y despejando q6, me qeda:

% q6 = -q4 + acos(r11*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) ...

% + r21*(cos(q2)*cos(q3)*sin(q1) - sin(q1)*sin(q2)*sin(q3)) ...
% - r31*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) ...

Pág. 44 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código principal
%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%============ RECOPLILACION DE CODIGOS PARA PROYECTO FINAL ===============%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%
%%
clear all
close all
clc
%=========================================================================%
fprintf('######################################################\n')
fprintf('# Recopilacion de códigos #\n')
fprintf('######################################################\n\n')
fprintf('Seleccione el n° de una de las opciones: \n')
fprintf('1 : Denavit-Hartenberg \n')
fprintf('2 : Cinemática Directa \n')
fprintf('3 : Cinemática Inversa \n')
fprintf('4 : Jacobiano \n')
fprintf('5 : Planif. y Gen. de Trayectorias \n')
OPT = input('\nOpción: ');
%=========================================================================%
switch OPT
case 1
fprintf('> Denavit-Hartenberg\n\n')
run('DenavitHartenberg')
case 2
fprintf('> Cinemática Directa\n\n')
run('CinematicaDirecta')
case 3
fprintf('> Cinemática Inversa\n\n')
run('CinematicaInversa')
case 4
fprintf('> Jacobiano\n\n')
run('Jacobiano')
case 5
fprintf('> Planif. y Gen. de Trayectorias\n\n')
run('Trayectoria')
otherwise
fprintf('> Opcion no definida\n\n')
end
%=========================================================================%
fprintf('\n######################################################\n')
fprintf('# Fin de Recopilacion de códigos #\n')
fprintf('######################################################\n')
%=========================================================================%

Pág. 45 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código de Denavit-Hartenberg

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%=========== Código para Trabajo Práctico de Denavit-Hartenberg ==========%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%
%%
clear all
close all
clc
%%

%-------------------------------------------------ROBOT 1
figure (1)
dh = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.240 -pi/2 0;
0.000 1.300 0.000 pi/2 0;
0.000 0.000 0.000 -pi/2 0;
0.000 0.200 0.000 0.000 0];

R = SerialLink(dh,'name','Fanuc S-420iF ROBOT 1');

%Vector q0
q = [0,0,0,0,0,0];

%Definicion de limites articulares ROBOT 1

[Link](1,1:2) = [-187, 187]*pi/180;

[Link](2,1:2) = [-76 , 77.8]*pi/180;
[Link](3,1:2) = [-33, 85]*pi/180;
[Link](4,1:2) = [-310, 310]*pi/180;
[Link](5,1:2) = [-134, 134]*pi/180;
[Link](6,1:2) = [-360, 360]*pi/180;

[Link] = eye(4,4); %Matriz base si quiero mover base de

robot 1
[Link] = eye(4,4); %Matriz herramienta
[Link] = [0 -90 0 0 0 180]*pi/180;

%R.PLOT3D)Q,PATH,PWD= LE DIGO QUE BUSQUE A ESA DIRECCION LOS MODLEOS 3D DEL

%ROBOT
[Link](q,'workspace',[-4 4 -4 4 -0 4])
%[Link](q)

%-------------------------------------------------ROBOT 2
figure (2)
dh2 = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.600 0.000 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.600 0.000 0];

Pág. 46 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

R2 = SerialLink(dh2,'name','Fanuc S-420iF ROBOT 2');

q2 = [0,0,0,0];

%Definicion de limites articulares ROBOT 2

[Link](1,1:2) = [-187, 187]*pi/180;

[Link](2,1:2) = [-33 , 85]*pi/180;
[Link](3,1:2) = [-111, 50]*pi/180;
[Link](4,1:2) = [-310, 310]*pi/180;

[Link] = eye(4,4); %Matriz base si quiero mover base robot

2
[Link] = eye(4,4); %Matriz herramienta no tiene
[Link] = [0 180 90 90]*pi/180;

[Link](q2,'workspace',[-4 4 -4 4 -0 4])
[Link](q2)

Pág. 47 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código de Cinemática Directa

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%=========== Código para Trabajo Práctico de Denavit-Hartenberg ==========%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%
%%
clear all
close all
clc
%%

%-------------------------------------------------ROBOT 1
figure (1)
dh = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.240 -pi/2 0;
0.000 1.300 0.000 pi/2 0;
0.000 0.000 0.000 -pi/2 0;
0.000 0.200 0.000 0.000 0];

R = SerialLink(dh,'name','Fanuc S-420iF ROBOT 1');

%Vector q0
q = [0,0,0,0,0,0];

%Definicion de limites articulares ROBOT 1

[Link](1,1:2) = [-187, 187]*pi/180;

[Link](2,1:2) = [-76 , 77.8]*pi/180;
[Link](3,1:2) = [-33, 85]*pi/180;
[Link](4,1:2) = [-310, 310]*pi/180;
[Link](5,1:2) = [-134, 134]*pi/180;
[Link](6,1:2) = [-360, 360]*pi/180;

[Link] = eye(4,4); %Matriz base si quiero mover base de

robot 1
[Link] = eye(4,4); %Matriz herramienta
[Link] = [0 -90 0 0 0 180]*pi/180;

%R.PLOT3D)Q,PATH,PWD= LE DIGO QUE BUSQUE A ESA DIRECCION LOS MODLEOS 3D DEL

%ROBOT
[Link](q,'workspace',[-4 4 -4 4 -0 4])
%[Link](q)

%-------------------------------------------------ROBOT 2
figure (2)
dh2 = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.600 0.000 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.600 0.000 0];

Pág. 48 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

R2 = SerialLink(dh2,'name','Fanuc S-420iF ROBOT 2');

q2 = [0,0,0,0];

%Definicion de limites articulares ROBOT 2

[Link](1,1:2) = [-187, 187]*pi/180;

[Link](2,1:2) = [-33 , 85]*pi/180;
[Link](3,1:2) = [-111, 50]*pi/180;
[Link](4,1:2) = [-310, 310]*pi/180;

[Link] = eye(4,4); %Matriz base si quiero mover base robot

2
[Link] = eye(4,4); %Matriz herramienta no tiene
[Link] = [0 180 90 90]*pi/180;

[Link](q2,'workspace',[-4 4 -4 4 -0 4])
[Link](q2)

Pág. 49 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código de función de Cinemática Directa para Robot 1

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%==================== FUNCION DE CINEMATICA DIRECTA ======================%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%%

function [T0,flag_limt] = cinemDirecta(R,q)

%Calculo de las matrices de cinematica directa de cada articulacion
%Elemental: R(z,tita)*T(z,d)*T(x,a)*R(x,alfa)
T1= trotz (q(1)+[Link](1)) * transl(0,0,[Link](1).d) *
transl([Link](1).a,0,0) * trotx ([Link](1).alpha);
T2= trotz (q(2)+[Link](2)) * transl(0,0,[Link](2).d) *
transl([Link](2).a,0,0) * trotx ([Link](2).alpha);
T3= trotz (q(3)+[Link](3)) * transl(0,0,[Link](3).d) *
transl([Link](3).a,0,0) * trotx ([Link](3).alpha);
T4= trotz (q(4)+[Link](4)) * transl(0,0,[Link](4).d) *
transl([Link](4).a,0,0) * trotx ([Link](4).alpha);
T5= trotz (q(5)+[Link](5)) * transl(0,0,[Link](5).d) *
transl([Link](5).a,0,0) * trotx ([Link](5).alpha);
T6= trotz (q(6)+[Link](6)) * transl(0,0,[Link](6).d) *
transl([Link](6).a,0,0) * trotx ([Link](6).alpha);

Base = [Link];
Tool = [Link];
%Calculo de la matriz de cinematica directa de todo el robot
T0 = Base*T1*T2*T3*T4*T5*T6*Tool;

flag_limt = true;
for j = 1:length(q)
% Rercorro los limites para corroborar que los q no esten fuera!!
if (q(j) < [Link](j,1) || q(j)> [Link](j,2)) % Con esta funcion se definen
los limetes positivos y negativos.
q(j) = sign(q(j)) * abs([Link](j,1));

flag_limt = false;
end
end

end

Pág. 50 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código de función de Cinemática Directa para Robot 2

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%==================== FUNCION DE CINEMATICA DIRECTA ======================%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%%

function [T0,flag_limt] = cinemDirectaR2(R,q)

%Calculo de las matrices de cinematica directa de cada articulacion
%Elemental: R(z,tita)*T(z,d)*T(x,a)*R(x,alfa)
T1= trotz (q(1)+[Link](1)) * transl(0,0,[Link](1).d) *
transl([Link](1).a,0,0) * trotx ([Link](1).alpha);
T2= trotz (q(2)+[Link](2)) * transl(0,0,[Link](2).d) *
transl([Link](2).a,0,0) * trotx ([Link](2).alpha);
T3= trotz (q(3)+[Link](3)) * transl(0,0,[Link](3).d) *
transl([Link](3).a,0,0) * trotx ([Link](3).alpha);
T4= trotz (q(4)+[Link](4)) * transl(0,0,[Link](4).d) *
transl([Link](4).a,0,0) * trotx ([Link](4).alpha);

Base = [Link];
%Calculo de la matriz de cinematica directa de todo el robot
T0 = Base*T1*T2*T3*T4;

flag_limt = true;
for j = 1:length(q)
% Rercorro los limites para corroborar que los q no esten fuera!!
if (q(j) < [Link](j,1) || q(j)> [Link](j,2)) % Con esta funcion se definen
los limetes positivos y negativos.
q(j) = sign(q(j)) * abs([Link](j,1));
flag_limt = false;
end
end

end

Pág. 51 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código de Cinemática Inversa

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%========= Código para Trabajo Práctico de Cinemática Inversa ============%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%
%%
clear all
close all
clc
%%
%======================= ¡¡¡¡¡¡ IMPORTANTE !!!!!! =========================
%==== Antes de ejecutar el programa lea la seccion INGRESO DEL PUNDO
%DESEADO EN EL ESPACIO DE TRABAJO. Existen 2 maneras de hacerlo.
%============================================================================
%%
%%
%Matriz de D-H ROBOT 1
dh = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.240 -pi/2 0;
0.000 1.300 0.000 pi/2 0;
0.000 0.000 0.000 -pi/2 0;
0.000 0.200 0.000 0.000 0];

%Creo el objeto SerialLink

R = SerialLink(dh,'name',' ');
[Link] = [0 -90 0 0 0 180]*pi/180;
[Link] = eye(4,4); %Matriz base si quiero mover base de
robot 1
[Link] = eye(4,4); %Matriz herramienta

%Definicion de limites articulares

[Link](1,1:2) = [-187, 187]*pi/180;
[Link](2,1:2) = [-76 , 77.8]*pi/180;
[Link](3,1:2) = [-33, 85]*pi/180;
[Link](4,1:2) = [-310, 310]*pi/180;
[Link](5,1:2) = [-134, 134]*pi/180;
[Link](6,1:2) = [-360, 360]*pi/180;
%ROBOT 2
dh2 = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.600 0.000 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.600 0.000 0];

%Creo el objeto SerialLink

R2 = SerialLink(dh2,'name',' ');
[Link] = [0 180 90 90]*pi/180;
[Link] = eye(4,4); %Matriz base si quiero mover base robot
2
[Link] = eye(4,4); %Matriz herramienta no tiene

%Definicion de limites articulares ROBOT 2, REALMENTE NO ES NECESARIO EL 1 YA LO

%LIMITA

Pág. 52 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

% [Link](1,1:2) = [-187, 187]*pi/180;

% [Link](2,1:2) = [-33 , 85]*pi/180;
% [Link](3,1:2) = [-111, 50]*pi/180;
% [Link](4,1:2) = [-310, 310]*pi/180;
%%
%============================================================================
%Seteo de cordenadas articulares inicial
%load('qactual','q0'); %Vector de cordenadas iniciales/actuales
q01=[0,0,0,0,0,0];
q02=[0,0,0,0];
fprintf('Coordenadas articulares iniciales \n');
disp((q01*180/pi));

%%
%Corro un bucle para generar 30 puntos
for i=1:1
fprintf('Presione 2 veces enter para salir \n');
%INGRESO DEL PUNDO DESEADO EN EL ESPACIO DE TRABAJO

%OPCION 1, ingreso como matriz homogena

% fprintf('----------------------------------------------------')
%fprintf('----- FORMATO DE INGRESO DE PUNTO DESEADO:\n')
%fprintf('----- OPCION 1\n')
% T60 = input('\nPunto deseado como matriz homogenea:');
% fprintf('----------------------------------------------------')
% fprintf('\n Presione Enter para continuar.\n')
% pause
% % ejemplo de matris a ingresa
% % T60=[
% % 0 0 1.0000 1.6800
% % 0 -1.0000 0 0
% % 1.0000 0 0 2.1900
% % 0 0 0 1.0000];
% %Rot (x y z) Tras(x,y,z)

%OPCION 2 mas facil, genero la matriz con C.D.

%DESCOMENTAR Y COMENTAR OPCION 1 "Ejemplo a ingresar q60
%en grados [0,20,-30,0,50,120][0,0,0,0,0,0]
fprintf('----------------------------------------------------\n')
fprintf('----- FORMATO DE INGRESO DE PUNTO DESEADO:\n')
fprintf('----- OPCION 2\n')
q60 = input('\n Escriba nuevo punto deseado como valor articular en Grados:');
fprintf('----------------------------------------------------')
fprintf('\n Presione Enter para continuar.\n')
pause
q60 = q60*pi/180;
[T60]= cinemDirecta(R,q60);
%T60=T60+3 % Puedo sumarle un valor para sacarlo del espacio de trabajo
fprintf('Matriz homogenea del punto deseado \n');
disp(T60);

%%
%============================================================================
%Calculo de Vector de coordenadas con Cinematica Inversa
[qsol1,flag2,dc0]= cinemInversaR1(R,q01,T60);
[qsol2]= cinemInversaR2(R2,qsol1);
fprintf('\nVector solucion R1\n')
disp(qsol1*(180/pi));
fprintf('\nVector solucion R2\n')

Pág. 53 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

disp(qsol2*(180/pi));

%%
%============================================================================
%Grafico el Robot 1
%Grafico el Robot 1
qtraj = jtraj(q01, qsol1, 20);
qtraj2 = jtraj(q02, qsol2, 20);
figure(1)
i=1;

for qtrayectoria=qtraj'

qtrayectoria2=qtraj2(i,:);
[Link](qtrayectoria','workspace',[-4 4 -4 4 -0 4],'trail','r');% %linea
hold on
[Link](qtrayectoria2,'workspace',[-4 4 -4 4 -0 4]);
%[Link]

% plot3(dc0(1),dc0(2),dc0(3),'b*');
% text(dc0(1)+0.15,dc0(2),dc0(3)+0.3,' dc');
% % plot3(dc0(1),dc0(2),0+0.05,'r*');
% % text(dc0(1),dc0(2),0+0.1,' Xc,Yc');
% % plot3([0,dc0(1)],[0,dc0(2)],[0,0],'k');
% % plot3([dc0(1),dc0(1)],[dc0(2),dc0(2)],[0,dc0(3)],'k');
% plot3([0,dc0(1)],[0,dc0(2)],[0,dc0(3)],'k');
title('Fanuc S-420iF')
legend({'Fanuc S-420iF ROBOT 1 y 2'},'Location','southwest')
i=i+1;
end
q01=qsol1'; %actualizo vector de coordenadas articulares "actuales" robot 1
q02=qsol2; %actualizo vector de coordenadas articulares "actuales" robot2
end

Pág. 54 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código de función de Cinemática Inversa para Robot 1

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%================ FUNCION DE CINEMATICA INVERSA ROBOT 1 ==================%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%%
%Calcula en funcion de los parametros del robot y la posicion inicial o
%anterior, los valores articualres q y una bandera que si es exacta la
%solucion es verdadero, en caso contrario devuelve en q la posicion
%articular mas cercnla posible de generar.
%El robot tendra 8 solucion. De estas 8, 4 son con el primer q1 y las otras 4 con
el otro

function [qsol,flag2,dc0] = cinemInversaR1(R,q01,T60)

q = zeros(6,8);

%%
%Piper Problema 1
%Busco el punto dc
dh=[[Link]', R.d', R.a', [Link]']; %Asingo DH a T
T60 = invHomog([Link]) * T60 * invHomog([Link]);%muevo el punto
deseado si setea una base y un tool difernete a la identidad
dc0 = T60(1:3,4) - dh(6,2) * T60(1:3,3); %Vector ingresado por nosotros
trasladado al S4

%Calculo q1
q1(1) = atan2(dc0(2), dc0(1)); % -pi >= atan2 <= pi
%ventaja de atan2 respecto de atan, es que la
primera da resultados correctos en los límites de los cuadrantes, específicamente
con la coordenada X=0

if q1(1) > 0 %Como el brazo podría pasar por encima del mismo robot y
trabajar "hacia atrás"
q1(2) = q1(1) - pi; %Existen 2 valores de q1 que pueden dejar el extremo en
el mismo lugar, el calculado y el opuesto a 180º
else
q1(2) = q1(1) + pi;
end

qq(1,1:4) = [1 1 1 1] * q1(1);
qq(1,5:8) = [1 1 1 1] * q1(2);

%%
%%Calulo de q2
%Calculo las matrices de transformacion posibles de 1 0 con los 2 valores
%de q1
T10(:,:,1)= trotz (q1(1)+[Link](1)) * transl(0,0,dh(1,2)) * transl(dh(1,3),0,0)
* trotx (dh(1,4));
T10(:,:,2)= trotz (q1(2)+[Link](1)) * transl(0,0,dh(1,2)) * transl(dh(1,3),0,0)
* trotx (dh(1,4));

%Traslado el punto base al sistema 1 , diferente a dc0, no estan

asociados
dc1 = [invHomog(T10(:,:,1))*[dc0;1] invHomog(T10(:,:,2))*[dc0;1]]; %Vector desde
el sistema S4 al S1
norm(dc1(1:3,1));

Pág. 55 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

norm(dc1(1:3,2));

Beta(1) = atan2(dc1(2,1),dc1(1,1));
Beta(2) = atan2(dc1(2,2),dc1(1,2));
Beta2 = (Beta(:)*(180/pi));

c(1)=sqrt(dc1(2,1)^2 + dc1(1,1)^2); % Distancia hipotenusa

c(2)=sqrt(dc1(2,2)^2 + dc1(1,2)^2);

L2 = dh(2, 3); % a2
L3 =sqrt(dh(4,2)^2 + dh(3,3)^2); % b3 En nuestro caso es diferente el triangulo

Gama(1) = acos((L2^2 + c(1)^2 - L3^2) / (2 * c(1) * L2)); %2 valores posibles

Gama(2) = -acos((L2^2 + c(1)^2 - L3^2) / (2 * c(1) * L2));
Gama(3) = acos((L2^2 + c(2)^2 - L3^2) / (2 * c(2) * L2)); %2 valores posibles
Gama(4) = -acos((L2^2 + c(2)^2 - L3^2) / (2 * c(2) * L2));
Gama2 = real(Gama*(180/pi));

q2(1)=Beta(1)-real(Gama(1));
q2(2)=Beta(1)-real(Gama(2));
q2(3)=Beta(2)-real(Gama(3));
q2(4)=Beta(2)-real(Gama(4));
q2 = q2+(pi/2);
qq(2,:) = [q2(1) q2(1) q2(2) q2(2) q2(3) q2(3) q2(4) q2(4)];

%%
% %Calculo q3
T21(:,:,1)= trotz (q2(1)+[Link](2)) * transl(0,0,dh(2,2)) * transl(dh(2,3),0,0)
* trotx (dh(2,4));
T21(:,:,2)= trotz (q2(2)+[Link](2)) * transl(0,0,dh(2,2)) * transl(dh(2,3),0,0)
* trotx (dh(2,4));
T21(:,:,3)= trotz (q2(3)+[Link](2)) * transl(0,0,dh(2,2)) * transl(dh(2,3),0,0)
* trotx (dh(2,4));
T21(:,:,4)= trotz (q2(4)+[Link](2)) * transl(0,0,dh(2,2)) * transl(dh(2,3),0,0)
* trotx (dh(2,4));

T20(:,:,1)=T10(:,:,1)*T21(:,:,1);
T20(:,:,2)=T10(:,:,1)*T21(:,:,2);
T20(:,:,3)=T10(:,:,2)*T21(:,:,3);
T20(:,:,4)=T10(:,:,2)*T21(:,:,4);

%Traslado el punto 1 al sistema 2 , diferente a dc1, no estan asociados

dc2 = [invHomog(T20(:,:,1))*[dc0;1] invHomog(T20(:,:,2))*[dc0;1]
invHomog(T20(:,:,3))*[dc0;1] invHomog(T20(:,:,4))*[dc0;1]]; %Vector desde el
esistema S4 al S1

q3(1,1) = atan2(dc2(2,1),dc2(1,1))-atan2(dh(4,2),dh(3,3));
q3(1,2) = atan2(dc2(2,2),dc2(1,2))-atan2(dh(4,2),dh(3,3));
q3(1,3) = atan2(dc2(2,3),dc2(1,3))-atan2(dh(4,2),dh(3,3));
q3(1,4) = atan2(dc2(2,4),dc2(1,4))-atan2(dh(4,2),dh(3,3));
qq(3,:) = [q3(1) q3(1) q3(2) q3(2) q3(3) q3(3) q3(4) q3(4)];
%qq1=qq*(180/pi);

%============================================================================
%%
%Piper Problema 2
%
T23(:,:,1)= trotz (qq(3,1)+[Link](3)) * transl(0,0,dh(3,2)) *
transl(dh(3,3),0,0) * trotx (dh(3,4));

Pág. 56 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

T23(:,:,2)= trotz (qq(3,3)+[Link](3)) * transl(0,0,dh(3,2)) *

transl(dh(3,3),0,0) * trotx (dh(3,4));
T23(:,:,3)= trotz (qq(3,5)+[Link](3)) * transl(0,0,dh(3,2)) *
transl(dh(3,3),0,0) * trotx (dh(3,4));
T23(:,:,4)= trotz (qq(3,7)+[Link](3)) * transl(0,0,dh(3,2)) *
transl(dh(3,3),0,0) * trotx (dh(3,4));

T36(:,:,1) = invHomog(T23(:,:,1)) * invHomog(T21(:,:,1)) * invHomog(T10(:,:,1)) *

T60;
T36(:,:,2) = invHomog(T23(:,:,2)) * invHomog(T21(:,:,2)) * invHomog(T10(:,:,1)) *
T60;
T36(:,:,3) = invHomog(T23(:,:,3)) * invHomog(T21(:,:,3)) * invHomog(T10(:,:,2)) *
T60;
T36(:,:,4) = invHomog(T23(:,:,4)) * invHomog(T21(:,:,4)) * invHomog(T10(:,:,2)) *
T60;

q4=zeros(4,2);
q5=zeros(4,2);
q6=zeros(4,2);
for j=1:4
if abs(T36(3,3,j) - 1) < eps
% solución degenerada:
% > z3 y z5 alineados (y z6)
% > q4 y q6 generan el mismo movimiento
% > q5 = 0 (o q5 = 180º)
% > se asume q4 = q0(1,4)
q4(j,1)=q01(1,4);
q5(j,1) = 0;
q6(j,1) = atan2(T36(2,1,j), T36(1,1,j)) - q4(j,1);
q4(j,2) = q4(j,1);
q5(j,2) = 0;
q6(j,2) = q6(j,1);

else
% solución normal:
q4(j,1) = atan2(-T36(2,3,j), -T36(1,3,j));
if q4(j,1) > 0, q4(j,2) = q4(j,1) - pi; else, q4(j,2) = q4(j,1) + pi; end
q5(j,:) = zeros(1,2);
q6(j,:) = q5(j,:);
for i=1:2
T34(:,:,i)= trotz (q4(j,i)+[Link](4)) * transl(0,0,dh(4,2)) *
transl(dh(4,3),0,0) * trotx (dh(4,4));
T46(:,:,i) = invHomog(T34(:,:,i)) * T36(:,:,j);
q5(j,i) = atan2(-T46(1,3,i), T46(2,3,i));
T45(:,:,i)= trotz (q5(j,i)+[Link](5)) * transl(0,0,dh(5,2)) *
transl(dh(5,3),0,0) * trotx (dh(5,4));
T56(:,:,i) = invHomog(T45(:,:,i)) * T46(:,:,i);
q6(j,i) = atan2(T56(2,1,i), T56(1,1,i));
end
end
end
qq(4,:)=[q4(1,1), q4(1,2), q4(2,1), q4(2,2), q4(3,1), q4(3,2), q4(4,1), q4(4,2)];
qq(5,:)=[q5(1,1), q5(1,2), q5(2,1), q5(2,2), q5(3,1), q5(3,2), q5(4,1), q5(4,2)];
qq(6,:)=[q6(1,1), q6(1,2), q6(2,1), q6(2,2), q6(3,1), q6(3,2), q6(4,1), q6(4,2)];
q=qq;
fprintf('Posibles soluciones en coordenadas articulares del punto deseado');
qq1=qq*(180/pi)

%%
%Corroboro si el punto esta fuera del espacio de trabajo

Pág. 57 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

%Verifico que gama no tenga parte imaginaria, en dicho caso es solución

%valida. Si da un valor imaginario, es cuando no hay solución sin importar
%si estamos en singularidad o no.

%========== El booleano resulta verdadero cuando: _Sol exacta y dentro

% del espacio de trabajo
%========== El booleano resulta falso cuando: _Sol inexacta y/o fuera del
% espacio de trabajo
% _
%========== Consideraciones para espacio de trabajo: _Limites articulares
% _Gama imaginario

flag11=[0,0,0,0];
for i=1:4
if(isreal(Gama(i))) %no tiene parte imaginaria
flag11(1,i)=1;
else
flag11(1,i)=0;
end
end
flag12=[flag11(1,1) flag11(1,1) flag11(1,2) flag11(1,2) flag11(1,3) flag11(1,3)
flag11(1,3) flag11(1,3)];
%%
%Selecciono la solucion mas cercana al vector de coordenadas articulares actual.
%descarto soluciones fuera del rango articular
for i=1:8
[flagLim]=qLimArt(R,q(:,i)); % calculo si esta dentro de los limites
articulares fisicos
for j=1:6
if(flagLim==true)
qaux(j,i)=abs(q01(j)-q(j,i));%Lleno un vector con el delta de cada
articulacion respecto a la coordenada actual. Para cada posible solucion lo hago
else
qaux(j,i)=1000; % Asigno valor grande y descarto esa
articulacion
end
end
end
for i=1:8 %Calculo la menor distancia que se tienen que mover las 6
articulaciones para 1 solucion dada.
qaux1(1,i)=norm(qaux(:,i));%calculo los modulos de todos los delta de cada
articulacion, para cada solucion de las 8
end

[M,I] = min(qaux1);%busco el minimo de ellos (su valor y su indice), este será el

mas cercano
%La solucion elegida será
qsol=q(:,I);
%%
%Delimito la bandera, en funcion de si tiene solucion en el espacio de
%trabajo matematico y fisico(limite articular)
[flagLim]=qLimArt(R,q(:,I)); %1) calculo si esta dentro de los
limites articulares fisicos
if(flag12(1,I)==1) %2) pregunto si es una solucion
dentro del espacio de trabajo matematico
if(flagLim==true) %3) Si ademas esta dentro del espacio
de trabajo fisico
flag2=true; %4) Solucion exacta, esta dentro del
espacio de trabajo matematico y fisico
disp("Solucion exacta y mas cercana, esta dentro del espacio de
trabajo matematico y fisico(articular)");

Pág. 58 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

else
for i=1:8
[flagLim]=qLimArt(R,q(:,i));
if(flagLim==true)
[Tci]= cinemDirecta(R,q(:,i));
Tci=round(Tci,3);
T60=round(T60,3); %arriesgo precision a costa de
encontrar una solucion por la capacidad de computo.
if(Tci==T60)
qsol=q(:,i);
end
end
end
flag2=true; %5) Por mas que este en el espacio de
trabajo matematico, no lo esta en el fisico
disp("Solucion exacta y No es la mas cercana, esta dentro del
espacio de trabajo matematico y fisico(articular)");
end
else %6) No esta en el espacio de trabajo
matematico, pero puede ser un punto de singularidad
if(flagLim==true) %7) Pero puede ademas estar dentro
del espacio de trabajo fisico
[Tci]= cinemDirecta(R,q(:,I));
if(Tci==T60) %8) Corroboro que este punto singular
sea exacta la solucion comparando las matrices homog del q calculado y la T06 del
punto deseado
flag2=true; %9) Solucion exacta, no esta en
espacio de trabajo matematico y si fisico (Singularidad)
disp("Solucion exacta y mas cercana, No esta dentro del espacio
de trabajo matematico, pero si fisico(articular) y coincide con C.D");
else
%%Recalculo el qsol por que el de la distancia mas
%%cercana no verifica espacio matematico ni coincide el
%%punto final
for i=1:8
[flagLim]=qLimArt(R,q(:,i));
if(flagLim==true)
[Tci]= cinemDirecta(R,q(:,i));
Tci=round(Tci,3);
T60=round(T60,3); %arriesgo precision a costa de
encontrar una solucion por la capacidad de computo.
if(Tci==T60)
qsol=q(:,i);
end
end
end
disp("Solucion Exacta y 'No es la mas cercana', esta dentro
del espacio de trabajo matematico, si fisico(articular)");
flag2=true;
end
else
flag2=false; %11)No esta en el espacio de trabajo
matematico,y no lo esta en el fisico
disp("Solucion No exacta y mas cercana, No esta dentro del
espacio de trabajo matematico, tampoco en el fisico(articular)");
end
end
end

Pág. 59 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código de función de Cinemática Inversa para Robot 2

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%================ FUNCION DE CINEMATICA INVERSA ROBOT 2 ==================%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%%
function [qsol2] = cinemInversaR2(R2,qsol1)
qsol2=zeros(4,1);

qsol2(1)=qsol1(1);
qsol2(2)=(qsol1(3)+qsol1(2));

qsol2(3)= -qsol1(3);
qsol2(4)= qsol1(3);

end

Pág. 60 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código de función para realizar la inversa de la matriz de transformación homogénea

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%============= Función para el cálculo de la inversa de T ================%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%
%%
function iT = invHomog(T)
iT = eye(4);
iT(1:3, 1:3) = T(1:3, 1:3)';
iT(1:3, 4) = - iT(1:3, 1:3) * T(1:3, 4);
end

Código de función para realizar la verificación de límites articulares

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%=========== Funcion para verificación de límites articulares ============%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%
%%
function flag1= qLimArt(R,q)
flag1 = true;
for j = 1:length(q)
% Rercorro los limites para corroborar que los q no esten fuera!!
if (q(j) < [Link](j,1) || q(j)> [Link](j,2)) % Con esta funcion se definen
los limetes positivos y negativos.
%disp("No valida el limite nº:");
%disp(j);
flag1 = false;
end
end
end

Pág. 61 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código para el Jacobiano

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%============== Código para Trabajo Práctico de Jacobiano ================%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%
%%
clear all
close all
clc
%%

%Matriz de D-H simbólica

syms q1 q2 q3 q4 q5 q6 real
qsim = [q1 q2 q3 q4 q5 q6];
syms d1 d2 d3 d4 d5 d6 real
syms a1 a2 a3 a4 a5 a6 real
syms alf1 alf2 alf3 alf4 alf5 alf6 real

%Matriz de D-H ROBOT 1 SIMBOLICA

dhsim = [
0 d1 a1 alf1 0;
0 0 a2 0 0;
0 0 a3 alf3 0;
0 d4 0 alf4 0;
0 0 0 alf5 0;
0 d6 0 0 0];
dhsimq = [
q1 d1 a1 alf1 0;
q2 0 a2 0 0;
q3 0 a3 alf3 0;
q4 d4 0 alf4 0;
q5 0 0 alf5 0;
q6 d6 0 0 0];

%Matriz de D-H ROBOT 1

dh = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.240 -pi/2 0;
0.000 1.300 0.000 pi/2 0;
0.000 0.000 0.000 -pi/2 0;
0.000 0.200 0.000 0.000 0];

%Creo el objeto SerialLink SIMBOLICO

Rob = SerialLink(dhsim);

%Creo el objeto SerialLink

R = SerialLink(dh,'name',' ');
[Link] = [0 -90 0 0 0 0]*pi/180;
[Link] = eye(4,4); %Matriz base si quiero mover base de
robot 1
[Link] = eye(4,4); %Matriz herramienta

%Definicion de limites articulares

[Link](1,1:2) = [-187, 187]*pi/180;

Pág. 62 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

[Link](2,1:2) = [-76 , 77.8]*pi/180;

[Link](3,1:2) = [-30, 85]*pi/180;
[Link](4,1:2) = [-310, 310]*pi/180;
[Link](5,1:2) = [-134, 134]*pi/180;
[Link](6,1:2) = [-360, 360]*pi/180;

%ROBOT 2
dh2 = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.600 0.000 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.600 0.000 0];

%Creo el objeto SerialLink

R2 = SerialLink(dh2,'name',' ');
[Link] = [0 180 90 90]*pi/180;
[Link] = eye(4,4); %
[Link] = eye(4,4); %

%Definicion de limites articulares ROBOT 2, NO ES NECESARIO EL 1 YA LO

%LIMITA
[Link](1,1:2) = [-187, 187]*pi/180;
[Link](2,1:2) = [-33 , 85]*pi/180;
[Link](3,1:2) = [-111, 50]*pi/180;
[Link](4,1:2) = [-310, 310]*pi/180;
%%
%============================================================================
%Procedimiento basico para Jacobiano simbolico
% J = simplify(Rob.jacob0(qsim));
% fprintf('COLUMNA 1 \n');
% disp(J(:,1))
% fprintf('COLUMNA 2 \n');
% disp(J(:,2))
% fprintf('COLUMNA 3 \n');
% disp(J(:,3))
% fprintf('COLUMNA 4 \n');
% disp(J(:,4))
% fprintf('COLUMNA 5 \n');
% disp(J(:,5))
% fprintf('COLUMNA 6 \n');
% disp(J(:,6))
% %Obtencion del determinante simbolico y simplificado
% DJ = simplify(det(J));
% disp(DJ)

%%
%------------RESULTADOS DEL JACOBIANO SIMBOLICO--------------

%------------SE OMITE ESTA SECCIÓN POR DEMASIADO EXTENSA

%------------PUEDE VISUALIZARLA EN EL CÓDIGO ADJUNTO .m

%------------FIN DE RESULTADOS DEL JACOBIANO SIMBOLICO-------

%%
%TRATAMIENTO NUMERICO DEL JACOBIANO

%---Creo un vector articular aleatorio para trabajar

Pág. 63 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

%Seteo parametros generador de numeros aleatorios

rng('default');%seed= inicializo Mersenne Twister en 0 y
%generator=Mersenne Twister
%rng('shuffle','twister');%seed= inicializo segun tiempo reloj, da un resultado
%diferente cada vez que llamo a rng y
%generator=Mersenne Twister

%generar N números aleatorios en el intervalo (a,b)

%con la fórmula r = a + (b-a).*rand(N,1).

%Genero rn numeros aleatorios entre los respectivos limites articulares

rn=10000;
rad=pi/180;
q1r = -187*rad + (187+187)*rad .*rand(rn,1);
q2r = -76*rad + (77.8+76)*rad .*rand(rn,1);
q3r = -33*rad + (85+30)*rad .*rand(rn,1);
q4r = -310*rad + (310+310)*rad .*rand(rn,1);
q5r = -134*rad + (134+134)*rad .*rand(rn,1);
q6r = -360*rad + (360+360)*rad .*rand(rn,1);

qr=[q1r,q2r,q3r,q4r,q5r,q6r];
%minqr = min(abs(qr))
%sizeqv = size(qr)

%--------Obtencion del Jacobiano de manera simbolica/numerica

fprintf(' \n');
fprintf('-----INCISO 1-----\n');
fprintf('Calculo del Jacobiano y demas parametros para analisis\n');
for i=1:rn
J(i,1:6,1:6) = R.jacob0(qr(i,:)); %Jacobiano
Jr(i,1:3,1:6) = J(i,1:3,:);%trabajo con las tres primeras filas
%(x, y, z = porque estoy en el espacio
traslacional)
%Demás filas son: alfa, beta, gama
Je(1:6,1:6)=J(i,1:6,1:6);%matriz cuadrada usada para calcular det, rank y
cond, etc
dJ(i,1) = det(Je);%determinantes
rJ(i,1) = rank(Je);%rangos
ncJ(i,1) = cond(Je);%numeros de condicion
end

fprintf(' \n');
fprintf('Valores min y max del determinante\n');
mindJ=min(abs(dJ))
maxdJ=max(abs(dJ))
fprintf(' \n');
fprintf('Valores min y max del numero de condicion\n');
minncJ=min(abs(ncJ))
maxncJ=max(abs(ncJ))

%Graficas de puntos dispersos para algunos parametros respresentativos

x=ones(rn,1);
%Para los determinantes
figure (1)
auxi=ones(10000,1);
var=0;
for i=1:10000
var=var+1;
auxi(i,1)=auxi(i,1)*var;
end

Pág. 64 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

scatter(auxi,dJ,10,'filled','LineWidth',2)
title('Representación de los determinantes')
xlabel('Numero de vector articular calculado')
ylabel('Determinante del Jacobiano')
%Para los numeros de condicion
figure (2)
scatter(auxi,ncJ,10,'filled','LineWidth',2)
title('Representación de los números de condicion')
xlabel('Numero de vector articular calculado')
ylabel('Numero de condicion del Jacobiano')

fprintf(' \n');
fprintf('-----INCISO 2-----\n');
fprintf('Obtencion de vectores articulares conflictivos\n');

%Guardo en un vector los indices que identifican al vector articular cuyo

%determinante es menor al limite impuesto
for i=1:rn
val=abs(dJ(i,1));
if (val < 0.001)
indx(i,1)=i;
else
indx(i,1)=0;
end
end

indx(indx==0) = [];%elimino todos los elementos que sean cero

tam=size(indx,1)%almaceno el tamaño de este vector

fprintf(' \n');
fprintf('Vectores articulares con det<0.001:\n');
for i=1:tam %en un bucle para ese tamaño voy identificando de que indices se
trata y los ploteo
fprintf(' \n');
fprintf('Vector n°: %u\n',i);
in=indx(i,1);
qr(in,:)*(180/pi)
end

%Como lo anterior resultaria en muchisimas graficas, hago un conunto más

%chiquito, eligiendo aleatoriamente alguno de esos indices ya identificados
%y los voy ploteando
rng('default');
q1r = round(tam .*rand(5,1));

figure (3)
for i=1:5
fprintf(' \n');
fprintf('Vector articular n°: %u\n',i);
disp(qr(indx(q1r(i,1),1),:)*(180/pi))
figure (3)
[Link](qr(indx(q1r(i,1),1),:),'workspace',[-4 4 -4 4 -0 4]);% %linea
qr2=cinemInversaR2(R2,qr(indx(q1r(i,1),1),:));
hold on
[Link](qr2','workspace',[-4 4 -4 4 -0 4],'delay',1);% %linea
%title('Representación de los números de condicion');
%legend(('Fanuc S-420iF: Robot 1 y 2'),'Location','southwest');
%hold off
end

Pág. 65 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

hold off
%close all;%COMENTAR ESTA LINEA PARA VER LAS GRAFICAS DE LOS INCISOS 1, 2 Y 3

fprintf(' \n');
fprintf('-----INCISO 4-----\n');
fprintf('Grafica de elipsoide de manipulabilidad traslacional\n');
%Utilizo la cinematica directa para obtener coordenadas del centro de la
%elipsoide

%PRIMERA FORMA (de manera manual)

%figure(4)
%[Link]([0,0,0,0,0,0],'workspace',[-4 4 -4 4 -0 4]);% %linea
%[Link]([0,0,0,0,0,0],'callback', @(R,q) [Link](q), 'workspace',[-6 6 -6 6 -
7 7])

%SEGUNDA FORMA (usando los resultados obtenidos anteriormente)

figure (5)
%ind=5;
%Jrr(1:3,1:6)=Jr(indx(q1r(ind,1),1),1:3,1:6);
%Selección manual de los vectores a graficar con elipsoide de
%manipulabilidad traslacional
%Estos vectores son los correspondientes a puntos específicos de la
%trayecoria de trabajo de nuestra aplicación
P1=[45.0000,68.1881,-33.0007,0,54.8126,135.0000]*rad;%Punto inicial de la
trayectoria
P2=[0,34.4132,24.1323,0,31.4545,90.0000]*rad;%Punto medio de la trayectoria
%P2=[-45.0000,68.1881,-33.0007,0,54.8126,45.0000]*rad;%Punto final de la
trayectoria
qr2=cinemInversaR2(R2,P2);
qr2=qr2';
J1=R.jacob0(P1);
J2=R.jacob0(P2);
Jrr1(1:3,1:6)=J1(1:3,1:6);
Jrr2(1:3,1:6)=J2(1:3,1:6);
%[Link](qr(indx(q1r(ind,1),1),:), 'workspace',[-6 6 -6 6 -7 7])
[Link](P2, 'workspace',[-5 5 -5 5 -2 6])
hold on
[Link](qr2,'workspace',[-5 5 -5 5 -2 6]);
%plot_ellipse(Jrr*Jrr', transl([Link](qr(indx(q1r(ind,1),1),:))));
plot_ellipse(Jrr2*Jrr2', transl([Link](P2)));
hold off

Pág. 66 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

Código para planificación y generación de trayectoria

%%
%=========================================================================%
%======================== ROBOTICA 1 - Año 2020 ==========================%
%======= Código para Trabajo Práctico de P. y G. de Trayectoria ==========%
%=================== Ortega Pablo - Alfredo Iglesias =====================%
%=================== Leg.: 10666 - Leg.: 10299 ========================%
%=========================================================================%
%
%%
clear all
close all
clc
%%
%ROBOT 1
dh = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.240 -pi/2 0;
0.000 1.300 0.000 pi/2 0;
0.000 0.000 0.000 -pi/2 0;
0.000 0.200 0.000 0.000 0];

R = SerialLink(dh,'name',' ');
[Link] = [0 -90 0 0 0 0]*pi/180;
[Link] = eye(4,4); %Matriz base si quiero mover base de
robot 1
[Link] = eye(4,4); %Matriz herramienta
%Definicion de limites articulares
[Link](1,1:2) = [-187, 187]*pi/180;
[Link](2,1:2) = [-76 , 77.8]*pi/180;
[Link](3,1:2) = [-33, 85]*pi/180;
[Link](4,1:2) = [-310, 310]*pi/180;
[Link](5,1:2) = [-134, 134]*pi/180;
[Link](6,1:2) = [-360, 360]*pi/180;

%ROBOT 2
dh2 = [
0.000 1.000 0.180 -pi/2 0;
0.000 0.000 0.600 0.000 0;
0.000 0.000 0.950 0.000 0;
0.000 0.000 0.600 0.000 0];

%Creo el objeto SerialLink

R2 = SerialLink(dh2,'name',' ');
[Link] = [0 180 90 90]*pi/180;

%%
%EJERCICIO TF 3.a)

T1=[ 0 -1 0 1.6;
-1 0 0 1.6;
0 0 -1 0.6;
0 0 0 1 ];

T2=[ 0 -1 0 1.6;
-1 0 0 -1.6;
0 0 -1 0.6;
0 0 0 1 ];

Pág. 67 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

q0=[0,0,0,0,0,0];
[q1]= cinemInversaR1(R,q0,T1);
[q2]= cinemInversaR1(R,q1,T2);
Qa = jtraj(q1,q2,100); % interpolación articular

% %%Si no quiero ver el ejer 3.a), pero si utilizar sus datos, comento esto
fprintf(' \n');
fprintf('-----EJERCICIO TF 3.a)-----\n');

fprintf('\n Punto 1 cartesiano\n')

disp(T1);
fprintf('\n Punto 2 cartesiano\n')
disp(T2);

fprintf('\n A continuación: Interpolación articular de T1 a T2.\n')

fprintf('\n Presione Enter para continuar.\n')
pause
figure (3);
title('Trayectoria desde T1 a T2 Interpolacion Articular TF 3.a)');
for i=1:100
Qa1=Qa(i,:);
[Link](Qa1,'workspace',[-4 4 -4 4 -0 4]); % animación
qr2=cinemInversaR2(R2,Qa1);
hold on
[Link](qr2','workspace',[-4 4 -4 4 -0 4]);% animación
[Ttrayectoria]= cinemDirecta(R,Qa1); % Como no puedo graficar ambos robots
juntos, necesito graficar yo la tryaectoria
x=Ttrayectoria(1,4);
y=Ttrayectoria(2,4);
z=Ttrayectoria(3,4);
trayect= plot3(x,y,z,'b o','LineWidth',3,'MarkerSize',2); % Ceo un punto por cada
uno de los 100 pasos
legend({'Fanuc S-420iF: Robot 1 y 2','* Trayectoria'},'Location','southwest');
end
hold off
figure (4);
qplot(Qa); % gráficas
title('Trayectoria articular Ejer2');

%%
%--------------------------------------------------------------------------
%EJERCICIO TF 3.b)

TT = ctraj(T1, T2, 100);

q0=[0,0,0,0,0,0];
Qb1=zeros(100,6);
for i=1:100
[Qb]= cinemInversaR1(R,q0,TT(:,:,i)); %Cambio de la trayectoria TT a cord
articular
q0=Qb; %Actualizo el punto anterior
Qb1(i,:)=Qb;
end
fprintf('\n A continuación: Interpolación cartesiana de T1 a T2.\n')
fprintf('\n Presione Enter para continuar.\n')
pause
% %%Si no quiero ver el ejer 3.b), pero si utilizar sus datos, comento esto
fprintf(' \n');
fprintf('-----EJERCICIO TF 3.b)-----\n');

Pág. 68 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

fprintf('\n Punto 1 cartesiano\n')

disp(T1);
fprintf('\n Punto 2 cartesiano\n')
disp(T2);
figure (5);
title('Trayectoria desde T1 a T2 Interpolacion Cartesiana TF 3.b)');
for i=1:100
[Link](Qb1(i,:),'workspace',[-4 4 -4 4 -0 4]); % animación
qr2=cinemInversaR2(R2,Qb1(i,:));
hold on
[Link](qr2','workspace',[-4 4 -4 4 -0 4]);% animación

[Ttrayectoria]= cinemDirecta(R,Qb1(i,:)); % Como no puedo graficar ambos robots

juntos, necesito graficar yo la tryaectoria
x=Ttrayectoria(1,4);
y=Ttrayectoria(2,4);
z=Ttrayectoria(3,4);
trayect= plot3(x,y,z,'b o','LineWidth',3,'MarkerSize',2); % Ceo un punto por cada
uno de los 100 pasos
legend({'Fanuc S-420iF: Robot 1 y 2','* Trayectoria'},'Location','southwest');
end
hold off
figure (6);
qplot(Qb1); % gráficas
title('Trayectoria articular TF 3.b)');

%%
% %-------------------------------------------------------------------------
%EJERCICIO TF 4 Pos,Vel y Acel Intepolacion Articular (ESPACIO ARTICULAR)

[Q2,QD2,QDD2] = jtraj(q1,q2,100);

% %%Si no quiero ver el ejer 4 Pos,Vel y Acel Intep Articular, pero si utilizar
sus datos, comento esto
fprintf(' \n');
fprintf('-----EJERCICIO TF 4 Pos,Vel y Acel Intepolacion Articular-----\n');
fprintf('\n A continuación: Trayectoria articular (Interp. Art.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (7);
qplot(Q2); % gráficasv
title('Trayectoria articular con Interp. Articular');
fprintf('\n A continuación: Velocidad articular (Interp. Art.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (8);
qplot(QD2);
title('Velocidad articular con Interp. Articular');
fprintf('\n A continuación: Aceleración articular (Interp. Art.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (9);
qplot(QDD2);
title('Aceleracion articular con Interp. Articular');

%Inciso 4 Pos,Vel y Acel Intepolacion Cartesiana (ESPACIO ARTICULAR)-------------

---------------------------------------------------
%Como este calculo derivativo, anula el punto inicial y final, se agregan
QD3=zeros(100,6);

Pág. 69 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

QDD3=zeros(100,6);
QD3(2:100,:)= diff(Qb1)/0.01; %primera derivada (velocidad)
QDD3(2:99,:)= diff(Qb1,2)/0.0001; %segunda derivada (aceleracion)

QD3(100,:)=[0 0 0 0 0 0];

% %%Si no quiero ver el ejer TF 4 Pos,Vel y Acel Intepolacion Cartesiana, pero si

utilizar sus datos, comento esto
fprintf(' \n');
fprintf('-----EJERCICIO TF 4 Pos,Vel y Acel Intepolacion Cartesiana-----\n');
fprintf('\n A continuación: Trayectoria articular (Interp. Cart.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (11);
qplot(Qb1); % gráficas
title('Trayectoria articular con Interp. Cartesiana');
fprintf('\n A continuación: Velocidad articular (Interp. Cart.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (12);
qplot(QD3);
title('Velocidad articular con Interp. Cartesiana');
fprintf('\n A continuación: Aceleración articular (Interp. Cart.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (13);
qplot(QDD3);
title('Aceleracion articular con Interp. Cartesiana');
%--------------------------------------------------------------------------------
--------------------
%%
%Inciso 4 Pos,Vel y Acel Interpolacion Articular (ESPACIO DE TRABAJO)
TT2 = double([Link](Q2));% Buesco primero las matrices correspondientes a las Q2
x2(1,1:100)=TT2(1,4,:); % Extraigo la informacion de las TT2 de la trayectoria
ejer 3
y2(1,1:100)=TT2(2,4,:);
z2(1,1:100)=TT2(3,4,:);
t=1:100;

xd2=zeros(1,100);
yd2=zeros(1,100);
zd2=zeros(1,100);
xd2(1,2:100)= diff(x2)/0.01;
yd2(1,2:100)= diff(y2)/0.01;
zd2(1,2:100)= diff(z2)/0.01;
xd2(1,100)=0;
yd2(1,100)=0;
zd2(1,100)=0;

xdd2=zeros(1,100);
ydd2=zeros(1,100);
zdd2=zeros(1,100);
xdd2(1,2:99)= diff(x2,2)/0.0001;
ydd2(1,2:99)= diff(y2,2)/0.0001;
zdd2(1,2:99)= diff(z2,2)/0.0001;

%Si no quiero ver el ejer 4.3, pero si utilizar sus datos, comento esto y
%lo de la figura 15 tambien
fprintf(' \n');

Pág. 70 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

fprintf('-----EJERCICIO TF 4 Interpolacion Articular, punos cartesianos-----\n');

fprintf('\n A continuación: Trayectoria cartesiana (Interp. Art.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (14);
plot(t,x2,t,y2,t,z2);
legend('X2','Y2','Z2')
xlabel('Tiempo')
ylabel('Desplazamiento cartesiano')
title('TF 3.a) Posicion cartesianas X,Y,Z extremo robot (Espacio de trabajo)
(Intp. Art.)');
fprintf('\n A continuación: Velocidad cartesiana (Interp. Art.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (15);
plot(t,xd2,t,yd2,t,zd2);
legend('Xd2','Yd2','Zd2')
xlabel('Tiempo')
ylabel('Velocidad cartesiana')
title('TF 3.a) Velocidad cartesianas X,Y,Z extremo robot (Espacio de trabajo)
(Intp. Art.)');
fprintf('\n A continuación: Acelración cartesiana (Interp. Art.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (16);
plot(t,xdd2,t,ydd2,t,zdd2);
legend('Xdd2','Ydd2','Zdd2')
xlabel('Tiempo')
ylabel('Aceleracion cartesiano')
title('TF 3.a) Aceleracion cartesianas X,Y,Z extremo robot (Espacio de trabajo)
(Intp. Art.)');

% ---------Pos,Vel y Acel Interpolacion Cartesiana (ESPACIO DE TRABAJO)

x3(1,1:100)=TT(1,4,:); % Extraigo la informacion de las TT de la trayectoria
ejer 3
y3(1,1:100)=TT(2,4,:);
z3(1,1:100)=TT(3,4,:);

xd3=zeros(1,100);
yd3=zeros(1,100);
zd3=zeros(1,100);
xd3(1,2:100)= diff(x3)/0.01;
yd3(1,2:100)= diff(y3)/0.01;
zd3(1,2:100)= diff(z3)/0.01;
xd3(1,100)=0;
yd3(1,100)=0;
zd3(1,100)=0;

xdd3=zeros(1,100);
ydd3=zeros(1,100);
zdd3=zeros(1,100);
xdd3(1,2:99)= diff(x3,2)/0.0001;
ydd3(1,2:99)= diff(y3,2)/0.0001;
zdd3(1,2:99)= diff(z3,2)/0.0001;
%Figura 15 para no ver el TF 4 completo, pero si utilizar sus datos,
%comento esto tambien
fprintf('\n A continuación: Trayectoria cartesiana (Interp. Cart.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (17);
plot(t,x3,t,y3,t,z3);

Pág. 71 de 72
UNCUYO – Ing. Mecatrónica Robótica I Autores: [Link]; [Link]
Mendoza – Argentina Modelado cinemático de robot FANUC S-420I F Rev.0: 16/10/2020

legend('X3','Y3','Z3')
xlabel('Tiempo')
ylabel('Desplazamiento cartesiano')
title('TF 3.b) Posicion cartesianas X,Y,Z extremo robot (Espacio de trabajo)
(Intp. Carte.)');
fprintf('\n A continuación: Velocidad cartesiana (Interp. Cart.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (18);
plot(t,xd3,t,yd3,t,zd3);
legend('Xd3','Yd3','Zd3')
xlabel('Tiempo')
ylabel('Velocidad cartesiana')
title('TF 3.b) Velocidad cartesianas X,Y,Z extremo robot (Espacio de trabajo)
(Intp. Carte.)');
fprintf('\n A continuación: Aceleración cartesiana (Interp. Cart.).\n')
fprintf('\n Presione Enter para continuar.\n')
pause
figure (19);
plot(t,xdd3,t,ydd3,t,zdd3);
legend('Xdd3','Ydd3','Zdd3')
xlabel('Tiempo')
ylabel('Aceleracion cartesiano')
title('TF 3.b) Aceleracion cartesianas X,Y,Z extremo robot (Espacio de trabajo)
(Intp. Carte.)');

%%
%Inciso 4 TF X vs Y--------------------------------------------------------------
--
%Si no quiero ver, comento todo esto
% fprintf(' \n');
% fprintf('-----EJERCICIO TF X vs Y cartesiano extremo robot4-----\n');
% figure (20);
% plot(x2,y2);
% legend('Y2 vs X2')
% xlabel('Desplazamiento cartesiano en X2')
% ylabel('Desplazamiento cartesiano en Y2')
% title('TF 3.a) Variables cartesianas X,Y extremo robot');
%
% figure (21);
% plot(y3,y3);
% legend('Y3 vs X3')
% xlabel('Desplazamiento cartesiano en X3')
% ylabel('Desplazamiento cartesiano en Y3')
% title('TF 3.b) Variables X,Y extremo robot');

%--------------------------------------------------------------------------

Pág. 72 de 72