SEP SES TecNM

INSTITUTO TECNOLÓGICO DE TOLUCA

ROBOTICA
GRUPO 190400

MECATRONICA

REPORTE
ALGORITMO DE DAVNIT-HATENBERG

PRESENTA:

JIMENEZ VALDEZ JOSE ALFREDO 19280547

PROFESOR:

ELISEO RIVERA SILVA

METEPEC, ESTADO DE MÉXICO, JULIO 2024

 ROBOTICA
FECHA: 2024

Reporte de Práctica No: 2 Tema: 2 Grupo: 190400

Introducción

El algoritmo de Denavit-Hartenberg (D-H) es una metodología fundamental en la robótica para

describir la cinemática de los robots manipuladores. Este algoritmo proporciona una forma
sistemática de representar las relaciones espaciales entre eslabones consecutivos de un robot
mediante transformaciones homogéneas. Utilizando cuatro parámetros (ángulo de torsión,
desplazamiento, longitud del eslabón y ángulo del eslabón), el modelo D-H simplifica la
complejidad de las ecuaciones cinemáticas, facilitando así el análisis y control de movimientos en
robots de múltiples grados de libertad. En esta práctica, exploraremos la aplicación del algoritmo
de Denavit-Hartenberg para modelar y simular un brazo robótico, destacando su importancia en
la programación y diseño de sistemas robóticos.

Objetivos de la practica

Familiarizarse con el algoritmo de Denavit-Hartenberg para describir la estructura y configuración

de un brazo robótico. Se busca comprender cómo utilizar los parámetros D-H para modelar la
disposición espacial de eslabones y juntas, facilitando así el análisis estructural de robots
manipuladores.

Material utilizado:
• Computadora
• Software Solidworks
• Software CoppeliaSim Edu
• Software Code::Blocks
• Software SageMathCell

Desarrollo:
1. El primer paso fue el diseño de cada uno de los eslabones del robot de 6 grados de
libertad de acuerdo con el plano proporcionado con ayuda de Solidworks o algún otro
software de diseño:
 ROBOTICA

Eslabón 0 (base):
 ROBOTICA
Eslabón 1

Eslabón 2
 ROBOTICA
Eslabón 3

Eslabón 4
 ROBOTICA
Eslabón 5

Eslabón 6 (elemento terminal/pinza)

 ROBOTICA
2. Una vez diseñadas las piezas, procedemos a realizar el ensamble entre cada uno de los
eslabones:

3. Con base a esa imagen (vista isométrica), identificamos las articulaciones del robot,
siendo todas de tipo rotativas:
 ROBOTICA
4. Aplicación del algoritmo de Denavit Hartenberg: Se estableció un sistema de referencias
para cada eslabón tomando en cuenta los siguientes criterios:
• Cada eslabón debe cumplir con el sistema de la mano derecha.
• Se toma como primera relación la base del robot que se establece de manera arbitraria.
• El método de Denavit-Hartenberg dice que la orientación del eje “𝑘𝑛” se dará
acomodando la orientación del eje apuntando al siguiente eslabón.
Para ello, colocamos una vista explosionada de nuestro robot, colocamos los sistemas de
referencias con colores:
 ROBOTICA
5. Para poder entender mejor la dirección de los ejes de cada sistema se adjuntan algunas
vistas adicionales del robot, además, acotando las distancias entre las articulaciones para
después poder realizar la tabla de parámetros de Devanit-Hatenberg:
Vista lateral izquierda
 ROBOTICA
Vista frontal

Vista superior
 ROBOTICA
6. Realizamos nuestra tabla de parámetros con base a la configuración de los sistemas de
referencias de nuestro robot, quedado de la siguiente forma:
ESLABON 𝑋𝐼 𝑍𝐼 𝜃𝐼 𝛼𝐼
[m] [m] [rad] [rad]
1 0 1.02 0 −𝜋⁄
2
2 0.5 0 0 0
3 0 0 0 −𝜋⁄
2
4 0 0.485 0 −𝜋⁄
2
5 0 0 𝜋⁄ −𝜋⁄
2 2
6 0 0 0 0
7. Importamos nuestras piezas. STL en el software CoppeliaSim, y damos los movimientos
de rotación y traslación de acuerdo con la tabla anterior, dando como resultado el
siguiente ensamble:

8. Corregiremos la forma en que ensamblamos nuestros eslabones, haciendo uso de

articulaciones desde Coppelia:
 ROBOTICA
9. Ahora comprobaremos si nuestro ensamblaje y tabla de parámetros son correctos
ayuda de la realización de las siguientes transformadas: 𝑆0 𝑆1 𝑆2 𝑆3 𝑆4 𝑆5
𝑆1𝑇 , 𝑆2𝑇 , 𝑆3𝑇, 𝑆4𝑇 , 𝑆5𝑇 , 𝑆6𝑇
0.93969 −0.00000 0.34202 0.00000
𝑆0 −0.34202 −0.00000 0.93969 0.00000
𝑆1𝑇 = ( 0.00000 −1.00000 −0.00000 1.02000
)
0.00000 0.00000 0.00000 1.00000

0.93969 0.34202 0.00000 0.46985

𝑆1 −0.34202 0.93969 0.00000 −0.17101
𝑆2𝑇 = ( 0.00000 1.00000 0.00000 0.00000
)
0.00000 0.00000 0.00000 1.00000

0.68200 0.00000 −0.73135 0.00000

𝑆2 0.73135 −0.00000 0.68200 0.00000
𝑆3𝑇 = (0.00000 −1.00000 0.00000 0.00000
)
0.00000 0.00000 0.00000 1.00000

0.70711 0.00000 −0.70711 0.00000

𝑆3 0.70711 −0.00000 0.70711 0.00000
𝑆4𝑇 = (0.00000 −1.00000 −0.00000 0.48500
)
0.00000 0.00000 0.00000 1.00000

−0.94552 0.00000 −0.32557 0.00000

𝑆4 0.32557 0.00000 −0.94552 0.00000
𝑆5𝑇 =( )
0.00000 −1.00000 −0.00000 0.00000
0.00000 0.00000 0.00000 1.00000

1.00000 0.00000 0.00000 0.00000

𝑆5 0.00000 1.00000 0.00000 0.00000
𝑆6𝑇 =( )
0.00000 0.00000 1.00000 0.00000
0.00000 0.00000 0.00000 1.00000

Para comprobar que se hayan realizado correctamente hacemos uso de nuestro programa
realizado en CodeBlocks en que añadiremos nuestra tabla de la siguiente manera:
 ROBOTICA

Por último, tendremos la matriz que nos indica la posición del eslabón final:

−0.00000 0.00000 −1.00000 0.50000

0.00000 1.00000 0.00000 −0.00000
T[06] = ( )
1.00000 −0.00000 −0.00000 0.53500
0.00000 0.00000 0.00000 1.00000
 ROBOTICA
Esos valores nos sirven para comprobar que las transformadas y el ensamble son correctos,
pues dentro de Coppelia seleccionamos nuestro último eslabón y en la parte de arriba nos
saldrá la información de la ubicación de este:

Como se puede observar, los datos en los tres casos coinciden indicándonos que el ensamble es
correcto.

10. Se realizo una prueba más añadiendo un movimiento angular al segundo eslabón de -
45°:
 ROBOTICA
Agregamos las siguientes líneas al código de nuestra calculadora:

Como únicamente se le da movimiento de -45° al eslabón 2, se agrega ese valor al código de la

siguiente manera:

Ejecutamos el programa y nos da las siguientes matrices:

1.00000 0.00000 0.00000 0.00000

𝑆0 0.00000 0.00000 1.00000 0.00000
𝑆1𝑇 = (0.00000 −1.00000 0.00000 1.02000
)
0.00000 0.00000 0.00000 1.00000

0.70711 0.70711 0.00000 0.35355

𝑆1 −0.70711 0.70711 0.00000 −0.35355
𝑆2𝑇 =( )
0.00000 0.00000 1.00000 0.00000
0.00000 0.00000 0.00000 1.00000

1.00000 0.00000 0.00000 0.00000

𝑆2 0.00000 0.00000 1.00000 0.00000
𝑆3𝑇 =( )
0.00000 −1.00000 0.00000 0.00000
0.00000 0.00000 0.00000 1.00000
 ROBOTICA
1.00000 0.00000 0.00000 0.00000
𝑆3 0.70711 −0.00000 1.00000 0.00000
𝑆4𝑇 =( )
0.00000 −1.00000 0.00000 0.48500
0.00000 0.00000 0.00000 1.00000

0.00000 0.00000 −1.00000 0.00000

𝑆4 1.00000 0.00000 0.00000 0.00000
𝑆5𝑇 =( )
0.00000 −1.00000 −0.00000 0.00000
0.00000 0.00000 0.00000 1.00000

1.00000 0.00000 0.00000 0.00000

𝑆5 0.00000 1.00000 0.00000 0.00000
𝑆6𝑇 = (0.00000 0.00000 1.00000 0.00000
)
0.00000 0.00000 0.00000 1.00000

Nuevamente nos arrojara la matriz de transformación del último eslabón respecto a origen

−0.70711 0.00000 −0.70711 0.69650

0.00000 1.00000 0.00000 0.00000
𝑇[06] = ( )
0.70711 0.00000 −0.70711 1.03061
0.00000 0.00000 0.00000 1.00000

Corroboramos en Coppelia y vemos que coinciden de nuevo:

 ROBOTICA
CINEMATICA INVERSA
En la cinemática directa, como se puede ver en la implementación del código, orienta los
eslabones del robot conociendo sus ángulos θ1 , θ2 , θ3 , θ4 , θ5 y θ6 ; o bien sus desfases, como lo
son q1 , 𝑞2 , 𝑞3 , 𝑞4 , 𝑞5 y 𝑞6 𝑞. En este caso se utiliza θ1 = 𝑞1 , θ2 = 𝑞2 , θ3 = 𝑞3 , θ4 = 𝑞4 , θ5 = 𝑞5,
θ6 = 𝑞6 . Si nosotros no conociéramos los ángulos correspondientes, pero sí las coordenadas del
punto 𝑟𝑂6 , se podría asumir por medio de la sustitución y despejes de variables la equivalencia
para cualquier ángulo de las articulaciones. Aludiendo al desarrollo anterior se puede observar
que 𝑥1 , 𝑥2 , 𝑥3 , 𝑥4 , 𝑥5 y 𝑥6 así como, 𝑧1, 𝑧2 , 𝑧3 , 𝑧4 , 𝑧5 y 𝑧6 ya han sido sustituidas por sus constantes
correspondientes en las matrices. Si no se asigna un valor numérico a dichas variables, se seguirá
operando con los valores simbólicos correspondientes.

Para el caso del estudio de este robot se seguirán conservando los valores de 𝑥1 , 𝑥2 , 𝑥3 , 𝑥4 , 𝑥5 y
𝑥6 iguales a cero, mientras que a 𝑧1, 𝑧2 , 𝑧3 , 𝑧4 , 𝑧5 y 𝑧6 se les considera como apoyo para observar
su generalización.
𝑥6
𝑦6
Si se conoce una posición en el punto 𝑟07 , entonces: 𝑟𝑂6 = [𝑧 ]
6
1

CINEMATICA INVERSA METODO NEWTON-RAPHSON

Entre los métodos para encontrar dichos ángulos se puede utilizar el método de NewtonRaphson
para múltiples variables, una forma analítica o simplemente una forma geométrica. Por usos
prácticos se dedujeron los ángulos con el método de NewtonRaphson para múltiples variables.
Para el método de newton se utiliza el jacobiano del vector a encontrar que no son más que las
derivadas parciales respecto a las funciones que varían en el tiempo.
Considerando la convergencia del sistema bajo la siguiente función:
𝑥𝑂6
𝐵0
𝑆0 𝐵6 𝑅
𝑦𝑂6
𝑆6 𝑇 = ( 𝑧𝑂6 )
0 0 0 1
Donde 𝑥𝑂6 = 𝑓1 (𝑞1 , 𝑞2 , 𝑞3 )
𝑦𝑂6 = 𝑓2 (𝑞1 , 𝑞2 , 𝑞3 )
𝑧𝑂6 = 𝑓3 (𝑞1 , 𝑞2 , 𝑞3 )

Sean
𝑞1
𝑞̅ = (𝑞2 )
𝑞3
𝑓1
𝐹̅ = (𝑓2 )
𝑓3
Y
𝑟̅𝑂6 = 𝑟𝑂6 (𝑞1 , 𝑞2 , 𝑞3 )
 ROBOTICA
𝑓1 (𝑞1 , 𝑞2 , 𝑞3 )
𝑟̅𝑂6 = (𝑓2 (𝑞1 , 𝑞2 , 𝑞3 )) es conocido
𝑓3 (𝑞1 , 𝑞2 , 𝑞3 )

𝑓1 − 𝑥𝑂6 = 0
⟹ 𝑓2 − 𝑦𝑂6 = 0
𝑓3 − 𝑧𝑂6 = 0

La solución para las q seria:

𝑞𝑖+1 = 𝑞̅𝑖 − 𝐽 −1 ∙ 𝑑𝐹̅
̅̅̅̅̅

Donde

𝑑𝐹̅ = 𝐹̅ (𝑞̅𝑖 ) − 𝑟̅𝑂6

Y tenemos que

𝜕𝑓1 𝜕𝑓1 𝜕𝑓1

𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
𝜕𝑓2 𝜕𝑓2 𝜕𝑓2
𝐽=
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
𝜕𝑓3 𝜕𝑓3 𝜕𝑓3
(𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 )

Ahora dentro de SageMathCell, crearemos un código para poder sacar las Jacobianas de cada una
de las transformaciones de acuerdo con nuestra tabla D-H.
 ROBOTICA
Primeramente, inicializamos e importamos las librerías necesarias y también realizamos la
declaración de variables:

reset()
import sage.all
from sympy.utilities.codegen import codegen
from sage.ext.fast_eval import fast_float

var ('t1v t2v t3v t4v t1a t2a t3a t4a alp1 alp2
alp3');

var('t1', latex_name=r'\theta_1');
var('t2', latex_name=r'\theta_2');
var('t3', latex_name=r'\theta_3');
var('t4', latex_name=r'\theta_4');
var('t5', latex_name=r'\theta_5');
var('t6', latex_name=r'\theta_6');

var('z1', latex_name='z_1');
var('z2', latex_name='z_2');
var('z3', latex_name='z_3');
var('z4', latex_name='z_4');
var('z5', latex_name='z_5');
var('z6', latex_name='z_6');

var('x1', latex_name='x_1');
var('x2', latex_name='x_2');
var('x3', latex_name='x_3');
var('x4', latex_name='x_4');
var('x5', latex_name='x_5');
var('x6', latex_name='x_6');

var('q1', latex_name='q_1');
var('q2', latex_name='q_2');
var('q3', latex_name='q_3');
var('q4', latex_name='q_4');
var('q5', latex_name='q_5');
var('q6', latex_name='q_6');
 ROBOTICA
Creamos nuestra tabla de D-H:
t1=0; alp1=-pi/2; z1=1.02; x1=0;
t2=0; alp2=0; z2=0; x2=0.5;
t3=0; alp3=-pi/2; z3=0; x3=0;
t4=0; alp4=-pi/2; z4=0.485; x4=0;
t5=pi/2; alp5=-pi/2; z5=0; x5=0;
t6=0; alp6=0; z6=0; x6=0;

Creamos nuestras matrices de transformaciones para cada eslabón, del 1 al 6:

Tz1= Matrix([[cos(t1+q1),-sin(t1+q1), 0,
0],[sin(t1+q1),cos(t1+q1),0,0],[0,0,1, z1],[0 ,0,0,1]]);
Tx1= Matrix([[1,0, 0, x1],[0,cos(alp1),-
sin(alp1),0],[0,sin(alp1),cos(alp1), 0],[0 ,0,0,1]]);
T01 = Tz1*Tx1;

Tz2= Matrix([[cos(t2+q2),-sin(t2+q2), 0,
0],[sin(t2+q2),cos(t2+q2),0,0],[0,0,1, z2],[0 ,0,0,1]]);
Tx2= Matrix([[1,0, 0, x2],[0,cos(alp2),-
sin(alp2),0],[0,sin(alp2),cos(alp2), 0],[0 ,0,0,1]]);
T12 = Tz2*Tx2;
T02=T01*T12;

Tz3= Matrix([[cos(t3+q3),-sin(t3+q3), 0,
0],[sin(t3+q3),cos(t3+q3),0,0],[0,0,1, z3],[0 ,0,0,1]]);
Tx3= Matrix([[1,0, 0, x3],[0,cos(alp3),-
sin(alp3),0],[0,sin(alp3),cos(alp3), 0],[0 ,0,0,1]]);
T23 = Tz3*Tx3;
T03=T02*T23;

Tz4= Matrix([[cos(t4+q4),-sin(t4+q4), 0,
0],[sin(t4+q4),cos(t4+q4),0,0],[0,0,1, z4],[0 ,0,0,1]]);
Tx4= Matrix([[1,0, 0, x4],[0,cos(alp4),-
sin(alp4),0],[0,sin(alp4),cos(alp4), 0],[0 ,0,0,1]]);
T34 = Tz4*Tx4;
T04=T03*T34;

Tz5= Matrix([[cos(t5+q5),-sin(t5+q5), 0,
0],[sin(t5+q5),cos(t5+q5),0,0],[0,0,1, z5],[0 ,0,0,1]]);
Tx5= Matrix([[1,0, 0, x5],[0,cos(alp5),-
sin(alp5),0],[0,sin(alp5),cos(alp5), 0],[0 ,0,0,1]]);
T45 = Tz5*Tx5;
T05=T04*T45;

Tz6= Matrix([[cos(t6+q6),-sin(t6+q6), 0,
0],[sin(t6+q6),cos(t6+q6),0,0],[0,0,1, z6],[0 ,0,0,1]]);
Tx6= Matrix([[1,0, 0, x6],[0,cos(alp6),-
sin(alp6),0],[0,sin(alp6),cos(alp6), 0],[0 ,0,0,1]]);
T56 = Tz6*Tx6;
T06=(T05*T56).simplify_full()
 ROBOTICA
Imprimimos cada una de las transformadas con las siguientes líneas del código:
show('T[01]= ', T01)
show('T[02]= ', T02)
show('T[03]= ', T03)
show('T[04]= ', T04)
show('T[05]= ', T05)
show('T[06]= ', T06)
Dándonos como resultado:
co s(𝑞1 ) 0 −si n(𝑞1 ) 0
) 0 co s(𝑞1 )
T[01]= ( si n(𝑞1 0 )
0 −1 0 1.02000000000000
0 0 0 1

1.0000co s(𝑞1 ) co s(𝑞2 ) −1.000co s(𝑞1 ) si n(𝑞2 ) −1.0000si n(𝑞1 ) 0.500co s(𝑞1 ) co s(𝑞2 )
10000co s(𝑞2 ) si n(𝑞1 ) −1.000si n(𝑞1 ) si n(𝑞2 ) 1.0000co s(𝑞1 ) 0.5000co s(𝑞2 ) si n(𝑞1 )
T[02]= ( )
−1.0000si n(𝑞2 ) −1.000co s(𝑞2 ) 0 −0.5000si n(𝑞2 ) + 1.0200
0 0 0 1
co s(𝑞1) co s(𝑞2) co s(𝑞3) − co s(𝑞1) si n(𝑞2) si n(𝑞3) si n(𝑞1) −co s(𝑞1) co s(𝑞3) si n(𝑞2) − co s(𝑞1) co s(𝑞2) si n(𝑞3) 0.5co s(𝑞1) co s(𝑞2)
co s(𝑞2) co s(𝑞3) si n(𝑞1) − si n(𝑞1) si n(𝑞2) si n(𝑞3) −co s(𝑞1) −co s(𝑞3) si n(𝑞1) si n(𝑞2) − co s(𝑞2) si n(𝑞1) si n(𝑞3) 0.5co s(𝑞2) si n(𝑞1)
T[03]= ( )
−co s(𝑞3) si n(𝑞2) − co s(𝑞2) 𝑠𝑖 n(𝑞3) 0 −co s(𝑞2) co s(𝑞3) + si n(𝑞2) si n(𝑞3) −0.5si n(𝑞2) + 1.020
0 0 0 1

A partir de la transformada 4, usaremos la siguiente expresión para imprimir columna por

columna que conforman a las matrices restantes ya que son muy grandes y se hará de este modo
para poder visualizarlas de mejor forma:
for i in range(4):
cm4=T04[0:3,i]
show(f'T[04c{i+1}]= ', cm4)

for i in range(4):
cm5=T05[0:3,i]
show(f'T[05c{i+1}]= ', cm5)

for i in range(4):
cm6=T06[0:3,i]
show(f'T[06c{i+1}]= ', cm6)

𝑆
Columnas de la matriz 𝑆04𝑇

(co s(𝑞1 ) co s(𝑞2 ) co s(𝑞3 ) − co s(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) + si n(𝑞1 ) si n(𝑞4 )
T[04c1]= ( (co s(𝑞2 ) co s(𝑞3 ) si n(𝑞1 ) − si n(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) − co s(𝑞1 ) si n(𝑞4 ) )
(−co s(𝑞3 ) si n(𝑞2 ) − co s(𝑞2 ) si n(𝑞3 ))co s(𝑞4 )

co s(𝑞1 ) co s(𝑞3 ) si n(𝑞2 ) + co s(𝑞1 ) co s(𝑞2 ) si n(𝑞3 )

T[04c2]= ( co s(𝑞3 ) si n(𝑞1 ) si n(𝑞2 ) + co s(𝑞2 ) si n(𝑞1 ) si n(𝑞3 ) )
co s(𝑞2 ) co s(𝑞3 ) − si n(𝑞2 ) si n(𝑞3 )
 ROBOTICA
co s(𝑞4 ) si n(𝑞1 ) − (co s(𝑞1 ) co s(𝑞2 ) co s(𝑞3 ) − co s(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))si n(𝑞4 )
T[04c3]= (−co s(𝑞1 ) co s(𝑞4 ) − (co s(𝑞2 ) co s(𝑞3 ) si n(𝑞1 ) − si n(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))si n(𝑞4 ))
−(−co s(𝑞3 ) 𝑠𝑖 n(𝑞2 ) − co s(𝑞2 ) si n(𝑞3 ))si n(𝑞4 )

−0.485co s(𝑞1 ) co s(𝑞3 ) si n(𝑞2 ) − 0.485co s(𝑞1 ) co s(𝑞2 ) si n(𝑞3 ) + 0.5co s(𝑞1 ) co s(𝑞2 )
T[04c4]= ( −0.485co s(𝑞3 ) si n(𝑞1 ) si n(𝑞2 ) − 0.485co s(𝑞2 ) si n(𝑞1 ) si n(𝑞3 ) + 0.5co s(𝑞2 ) si n(𝑞1 ) )
−0.485co s(𝑞2 ) co s(𝑞3 ) + 0.485si n(𝑞2 ) si n(𝑞3 ) − 0.5si n(𝑞2 ) + 1.02

𝑆
Columnas de la matriz 𝑆05𝑇

1 1
((co s(𝑞1 ) co s(𝑞2 ) co s(𝑞3 ) − co s(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) + si n(𝑞1 ) si n(𝑞4 ))co s ( 𝜋 + 𝑞5 ) + (co s(𝑞1 ) co s(𝑞3 ) si n(𝑞2 ) + co s(𝑞1 ) co s(𝑞2 ) si n(𝑞3 ))si n ( 𝜋 + 𝑞5 )
2 2
1 1
T[05c1]= ((co s(𝑞2 ) co s(𝑞3 ) si n(𝑞1 ) − si n(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) − co s(𝑞1 ) si n(𝑞4 ))co s ( 𝜋 + 𝑞5 ) + (co s(𝑞3 ) si n(𝑞1 ) si n(𝑞2 ) + co s(𝑞2 ) si n(𝑞1 ) si n(𝑞3 ))si n ( 𝜋 + 𝑞5 )
2 2
1 1
(−co s(𝑞3 ) si n(𝑞2 ) − co s(𝑞2 ) si n(𝑞3 ))co s ( 𝜋 + 𝑞5 ) co s(𝑞4 ) + (co s(𝑞2 ) co s(𝑞3 ) − si n(𝑞2 ) si n(𝑞3 ))si n ( 𝜋 + 𝑞5 )
( 2 2 )

−co s(𝑞4 ) si n(𝑞1 ) + (co s(𝑞1 ) co s(𝑞2 ) co s(𝑞3 ) − co s(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))si n(𝑞4 )
T[05c2]= ( co s(𝑞1 ) co s(𝑞4 ) + (co s(𝑞2 ) co s(𝑞3 ) si n(𝑞1 ) − si n(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))si n(𝑞4 ) )
(−co s(𝑞3 ) si n(𝑞2 ) − co s(𝑞2 ) si n(𝑞3 ))si n(𝑞4 )

1 1
(co s(𝑞1 ) co s(𝑞3 ) si n(𝑞2 ) + co s(𝑞1 ) co s(𝑞2 ) si n(𝑞3 ))co s ( 𝜋 + 𝑞5 ) − ((co s(𝑞1 ) co s(𝑞2 ) co s(𝑞3 ) − co s(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) + si n(𝑞1 ) si n(𝑞4 ))si n ( 𝜋 + 𝑞5 )
2 2
1 1
T[05c3]= (co s(𝑞3 ) si n(𝑞1 ) si n(𝑞2 ) + co s(𝑞2 ) si n(𝑞1 ) si n(𝑞3 ))co s ( 𝜋 + 𝑞5 ) − ((co s(𝑞2 ) co s(𝑞3 ) si n(𝑞1 ) − si n(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) − co s(𝑞1 ) si n(𝑞4 ))si n ( 𝜋 + 𝑞5 )
2 2
1 1
−(−co s(𝑞3 ) si n(𝑞2 ) − co s(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) si n ( 𝜋 + 𝑞5 ) + (co s(𝑞2 ) co s(𝑞3 ) − si n(𝑞2 ) si n(𝑞3 ))co s ( 𝜋 + 𝑞5 )
( 2 2 )

−0.485co s(𝑞1 ) co s(𝑞3 ) si n(𝑞2 ) − 0.485co s(𝑞1 ) co s(𝑞2 ) si n(𝑞3 ) + 0.5co s(𝑞1 ) co s(𝑞2 )
T[05c4]= ( −0.485co s(𝑞3 ) si n(𝑞1 ) si n(𝑞2 ) − 0.485co s(𝑞2 ) si n(𝑞1 ) si n(𝑞3 ) + 0.5co s(𝑞2 ) si n(𝑞1 ) )
−0.485co s(𝑞2 ) co s(𝑞3 ) + 0.485si n(𝑞2 ) si n(𝑞3 ) − 0.5si n(𝑞2 ) + 1.02

𝑆
Columnas de la matriz 𝑆06𝑇 : en este caso pondremos atención en cada columna ya que
seleccionaremos aquella que dependa de q1, q2 y q3 para después obtener su matriz Jacobiana
y proceder al cálculo de estos parámetros:

Descartamos la primera columna ya que es una expresión muy grande y además depende de q5
y q6:
((𝑐𝑜 𝑠(𝑞1)𝑐𝑜 𝑠(𝑞3)𝑠𝑖 𝑛(𝑞2) + 𝑐𝑜 𝑠(𝑞1)𝑐𝑜 𝑠(𝑞2)𝑠𝑖 𝑛(𝑞3))𝑐𝑜𝑠(𝑞5) + ((𝑐𝑜 𝑠(𝑞1) 𝑐𝑜 𝑠(𝑞2)𝑐𝑜 𝑠(𝑞3) + 𝑐𝑜 𝑠(𝑞1)𝑠𝑖 𝑛(𝑞2)𝑠𝑖 𝑛(𝑞3))𝑐𝑜 𝑠(𝑞4) − 𝑠𝑖 𝑛(𝑞1)𝑠𝑖 𝑛(𝑞4))𝑠𝑖 𝑛(𝑞5)) 𝑐𝑜 𝑠(𝑞6) + (−𝑐𝑜 𝑠(𝑞4)𝑠𝑖 𝑛(𝑞1) + (𝑐𝑜 𝑠(𝑞1)𝑐𝑜 𝑠(𝑞2) 𝑐𝑜 𝑠(𝑞3 ) − 𝑐𝑜 𝑠(𝑞1)𝑠𝑖 𝑛(𝑞2)𝑠𝑖 𝑛(𝑞3))𝑠𝑖 𝑛(𝑞4))𝑠𝑖 𝑛(𝑞6)
T[06c1]= ((𝑐𝑜 𝑠(𝑞3)𝑠𝑖 𝑛(𝑞1)𝑠𝑖 𝑛(𝑞2) + 𝑐𝑜 𝑠(𝑞2)𝑠𝑖 𝑛(𝑞1)𝑠𝑖 𝑛(𝑞3))𝑐𝑜𝑠(𝑞5) + ((−𝑐𝑜 𝑠(𝑞2)𝑐𝑜 𝑠(𝑞3) 𝑠𝑖 𝑛(𝑞1) + 𝑠𝑖 𝑛(𝑞1) 𝑠𝑖 𝑛(𝑞2) 𝑠𝑖 𝑛(𝑞3))𝑐𝑜 𝑠(𝑞4) + 𝑐𝑜 𝑠(𝑞1) 𝑠𝑖 𝑛(𝑞4))𝑠𝑖 𝑛(𝑞5)) 𝑐𝑜 𝑠(𝑞6) + (𝑐𝑜 𝑠(𝑞1)𝑐𝑜 𝑠(𝑞4) + (𝑐𝑜 𝑠(𝑞2)𝑐𝑜 𝑠(𝑞3)𝑠𝑖 𝑛(𝑞1 ) − 𝑠𝑖 𝑛(𝑞1)𝑠𝑖 𝑛(𝑞2)𝑠𝑖 𝑛(𝑞3))𝑠𝑖 𝑛(𝑞4))𝑠𝑖 𝑛(𝑞6)
(−𝑐𝑜 𝑠(𝑞3)𝑠𝑖 𝑛(𝑞2) − 𝑐𝑜 𝑠(𝑞2)𝑠𝑖 𝑛(𝑞3))𝑠𝑖 𝑛(𝑞4)𝑠𝑖 𝑛(𝑞6) + ((1.0𝑐𝑜 𝑠(𝑞3)𝑠𝑖 𝑛(𝑞2) + 𝑐𝑜 𝑠(𝑞2)𝑠𝑖 𝑛(𝑞3))𝑐𝑜𝑠(𝑞4)𝑠𝑖 𝑛(𝑞5) + (𝑐𝑜 𝑠(𝑞2)𝑐𝑜 𝑠(𝑞3) − 𝑠𝑖 𝑛(𝑞2) 𝑠𝑖 𝑛(𝑞3))𝑐𝑜𝑠(𝑞5))𝑐𝑜 𝑠(𝑞6 )
( )

También descartamos la segunda columna

(−co s(𝑞4) si n(𝑞1) + (cos(𝑞1)co s(𝑞2)co s(𝑞3) − co s(𝑞1)si n(𝑞2)si n(𝑞3))sin(𝑞4))cos(𝑞6) + ((−cos(𝑞1)co s(𝑞3)si n(𝑞2) − co s(𝑞1)co s(𝑞2)si n(𝑞3))cos(𝑞5) + ((cos(𝑞1) co s(𝑞2) cos(𝑞3) − co s(𝑞1)si n(𝑞2)si n(𝑞3))cos(𝑞4) + si n(𝑞1)si n(𝑞4))si n(𝑞5))si n(𝑞6)
T[06c2]= (cos(𝑞1)co s(𝑞4) + (cos(𝑞2)co s(𝑞3)si n(𝑞1) − si n(𝑞1) si n(𝑞2)si n(𝑞3))sin(𝑞4))cos(𝑞6) + ((−co s(𝑞3) si n(𝑞1)si n(𝑞2) − co s(𝑞2)si n(𝑞1)si n(𝑞3))cos(𝑞5) + ((cos(𝑞2)co s(𝑞3)si n(𝑞1) − si n(𝑞1)si n(𝑞2)si n(𝑞3))co s(𝑞4) − co s(𝑞1)si n(𝑞4))si n(𝑞5)) si n(𝑞6)
(−cos(𝑞3 )si n(𝑞2) − co s(𝑞2)si n(𝑞3))cos(𝑞6)si n(𝑞4) + ((−co s(𝑞3) si n(𝑞2) − co s(𝑞2)si n(𝑞3))cos(𝑞4)si n(𝑞5) + (−co s(𝑞2) co s(𝑞3) + si n(𝑞2)si n(𝑞3))cos(𝑞5))si n(𝑞6)
( )

La tercera columna también queda descartada

((−𝑐𝑜 s(𝑞1 ) co s(𝑞2 ) co s(𝑞3 ) + co s(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) − si n(𝑞1 ) si n(𝑞4 ))co s(𝑞5 ) + (−co s(𝑞1 ) co s(𝑞3 ) si n(𝑞2 ) − co s(𝑞1 ) co s(𝑞2 ) si n(𝑞3 ))si n(𝑞5 )
T[06c3]= ( ((−co s(𝑞2 ) co s(𝑞3 ) si n(𝑞1 ) + si n(𝑞1 ) si n(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) + co s(𝑞1 ) si n(𝑞4 ))co s(𝑞5 ) + (−co s(𝑞3 ) si n(𝑞1 ) si n(𝑞2 ) − co s(𝑞2 ) si n(𝑞1 ) si n(𝑞3 ))si n(𝑞5 ) )
(co s(𝑞3 ) si n(𝑞2 ) + co s(𝑞2 ) si n(𝑞3 ))co s(𝑞4 ) co s(𝑞5 ) + (−co s(𝑞2 ) co s(𝑞3 ) + si n(𝑞2 ) si n(𝑞3 ))si n(𝑞5 )

Hasta la cuarta columna tendremos la columna adecuada, ya que solo depende de los tres
parámetros antes mencionado:
−0.485co s(𝑞1 ) co s(𝑞3 ) si n(𝑞2 ) − 0.485co s(𝑞1 ) co s(𝑞2 ) si n(𝑞3 ) + 0.5co s(𝑞1 ) co s(𝑞2 )
T[06c4]= ( −0.485co s(𝑞3 ) si n(𝑞1 ) si n(𝑞2 ) − 0.485co s(𝑞2 ) si n(𝑞1 ) si n(𝑞3 ) + 0.5co s(𝑞2 ) si n(𝑞1 ) )
−0.485co s(𝑞2 ) co s(𝑞3 ) + 0.485si n(𝑞2 ) si n(𝑞3 ) − 0.5si n(𝑞2 ) + 1.02
 ROBOTICA
Planteamos nuestra función F y obtenemos su matriz Jacobiana de la siguiente forma dentro de
SageMathCell:
F=T06[0:3,3:4]
J=(jacobian(F,[[q1],[q2],[q3]]))
show('F= ', F)
show('____________________________________________________________')
show('J= ', J)

Al ejecutar nos arroja los siguientes resultados:

−0.485co s(𝑞1) co s(𝑞3 ) si n(𝑞2 ) − 0.485co s(𝑞1) co s(𝑞2 ) si n(𝑞3 ) + 0.5co s(𝑞1 ) co s(𝑞2 )
F= ( −0.485co s(𝑞3) si n(𝑞1 ) si n(𝑞2) − 0.485co s(𝑞2 ) si n(𝑞1) si n(𝑞3 ) + 0.5co s(𝑞2 ) si n(𝑞1 ) )
−0.485co s(𝑞2 ) co s(𝑞3 ) + 0.485si n(𝑞2 ) si n(𝑞3 ) − 0.5si n(𝑞2 ) + 1.02
0.485co s(𝑞3 ) si n(𝑞1 )si n(𝑞2 ) + 0.485co s(𝑞2) si n(𝑞1)si n(𝑞3 ) − 0.5co s(𝑞2) si n(𝑞1) −0.485co s(𝑞1 )co s(𝑞2) co s(𝑞3) + 0.485co s(𝑞1) si n(𝑞2) si n(𝑞3) − 0.5co s(𝑞1) si n(𝑞2) −0.485co s(𝑞1)co s(𝑞2) co s(𝑞3) + 0.485co s(𝑞1) si n(𝑞2) si n(𝑞3 )
J= (−0.485co s(𝑞1) co s(𝑞3) si n(𝑞2) − 0.485co s(𝑞1)co s(𝑞2) si n(𝑞3) + 0.5co s(𝑞1) co s(𝑞2) −0.485co s(𝑞2) co s(𝑞3 )si n(𝑞1) + 0.485si n(𝑞1) si n(𝑞2 ) si n(𝑞3 ) − 0.5si n(𝑞1) si n(𝑞2) −0.485co s(𝑞2 )co s(𝑞3 )si n(𝑞1) + 0.485si n(𝑞1)si n(𝑞2 )si n(𝑞3 ) )
0 0.485co s(𝑞3) si n(𝑞2) + 0.485co s(𝑞2 )si n(𝑞3 ) − 0.5co s(𝑞2) 0.485co s(𝑞3) si n(𝑞2) + 0.485co s(𝑞2 )si n(𝑞3 )

Para los parámetros q4, q5, y q6, crearemos una matriz F2 con la diagonal de la T06 y también
obtenemos su Jacobiana para poder calcular los parámetros:
F2=Matrix([[T06[0][0]],[T06[1][1]],[T06[2][2]]])
J2=jacobian(F2,[[q4],[q5],[q6]])
Ahora obtendremos el código C de cada componente de las 4 matrices para poder hacer el cálculo
en Code::Block, escribiendo las siguientes líneas:
Para F1 y J1:
for i in range (3):
[(c_name, c_code), (h_name,
c_header)]=codegen(("F"+str(i)+str(0),F[i][0]),'C','test')
print(c_code)

for i in range (3):

for j in range (3):
[(c_name, c_code), (h_name,
c_header)]=codegen(("J"+str(i)+str(j),J[i][j]),'C','test')
print(c_code)

Para F2 y J2:
for i in range (3):
[(c_name, c_code), (h_name, c_header)]=codegen(("F2-
"+str(i)+"0",F2[i][0]),'C','test')
print(c_code)

for i in range (3):

for j in range (3):
[(c_name, c_code), (h_name, c_header)]=codegen(("J2-
"+str(i)+str(j),J2[i][j]),'C','test')
print(c_code)
 ROBOTICA
Dentro de Code::Blocks realizaremos el cálculo de los parámetros q ya que tenemos cada
componente de las F y sus jacobianas, hacemos nuestra declaración de variables y el código main
de la siguiente forma:
#include <iostream>
#include <iomanip>
#include "Matrix.h"
#include <math.h>
#define pi 3.141592654
using namespace std;
Matrix Tx(float alpha,float x);
Matrix Tz(float theta,float z);
Matrix J(float q1, float q2, float q3);
Matrix F(float q1, float q2, float q3);
Matrix F2(float q1, float q2, float q3, float q4, float q5, float q6);
Matrix J2(float q1, float q2, float q3, float q4, float q5, float q6);
int main()
{
cout << fixed << showpoint;
cout << setprecision(5);
Matrix qi(3,1);
Matrix qi2(3,1);
qi.entrada(0,0)=0.6; qi.entrada(1,0)=0.3; qi.entrada(2,0)=-0.5;
qi2.entrada(0,0)=-0.6; qi2.entrada(1,0)=0.4; qi2.entrada(2,0)=0.1;
float ep=0;
Matrix O6(3,1);
Matrix dF(3,1);
O6.entrada(0,0)=0.473761518382354+0.1;
O6.entrada(1,0)=0.324117693101218;
O6.entrada(2,0)=0.396907606416328;
qi.mostrar();
qi2.mostrar();
do
{
dF=F(qi.entrada(0,0),qi.entrada(1,0),qi.entrada(2,0))-O6;
qi=qi-J(qi.entrada(0,0),qi.entrada(1,0),qi.entrada(2,0)).inversa()*dF;
ep=dF.magnitud();
cout<<"qi1 (q1,q2,q3) = "<<endl;
qi.mostrar();

}while(ep>0.001);
float q1=qi.entrada(0,0);
float q2=qi.entrada(1,0);
float q3=qi.entrada(2,0);

Matrix Rdiag(3,1);
Rdiag.entrada(0,0)=-0.377534699926054;
Rdiag.entrada(1,0)=0.4130596162053405;
Rdiag.entrada(2,0)=-0.5326812364391368;
Matrix dF2(3,1);
do
{
dF2=F2(q1,q2,q3,qi2.entrada(0,0),qi2.entrada(1,0),qi2.entrada(2,0))-Rdiag;
qi2=qi2-
J2(q1,q2,q3,qi2.entrada(0,0),qi2.entrada(1,0),qi2.entrada(2,0)).inversa()*dF2;
ep=dF2.magnitud();
cout<<"qi2 (q4,q5,q6) = "<<endl;
qi2.mostrar();

}while(ep>0.001);
return 0;
 ROBOTICA
Creamos las funciones F y Jacobianas e introducimos los valores obtenidos en SageMathCell:
Matrix J(float q1, float q2, float q3){
Matrix j(3,3);
j.entrada(0,0)=0.48499999999999999*sin(q1)*sin(q2)*cos(q3) +
0.48499999999999999*sin(q1)*sin(q3)*cos(q2) - 0.5*sin(q1)*cos(q2);
j.entrada(0,1)=0.48499999999999999*sin(q2)*sin(q3)*cos(q1) - 0.5*sin(q2)*cos(q1) -
0.48499999999999999*cos(q1)*cos(q2)*cos(q3);
j.entrada(0,2)=0.48499999999999999*sin(q2)*sin(q3)*cos(q1) -
0.48499999999999999*cos(q1)*cos(q2)*cos(q3);

j.entrada(1,0)=-0.48499999999999999*sin(q2)*cos(q1)*cos(q3) -
0.48499999999999999*sin(q3)*cos(q1)*cos(q2) + 0.5*cos(q1)*cos(q2);
j.entrada(1,1)=0.48499999999999999*sin(q1)*sin(q2)*sin(q3) - 0.5*sin(q1)*sin(q2) -
0.48499999999999999*sin(q1)*cos(q2)*cos(q3);
j.entrada(1,2)=0.48499999999999999*sin(q1)*sin(q2)*sin(q3) -
0.48499999999999999*sin(q1)*cos(q2)*cos(q3);

j.entrada(2,0)=0;
j.entrada(2,1)=0.48499999999999999*sin(q2)*cos(q3) + 0.48499999999999999*sin(q3)*cos(q2)
- 0.5*cos(q2);
j.entrada(2,2)=0.48499999999999999*sin(q2)*cos(q3) + 0.48499999999999999*sin(q3)*cos(q2);
return j;}
Matrix F(float q1, float q2, float q3){
Matrix f(3,1);
f.entrada(0,0)=-0.48499999999999999*sin(q2)*cos(q1)*cos(q3) -
0.48499999999999999*sin(q3)*cos(q1)*cos(q2) + 0.5*cos(q1)*cos(q2);
f.entrada(1,0)=-0.48499999999999999*sin(q1)*sin(q2)*cos(q3) -
0.48499999999999999*sin(q1)*sin(q3)*cos(q2) + 0.5*sin(q1)*cos(q2);
f.entrada(2,0)=0.48499999999999999*sin(q2)*sin(q3) - 0.5*sin(q2) -
0.48499999999999999*cos(q2)*cos(q3) + 1.02;
return f;}
Matrix F2(float q1, float q2, float q3, float q4, float q5, float q6){
Matrix f(3,1);
f.entrada(0,0)=(((1.0*sin(q2)*sin(q3)*cos(q1) - 1.0*cos(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q1)*sin(q4))*sin(q5) + (1.0*sin(q2)*cos(q1)*cos(q3) +
1.0*sin(q3)*cos(q1)*cos(q2))*cos(q5))*cos(q6) + ((-1.0*sin(q2)*sin(q3)*cos(q1) +
1.0*cos(q1)*cos(q2)*cos(q3))*sin(q4) - 1.0*sin(q1)*cos(q4))*sin(q6);
f.entrada(1,0)=(((-1.0*sin(q1)*sin(q2)*sin(q3) + 1.0*sin(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q4)*cos(q1))*sin(q5) + (-1.0*sin(q1)*sin(q2)*cos(q3) -
1.0*sin(q1)*sin(q3)*cos(q2))*cos(q5))*sin(q6) + ((-1.0*sin(q1)*sin(q2)*sin(q3) +
1.0*sin(q1)*cos(q2)*cos(q3))*sin(q4) + 1.0*cos(q1)*cos(q4))*cos(q6);
f.entrada(2,0)=(1.0*sin(q2)*sin(q3) - 1.0*cos(q2)*cos(q3))*sin(q5) + (1.0*sin(q2)*cos(q3)
+ 1.0*sin(q3)*cos(q2))*cos(q4)*cos(q5);
return f;}
Matrix J2(float q1, float q2, float q3, float q4, float q5, float q6){
Matrix j(3,3);
j.entrada(0,0)=((-1.0*sin(q2)*sin(q3)*cos(q1) + 1.0*cos(q1)*cos(q2)*cos(q3))*cos(q4) +
1.0*sin(q1)*sin(q4))*sin(q6) + (-(1.0*sin(q2)*sin(q3)*cos(q1) -
1.0*cos(q1)*cos(q2)*cos(q3))*sin(q4) - 1.0*sin(q1)*cos(q4))*sin(q5)*cos(q6);
j.entrada(0,1)=(((1.0*sin(q2)*sin(q3)*cos(q1) - 1.0*cos(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q1)*sin(q4))*cos(q5) - (1.0*sin(q2)*cos(q1)*cos(q3) +
1.0*sin(q3)*cos(q1)*cos(q2))*sin(q5))*cos(q6);
j.entrada(0,2)=-(((1.0*sin(q2)*sin(q3)*cos(q1) - 1.0*cos(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q1)*sin(q4))*sin(q5) + (1.0*sin(q2)*cos(q1)*cos(q3) +
1.0*sin(q3)*cos(q1)*cos(q2))*cos(q5))*sin(q6) + ((-1.0*sin(q2)*sin(q3)*cos(q1) +
1.0*cos(q1)*cos(q2)*cos(q3))*sin(q4) - 1.0*sin(q1)*cos(q4))*cos(q6);

j.entrada(1,0)=(-(-1.0*sin(q1)*sin(q2)*sin(q3) + 1.0*sin(q1)*cos(q2)*cos(q3))*sin(q4) -
1.0*cos(q1)*cos(q4))*sin(q5)*sin(q6) + ((-1.0*sin(q1)*sin(q2)*sin(q3) +
1.0*sin(q1)*cos(q2)*cos(q3))*cos(q4) - 1.0*sin(q4)*cos(q1))*cos(q6);
j.entrada(1,1)=(((-1.0*sin(q1)*sin(q2)*sin(q3) + 1.0*sin(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q4)*cos(q1))*cos(q5) - (-1.0*sin(q1)*sin(q2)*cos(q3) -
1.0*sin(q1)*sin(q3)*cos(q2))*sin(q5))*sin(q6);
j.entrada(1,2)=(((-1.0*sin(q1)*sin(q2)*sin(q3) + 1.0*sin(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q4)*cos(q1))*sin(q5) + (-1.0*sin(q1)*sin(q2)*cos(q3) -
1.0*sin(q1)*sin(q3)*cos(q2))*cos(q5))*cos(q6) - ((-1.0*sin(q1)*sin(q2)*sin(q3) +
1.0*sin(q1)*cos(q2)*cos(q3))*sin(q4) + 1.0*cos(q1)*cos(q4))*sin(q6);

j.entrada(2,0)= -(1.0*sin(q2)*cos(q3) + 1.0*sin(q3)*cos(q2))*sin(q4)*cos(q5);

j.entrada(2,1)=(1.0*sin(q2)*sin(q3) - 1.0*cos(q2)*cos(q3))*cos(q5) - (1.0*sin(q2)*cos(q3)
+ 1.0*sin(q3)*cos(q2))*sin(q5)*cos(q4);
j.entrada(2,2)=0;
return j;}
 ROBOTICA
Al ejecutar el programa veremos como convergen las soluciones de una manera muy rápida y

posición E6 1
 ROBOTICA
Ahora haremos los mismo dentro de un scrip en CoppeliaSimu. Ya que conocemos la pocion a la
que queremos llevar al eslabón 6, añadimos lo siguiente a nuestro Sript:
function sysCall_init()
sim = require('sim')

j1=sim.getObject('../Revolute_joint') -- son tipos enteros,

numeros enteros que representan una direcci?n de memoria
j2=sim.getObject('../E1/Revolute_joint')
j3=sim.getObject('../E2/Revolute_joint')
j4=sim.getObject('../E3/Revolute_joint')
j5=sim.getObject('../E4/Revolute_joint')
j6=sim.getObject('../E5/Revolute_joint')

qi={0.6,0.3,-0.5} -- Vector 3x1

q1=qi[1];q2=qi[2];q3=qi[3];
qi2={-0.6,0.4,0.1}
q4=qi2[1];q5=qi2[2];q6=qi2[3];

sim.setJointPosition(j1,q1) -- manda tu configuración a la

configuración presentada anteriormente (qi)
sim.setJointPosition(j2,q2)
sim.setJointPosition(j3,q3)
sim.setJointPosition(j4,q4)
sim.setJointPosition(j5,q5)
sim.setJointPosition(j6,q6)

E6=sim.getObject('../E6')

rotationMatrix = sim.getObjectMatrix(E6, -1)

t=0
dt=0.000001
O6={rotationMatrix[4],rotationMatrix[8],rotationMatrix[12]}

-- do some initialization here

curveHandle = sim.addDrawingObject(sim.drawing_lines, 2.0, 0, -1,
1000000, {1,0,0})
end

Dentro de esta función principal se incluye la declaración de la posición del eslabón 6, que
corresponde a las obtenidas en code::Blocks:
 ROBOTICA
Añadimos esto a las articulaciones de nuestro robot en el orden en el que se muestran en las
imágenes anteriores, de arriba hacia abajo y de izquierda a derecha:
Articulación 1

Articulación 2

Articulación 3

Articulación 4
 ROBOTICA
Articulación 5

Articulación 6

Ahora iniciamos nuestro ciclo de Newton-Raspshon dentro del script, como se muestra a
continuación:
 ROBOTICA
function sysCall_actuation()
sin = math.sin
cos = math.cos
sqrt = math.sqrt

t=t+dt
O=sumVectors3x1(O6,{0,-t,0})
sim.addDrawingObjectItem(curveHandle,
{O6[1],O6[2],O6[3],O[1],O[2],O[3]})
O6=O

-- Bucle while que se ejecuta mientras a sea mayor o

igual a 0.001
k=0
while true do
local q1= qi[1]
local q2= qi[2]
local q3= qi[3]
local q4= qi2[1]
local q5= qi2[2]
local q6= qi2[3]
J={ --Matriz 3x3;

--SE METEN LA MATRIZ 3X3 DE ECUACIONES JACOBIANAS OBTENIDAS,

SEPARADAS POR UNA COMA (,) EN VEZ DE PUNTO Y COMA.
--ESTAS ECUACIONES SON LAS OBTENIDAS EN SAGEMATHCELL
0.48499999999999999*sin(q1)*sin(q2)*cos(q3) +
0.48499999999999999*sin(q1)*sin(q3)*cos(q2) -
0.5*sin(q1)*cos(q2),
0.48499999999999999*sin(q2)*sin(q3)*cos(q1) -
0.5*sin(q2)*cos(q1) -
0.48499999999999999*cos(q1)*cos(q2)*cos(q3),
0.48499999999999999*sin(q2)*sin(q3)*cos(q1) -
0.48499999999999999*cos(q1)*cos(q2)*cos(q3),

-0.48499999999999999*sin(q2)*cos(q1)*cos(q3) -
0.48499999999999999*sin(q3)*cos(q1)*cos(q2) +
0.5*cos(q1)*cos(q2),
0.48499999999999999*sin(q1)*sin(q2)*sin(q3) -
0.5*sin(q1)*sin(q2) -
0.48499999999999999*sin(q1)*cos(q2)*cos(q3),
0.48499999999999999*sin(q1)*sin(q2)*sin(q3) -
0.48499999999999999*sin(q1)*cos(q2)*cos(q3),

0,
0.48499999999999999*sin(q2)*cos(q3) +
0.48499999999999999*sin(q3)*cos(q2) - 0.5*cos(q2),
0.48499999999999999*sin(q2)*cos(q3) +
0.48499999999999999*sin(q3)*cos(q2)
}
 ROBOTICA
J2={
((-1.0*sin(q2)*sin(q3)*cos(q1) + 1.0*cos(q1)*cos(q2)*cos(q3))*sin(q4) -
1.0*sin(q1)*cos(q4))*sin(q5)*cos(q6) + ((-1.0*sin(q2)*sin(q3)*cos(q1) +
1.0*cos(q1)*cos(q2)*cos(q3))*cos(q4) + 1.0*sin(q1)*sin(q4))*sin(q6),
(((1.0*sin(q2)*sin(q3)*cos(q1) - 1.0*cos(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q1)*sin(q4))*cos(q5) + (-1.0*sin(q2)*cos(q1)*cos(q3) -
1.0*sin(q3)*cos(q1)*cos(q2))*sin(q5))*cos(q6),
(((-1.0*sin(q2)*sin(q3)*cos(q1) + 1.0*cos(q1)*cos(q2)*cos(q3))*cos(q4) +
1.0*sin(q1)*sin(q4))*sin(q5) + (-1.0*sin(q2)*cos(q1)*cos(q3) -
1.0*sin(q3)*cos(q1)*cos(q2))*cos(q5))*sin(q6) + ((-
1.0*sin(q2)*sin(q3)*cos(q1) + 1.0*cos(q1)*cos(q2)*cos(q3))*sin(q4) -
1.0*sin(q1)*cos(q4))*cos(q6),

((-1.0*sin(q1)*sin(q2)*sin(q3) + 1.0*sin(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q4)*cos(q1))*cos(q6) + ((1.0*sin(q1)*sin(q2)*sin(q3) -
1.0*sin(q1)*cos(q2)*cos(q3))*sin(q4) -
1.0*cos(q1)*cos(q4))*sin(q5)*sin(q6),
(((-1.0*sin(q1)*sin(q2)*sin(q3) + 1.0*sin(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q4)*cos(q1))*cos(q5) + (1.0*sin(q1)*sin(q2)*cos(q3) +
1.0*sin(q1)*sin(q3)*cos(q2))*sin(q5))*sin(q6),
(((-1.0*sin(q1)*sin(q2)*sin(q3) + 1.0*sin(q1)*cos(q2)*cos(q3))*cos(q4) -
1.0*sin(q4)*cos(q1))*sin(q5) + (-1.0*sin(q1)*sin(q2)*cos(q3) -
1.0*sin(q1)*sin(q3)*cos(q2))*cos(q5))*cos(q6) +
((1.0*sin(q1)*sin(q2)*sin(q3) - 1.0*sin(q1)*cos(q2)*cos(q3))*sin(q4) -
1.0*cos(q1)*cos(q4))*sin(q6),

(-1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2))*sin(q4)*cos(q5),
(1.0*sin(q2)*sin(q3) - 1.0*cos(q2)*cos(q3))*cos(q5) + (-
1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2))*sin(q5)*cos(q4),
0,
}
-- CALCULAR LA INVERSA
local Jinv, errorMessage = inverse3x3(J)
local J2inv, errorMessage = inverse3x3(J2)
if Jinv then
--print("Matriz inversa:")
--printMatrix3x3(Jinv)
else
print(errorMessage)
print("inversa no existe")
break
end
if J2inv then
--print("Matriz inversa:")
--printMatrix3x3(Jinv)
else
print(errorMessage)
print("inversa no existe")
break
end
local O6i=F(qi)
local dT=restaVectors3x1(O6i,O6)
 ROBOTICA
-- Simulaci?n de alg?n procesamiento que diminuye el vlor de a
local v1= restaVectors3x1 (qi, multiplyMatrixVector3x3(Jinv, dT))
qi=v1

local v1_2= restaVectors3x1 (qi2, multiplyMatrixVector3x3(J2inv, dT))

qi2=v1_2

k=k+1
if(k>200) then
sim.stopSimulation()
print(" fin simulacion ")
break;
end

O6i=F(qi)
dT=restaVectors3x1(O6,O6i)
local d= sqrt(dT[1]^2+dT[2]^2+dT[3]^2)
--verifica si a es menor que 0.001
if d < 0.001 then

break
end

O6i=F2(qi2)
dT=restaVectors3x1(O6,O6i)
local d2= sqrt(dT[1]^2+dT[2]^2+dT[3]^2)
--verifica si a es menor que 0.001
if d2 < 0.001 then

break
end

end -- fin del ciclo infinito newton-rapshon

local q1=qi[1]
local q2=qi[2]
local q3=qi[3]
local q4=qi2[1]
local q5=qi2[2]
--local q6=qi2[3]
sim.setJointTargetPosition(j1,q1)
sim.setJointTargetPosition(j2,q2)
sim.setJointTargetPosition(j3,q3)
sim.setJointTargetPosition(j4,q4)
sim.setJointTargetPosition(j5,q5)
--sim.setJointTargetPosition(j6,q6)

end
 ROBOTICA
Las siguientes son funciones de operaciones matriciales:

Al final añadimos nuestras funciones F:

function F(a)
local f_ = {0,0,0}

q1= a[1];q2=a[2];q3=a[3];
f_[1]=-0.48499999999999999*sin(q2)*cos(q1)*cos(q3) -
0.48499999999999999*sin(q3)*cos(q1)*cos(q2) + 0.5*cos(q1)*cos(q2);
f_[2]=-0.48499999999999999*sin(q1)*sin(q2)*cos(q3) -
0.48499999999999999*sin(q1)*sin(q3)*cos(q2) + 0.5*sin(q1)*cos(q2);
f_[3]=0.48499999999999999*sin(q2)*sin(q3) - 0.5*sin(q2) -
0.48499999999999999*cos(q2)*cos(q3) + 1.02;
--aqui van las funciones F obtenidas desde sagemath, siempre separados por una
coma

return f_

end

function F2(a)
local f2_ = {0,0,0}

q4= a[1];q5=a[2];q6=a[3];
f2_[1]=(((1.0*sin(q2)*sin(q3)*cos(q1) -
1.0*cos(q1)*cos(q2)*cos(q3))*cos(q4) - 1.0*sin(q1)*sin(q4))*sin(q5) +
(1.0*sin(q2)*cos(q1)*cos(q3) + 1.0*sin(q3)*cos(q1)*cos(q2))*cos(q5))*cos(q6) +
((-1.0*sin(q2)*sin(q3)*cos(q1) + 1.0*cos(q1)*cos(q2)*cos(q3))*sin(q4) -
1.0*sin(q1)*cos(q4))*sin(q6);
f2_[2]=(((-1.0*sin(q1)*sin(q2)*sin(q3) + 1.0*sin(q1)*cos(q2)*cos(q3))*cos(q4)
- 1.0*sin(q4)*cos(q1))*sin(q5) + (-1.0*sin(q1)*sin(q2)*cos(q3) -
1.0*sin(q1)*sin(q3)*cos(q2))*cos(q5))*sin(q6) + ((-1.0*sin(q1)*sin(q2)*sin(q3) +
1.0*sin(q1)*cos(q2)*cos(q3))*sin(q4) + 1.0*cos(q1)*cos(q4))*cos(q6);
f2_[3]=(1.0*sin(q2)*sin(q3) - 1.0*cos(q2)*cos(q3))*sin(q5) +
(1.0*sin(q2)*cos(q3) + 1.0*sin(q3)*cos(q2))*cos(q4)*cos(q5);
--aqui van las funciones F obtenidas desde sagemath, siempre separados por una
coma

return f2_

end

-- funcion para imprimir un vector 3x1

function printVector3x1(vector)
for i = 1, 3 do
print(string.format("%f", vector[i]))
end
end
 ROBOTICA
Finalmente ejecutamos el simulador y nuestro robot comenzara a moverte de acuerdo con el eje
que le hayamos indicado dentro del script, y para visualizar ese movimiento se cambia la cámara
además de que se programó para que se dibuje una línea por donde pasa el actuador final:
 ROBOTICA
EJERCICIO 2
Las figuras muestran las velocidades angulares relativas que tiene cada eslabón respecto del
anterior, así como la configuración instantánea por cada eslabón.

Calcular la velocidad angular del efector final respecto del sistema de referencia global.
1. Procedemos con el diseño de cada uno de los eslabones como lo hicimos con el robot
anterior, así como con la colocación de cada uno de sus sistemas de referencia que en
este caso ya fueron proporcionados por el mismo problema, quedando de la siguiente
manera

Eslabón 0(base)
 ROBOTICA
Eslabón 1

Eslabón 2
 ROBOTICA

Eslabón 3
 ROBOTICA

Eslabón 4
 ROBOTICA
Eslabón 5

Eslabón 6
 ROBOTICA
2. Para realizar este ejercicio lo primero que realizamos fue nuestra tabla de parámetros de
Denavit-Hatenberg, quedando de la siguiente manera:

D-H Robot 2
E x z θ α
1 0 0.65 0 -π/2
2 0.45 0.35 -π/6 0
3 0.31 0 -2π/9 0
4 0 0 0 -π/2
5 0 0.51 0 0
6 0 0.13 0 0

3. Una vez elaborada nuestra tabla, importamos nuestras piezas en el software de

CoppeliaSimu y realizamos el ensamble en base a los parámetros, únicamente dejando
las rotaciones en Z (θ), quedándonos de la siguiente manera:

E x z θ α
1 0 0.65 0 -π/2
2 0.45 0.35 0 0
3 0.31 0 0 0
4 0 0 0 -π/2
5 0 0.51 0 0
6 0 0.13 0 0
 ROBOTICA
4. Añadimos un script dentro de coppelia donde obtendremos la velocidad angular de cada
uno de los eslabones a través del tiempo, y con ellos poder calcular la velocidad del
efector final:

5. Para verificar, también se realizó un código en SageMathCell:

 ROBOTICA

Colocamos nuestra tabla de Denavit:

Las coordenadas generalizadas (en home):

Se crean las matrices de transformación, se calcula la velocidad y se imprimen los resultados:

 ROBOTICA

Para el momento exacto en el que se desea saber la velocidad del efector final, agregaremos los
grados que se nos muestra en el ejercicio sobre las articulaciones revolutas correspondientes en
CoppeliaSimu, quedándonos de la siguiente manera:
 ROBOTICA
Daremos play en la simulación y enseguida la pausamos para poder visualizar en la consola las
coordenadas generalizadas que debemos añadir en el código de SageMath. Para ese momento,
se nos muestran estos resultados:

1.86
𝜔6 = [ 1.81 ] 𝑟𝑎𝑑/𝑠
−0.52

Y dentro de SageMath nos da el siguiente resultado:

1.863
𝜔6 = [ 1.803 ] 𝑟𝑎𝑑/𝑠
−0.5176

Para verificar de otra forma, reemplazamos los valores de q a como lo teníamos en la primera
tabla:
E x z θ α
1 0 0.65 0 -π/2
2 0.45 0.35 -π/6 0
3 0.31 0 -2π/9 0
4 0 0 0 -π/2
5 0 0.51 0 0
6 0 0.13 0 0
 ROBOTICA
1.87938524157182
1.80000000000000
Ejecutamos y nos vuelve a quedar el siguiente vector: ( )
−0.484040286651338
0.000000000000000

CINEMATICA INVERSA
Ahora a partir de esto podemos aplicar cinemática inversa obteniendo primero la matriz T06 que
también ya está programada, cada punto con referencia al origen 0:

1.00000000000000 0.000000000000000 0.000000000000000 0.000000000000000

0.000000000000000 0.000000000000000 1.00000000000000 0.000000000000000
T01= ( )
0.000000000000000 −1.00000000000000 0.000000000000000 0.650000000000000
0.000000000000000 0.000000000000000 0.000000000000000 1.00000000000000

1.00000000000000 0.000000000000000 0.000000000000000 0.450000000000000

0.000000000000000 0.000000000000000 1.00000000000000 0.350000000000000
T02= ( )
0.000000000000000 −1.00000000000000 0.000000000000000 0.650000000000000
0.000000000000000 0.000000000000000 0.000000000000000 1.00000000000000

0.866025403784439 0.500000000000000 0.000000000000000 0.718467875173176

0.000000000000000 0.000000000000000 1.00000000000000 0.350000000000000
T03= ( )
0.500000000000000 −0.866025403784439 0.000000000000000 0.805000000000000
0.000000000000000 0.000000000000000 0.000000000000000 1.00000000000000

0.342020143325669 0.000000000000000 0.939692620785908 0.718467875173176

0.000000000000000 −1.00000000000000 0.000000000000000 0.350000000000000
T04= ( )
0.939692620785908 0.000000000000000 −0.342020143325669 0.805000000000000
0.000000000000000 0.000000000000000 0.000000000000000 1.00000000000000

0.342020143325669 0.000000000000000 0.939692620785908 1.19771111177399

0.000000000000000 −1.00000000000000 0.000000000000000 0.350000000000000
T05= ( )
0.939692620785908 0.000000000000000 −0.342020143325669 0.630569726903909
0.000000000000000 0.000000000000000 0.000000000000000 1.00000000000000

0.342020143325669 0.000000000000000 0.939692620785908 1.31987115247616

0.000000000000000 −1.00000000000000 0.000000000000000 0.350000000000000
T06= ( )
0.939692620785908 0.000000000000000 −0.342020143325669 0.586107108271572
0.000000000000000 0.000000000000000 0.000000000000000 1.00000000000000

Poniendo atención especial en la T06, definiremos la matriz desde un principio y dejaremos

indicadas cada una de las q sabiendo que:
𝑆60 𝑇 = 𝑆10 𝑇 ⋅ 𝑆21 𝑇 ⋅ 𝑆32 𝑇 ⋅ 𝑆43 𝑇 ⋅ 𝑆54 𝑇 ⋅ 𝑆65 𝑇
Podemos aplicar desacoplamientos en las últimas tres articulaciones debido a que cuentan con
el mismo sistema de referencia y todos se trasladan al origen 3 por lo que la ecuación se simplifica
bastante quedándonos de la siguiente manera:
𝑆30 𝑇 = 𝑆10 𝑇 ⋅ 𝑆21 𝑇 ⋅ 𝑆32 𝑇
Con base a esto podemos inferir que 𝑞4 = 𝑞5 = 𝑞6 = 0 y ahora procedemos a calcular las q
restantes.
En primer lugar, debemos de conocer el origen O3, sabemos que el único origen es conocido y lo
obtenemos de la siguiente manera con SageMath
Tomamos a T06 y extraemos el origen auxiliándonos de la formula conocida:
 ROBOTICA
𝐵0 𝐵0
𝑆60 𝑇 = 𝐵6 𝑅 𝑂1 𝑟𝑂6
01×3 1
Extrayéndola con el siguiente comando:

0.342020143325669 0.000000000000000 0.939692620785908 1.09714095343694

0.000000000000000 −1.00000000000000 0.000000000000000 0.350000000000000
⬚06 𝑇 = ( )
0.939692620785908 0.000000000000000 −0.342020143325669 0.947411820715204
0.000000000000000 0.000000000000000 0.000000000000000 1.00000000000000

Este no es el origen que estamos buscando, sino el O3, y de acuerdo con el diagrama del robot
podemos inferir lo siguiente:

̂6
𝑂3 = 𝑂6 − (0.64) ∙ 𝑘
Y en SageMath las líneas que nos ayudan a extraer O3 son las siguientes:

Dándonos como resultado lo siguiente:

0.495737676133959
O3= (0.350000000000000)
1.16630471244363
Ahora sustituyendo q4, q5 y q6 nos queda
1.00000000000000co s(𝑞1 ) co s(𝑞2 ) co s(𝑞3 ) − 1.00000000000000co s(𝑞1 ) si n(𝑞2 ) si n(𝑞3 )
1.00000000000000co s(𝑞2 ) co s(𝑞3 ) si n(𝑞1 ) − 1.00000000000000si n(𝑞1 ) si n(𝑞2 ) si n(𝑞3 )
T03[0] = ( )
−1.00000000000000co s(𝑞3 ) si n(𝑞2 ) − 1.00000000000000co s(𝑞2 ) si n(𝑞3 )
0
 ROBOTICA
−1.00000000000000co s(𝑞1 ) co s(𝑞3 ) si n(𝑞2 ) − 1.00000000000000co s(𝑞1 ) co s(𝑞2 ) si n(𝑞3 )
−1.00000000000000co s(𝑞3 ) si n(𝑞1 ) si n(𝑞2 ) − 1.00000000000000co s(𝑞2 ) si n(𝑞1 ) si n(𝑞3 )
T03[1] = ( )
−1.00000000000000co s(𝑞2 ) co s(𝑞3 ) + 1.00000000000000si n(𝑞2 ) si n(𝑞3 )
0

−1.00000000000000sin (𝑞1 )
T03[2] = ( 1.00000000000000cos (𝑞1 ) )
0
0
0.310000000000000cos (𝑞1 )cos (𝑞2 )cos (𝑞3 ) − 0.310000000000000cos (𝑞1 )sin (𝑞2 )sin (𝑞3 ) + 0.450000000000000cos (𝑞1 )cos (𝑞2 ) − 0.350000000000000sin (𝑞1 )
0.310000000000000cos (𝑞2 )cos (𝑞3 )sin (𝑞1 ) − 0.310000000000000sin (𝑞1 )sin (𝑞2 )sin (𝑞3 ) + 0.450000000000000cos (𝑞2 )sin (𝑞1 ) + 0.350000000000000cos (𝑞1 )
T03[3] = ( )
−0.310000000000000cos (𝑞3 )sin (𝑞2 ) − 0.310000000000000cos (𝑞2 )sin (𝑞3 ) − 0.450000000000000sin (𝑞2 ) + 0.650000000000000
1.00000000000000

Como se pude observar, la matriz queda en términos de q1, q2 y q3.

Para poder encontrar q1 realizamos lo siguiente:
𝑆10 𝑇 −1 ∙ 𝑆30 𝑇 = 𝑆21 𝑇 ⋅ 𝑆32 𝑇
En SageMath se realiza de la siguiente manera:

0.342020143325669co s(𝑞1 ) 0.939692620785908co s(𝑞1 ) 1.0si n(𝑞1 ) 0.4957376761339589co s(𝑞1 ) + 0.35si n(𝑞1 )
A =( −0.939692620785908 0.342020143325669 0 −0.5163047124436321 )
−0.342020143325669si n(𝑞1 ) −0.939692620785908si n(𝑞1 ) 1.0co s(𝑞1 ) 0.35co s(𝑞1 ) − 0.4957376761339589si n(𝑞1 )
0 0 0 1.0
1.0cos (𝑞2 )cos (𝑞3 ) − 1.0sin (𝑞2 )sin (𝑞3 ) −1.0cos (𝑞3 )sin (𝑞2 ) − 1.0cos (𝑞2 )sin (𝑞3 ) 0 0.31cos (𝑞2 )cos (𝑞3 ) − 0.31sin (𝑞2 )sin (𝑞3 ) + 0.45cos (𝑞2 )
1.0cos (𝑞3 )sin (𝑞2 ) + 1.0cos (𝑞2 )sin (𝑞3 ) 1.0cos (𝑞2 )cos (𝑞3 ) − 1.0sin (𝑞2 )sin (𝑞3 ) 0 0.31cos (𝑞3 )sin (𝑞2 ) + 0.31cos (𝑞2 )sin (𝑞3 ) + 0.45sin (𝑞2 )
T13 = ( )
0 0 1.0 0.35
0 0 0 1.0

Igualamos los términos de la matriz A con los términos que son constantes en la matriz T13:
−0.342020143325669𝑠𝑖 𝑛(𝑞1 ) = 0

⇒ 𝑞1 = 0

Sustituimos en A:

0.342020143325669 0.939692620785908 0 0.4957376761339589

−0.939692620785908 0.342020143325669 0 −0.5163047124436321
Anum = ( )
0 0 1.0 0.35
0 0 0 1.0

Lo siguiente es realizar

𝑆10 𝑇 −1 ∙ 𝑆21 𝑇 −1 ∙ 𝑆30 𝑇 = 𝑆32 𝑇

44668708 8449857 8449857 44668708 12840747 15760887 9

co s(𝑞2 ) − si n(𝑞2 ) co s(𝑞2 ) + si n(𝑞2 ) 0 co s(𝑞2 ) − si n(𝑞2) −
130602565 8992150 8992150 130602565 25902302 30526328 20
8449857 44668708 44668708 8449857 15760887 12840747
B= − co s(𝑞2 ) − si n(𝑞2 ) co s(𝑞2) − si n(𝑞2 ) 0 − co s(𝑞2 ) − si n(𝑞2 )
8992150 130602565 130602565 8992150 30526328 25902302
0 0 1.0 0
( 0 0 0 1.0 )
 ROBOTICA
1.00000000000000cos (𝑞3 ) −1.00000000000000sin (𝑞3 ) 0 0.310000000000000cos (𝑞3)
T23 = ( 1.00000000000000sin (𝑞3 ) 1.00000000000000cos (𝑞3 ) 0 0.310000000000000sin (𝑞3 )
)
0 0 1.00000000000000 0
0 0 0 1.00000000000000

Cómo se puede observar, en este caso ninguna de las ecuaciones queda en términos de una sola
incógnita por lo que procederemos a plantear un sistema de ecuaciones de 2x2 quedado de la
siguiente manera:

44668708 8449857
𝑒𝑞1 = 𝑐𝑜𝑠(𝑞2 ) − 𝑠𝑖𝑛(𝑞2 ) = 𝑐𝑜𝑠(𝑞3 )
130602565 8992150

8449857 44668708
𝑒𝑞2 = − 𝑐𝑜𝑠(𝑞2 ) − 𝑠𝑖𝑛(𝑞2 ) = 𝑠𝑖𝑛(𝑞3 )
8992150 130602565

44668708 8449857
𝑎= ; 𝑏=
130602565 8992150

𝑎 𝑐𝑜𝑠(𝑞2 ) − 𝑏 𝑠𝑖𝑛(𝑞2 ) = 𝑐𝑜𝑠(𝑞3 ) 𝑐𝑜𝑠(𝑞2 )

⟹ { ( )
−𝑏 𝑐𝑜𝑠(𝑞2 ) − 𝑎 𝑠𝑖𝑛(𝑞2 ) = 𝑠𝑖𝑛(𝑞3 ) −𝑠𝑖𝑛(𝑞2 )

𝑎 𝑐𝑜𝑠2 (𝑞2 ) − 𝑏 𝑠𝑖𝑛(𝑞2 ) 𝑐𝑜𝑠(𝑞2 ) = 𝑐𝑜𝑠(𝑞3 ) 𝑐𝑜𝑠(𝑞2 )

⟹ {
𝑏 𝑐𝑜𝑠(𝑞2 )𝑠𝑖𝑛(𝑞2 ) + 𝑎 𝑠𝑖𝑛2 (𝑞2 ) = − 𝑠𝑖𝑛(𝑞3 ) 𝑠𝑖𝑛(𝑞2 )

𝑎 𝑐𝑜𝑠2 (𝑞2 ) = 𝑐𝑜𝑠(𝑞3 ) 𝑐𝑜𝑠(𝑞2 )

⟹ {
𝑎 𝑠𝑖𝑛2 (𝑞2 ) = − 𝑠𝑖𝑛(𝑞3 ) 𝑠𝑖𝑛(𝑞2 )

𝑎 [𝑐𝑜𝑠2 (𝑞2 ) + 𝑠𝑖𝑛2 (𝑞2 )] = 𝑐𝑜𝑠(𝑞3 ) 𝑐𝑜𝑠(𝑞2 ) − 𝑠𝑖𝑛(𝑞3 ) 𝑠𝑖𝑛(𝑞2 )

⟹ 𝑎 = 𝑐𝑜𝑠(𝑞3 ) 𝑐𝑜𝑠(𝑞2 ) − 𝑠𝑖𝑛(𝑞3 ) 𝑠𝑖𝑛(𝑞2 )

𝑎 = 𝑐𝑜𝑠(𝑞3 + 𝑞2 )

⟹ 𝑎𝑟𝑐𝑐𝑜𝑠(𝑎) = 𝑞3 + 𝑞2

𝑐 = 𝑎𝑟𝑐𝑐𝑜𝑠(𝑎)

⟹ 𝑞3 = −𝑞2 + 𝑐
Sustituimos en eq1:

𝑎 cos(𝑞2 ) − 𝑏 sin(𝑞2 ) = cos(𝑐 − 𝑞2 )

𝑎 cos(𝑞2 ) − 𝑏 sin(𝑞2 ) = cos(𝑐) cos(𝑞2 ) + sin(𝑐) sin(𝑞2 )

 ROBOTICA
𝑎 cos(𝑞2 ) − 𝑏 sin(𝑞2 ) − cos(𝑐) cos(𝑞2 ) − sin(𝑐) sin(𝑞2 ) = 0

cos(𝑞2 ) (𝑎 − cos(𝑐)) + sin(𝑞2 ) (−𝑏 − sin(𝑐)) = 0

cos(𝑞2 ) (𝑎 − cos(𝑐)) = − sin(𝑞2 ) (−𝑏 − sin(𝑐))

cos(𝑞2 ) (−𝑏 − sin(𝑐))

=
sin(𝑞2 ) (𝑎 − cos(𝑐))

(−𝑏 − sin(𝑐))
− cot(𝑞2 ) =
(𝑎 − cos(𝑐))

(−𝑏 − sin(𝑐))
cot(𝑞2 ) = −
(𝑎 − cos(𝑐))

(−𝑏 − sin(𝑐))
𝑞2 = 𝑎𝑟𝑐𝑐𝑜𝑡 (− )
(𝑎 − cos(𝑐))

Sustituyendo a y b, tenemos:

8449857 44668708
(− − sin (𝑎𝑟𝑐𝑐𝑜𝑠 ( )))
8992150 130602565
𝑞2 = 𝑎𝑟𝑐𝑐𝑜𝑡 −
44668708 44668708
( − cos (𝑎𝑟𝑐𝑐𝑜𝑠 ( )))
130602565 130602565
( )

𝑞2 = 0

⟹ 𝑞3 = −𝑞2 + 𝑐

𝑞3 = 𝑐

44668708
𝑞3 = 𝑎𝑟𝑐𝑐𝑜𝑠 ( ) = 1.22173047639602872833716791801144
130602565

⟹ 𝑞3 = 1.22173047639602872833716791801144

Al final tenemos que:

𝑞1 = 0
𝑞1 = 0
𝑞3 = 1.22173047639602872833716791801144
𝑞4 = 0
𝑞4 = 0
{ 𝑞4 = 0
 ROBOTICA

Conclusiones

En esta práctica, hemos explorado el uso del algoritmo de Denavit-Hartenberg para modelar la
estructura de un brazo robótico. A través de la identificación y aplicación de los parámetros D-H,
logramos describir de manera sistemática las relaciones espaciales entre los eslabones y juntas
del robot. Esta metodología ha demostrado ser una herramienta valiosa para simplificar la
representación de configuraciones complejas en robótica, proporcionando una base sólida para
futuros análisis cinemáticos y dinámicos. La comprensión y manejo adecuado del algoritmo D-H
es esencial para el diseño y control eficiente de robots manipuladores.