Se muestra un programa que realiza la multiplicación de matrices homogéneas formadas por Denavit Hartenberg para describir a un robot; posteriormente se utiliza la librería de python-roboticstoolbox para la comprobación.
Descargar teoría PDF
import sympy as sp
# Función para calcular la matriz homogénea
def dh_matrix(theta, d, a, alpha):
return sp.Matrix([
[sp.cos(theta), -sp.sin(theta)*sp.cos(alpha), sp.sin(theta)*sp.sin(alpha), a*sp.cos(theta)],
[sp.sin(theta), sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha), a*sp.sin(theta)],
[0, sp.sin(alpha), sp.cos(alpha), d],
[0, 0, 0, 1]
])
#dh_matrix(θ, d , a, α)
# Función para generar una matriz de rotación homogénea
def rotation_matrix(x_angle=0, y_angle=0, z_angle=0):
# Rotación alrededor del eje X
R_x = sp.Matrix([
[1, 0, 0, 0],
[0, sp.cos(x_angle), -sp.sin(x_angle), 0],
[0, sp.sin(x_angle), sp.cos(x_angle), 0],
[0, 0, 0, 1]
])
# Rotación alrededor del eje Y
R_y = sp.Matrix([
[sp.cos(y_angle), 0, sp.sin(y_angle), 0],
[0, 1, 0, 0],
[-sp.sin(y_angle), 0, sp.cos(y_angle), 0],
[0, 0, 0, 1]
])
# Rotación alrededor del eje Z
R_z = sp.Matrix([
[sp.cos(z_angle), -sp.sin(z_angle), 0, 0],
[sp.sin(z_angle), sp.cos(z_angle), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# Matriz de rotación combinada
return R_x @ R_y @ R_z # Usar @ para multiplicación de matrices
def reemplazar_trigonometria(matriz, n=3):
"""
Reemplaza las funciones trigonométricas en la matriz:
sin -> S
cos -> C
y evalúa los valores numéricos con 2 decimales.
"""
matriz_reemplazada = matriz.applyfunc(lambda expr: expr.replace(sp.sin, lambda x: sp.Symbol(f'S({x})')).replace(sp.cos, lambda x: sp.Symbol(f'C({x})')))
return matriz_reemplazada.evalf(n)
n=3
# Ejercicio 1:
print("\nEjercicio 1:")
# Definir las variables simbólicas
th1, a2, a1, a3, d1 = sp.symbols('th1 a2 a1 a3 d1')
# Primera fila de la tabla DH: [α=90, a=a2, θ=θ1, d=0]
T1 = dh_matrix(th1, 0, a2, sp.pi/2)
# Segunda fila de la tabla DH: [α=0, a=0, θ=0, d=a1 + a3 + d1]
T2 = dh_matrix(0, a1 + a3 + d1, 0, 0)
# Matriz final: Producto de las transformaciones
T = T1 @ T2
# Mostrar las matrices individuales y el producto final, redondeado a 2 decimales
print("Matriz T1:")
sp.pprint(reemplazar_trigonometria(T1)) # Redondear a 2 decimales
print("\nMatriz T2:")
sp.pprint(reemplazar_trigonometria(T2)) # Redondear a 2 decimales
print("\nMatriz homogénea total T:")
sp.pprint(reemplazar_trigonometria(T)) # Redondear a 2 decimales
# Ejercicio 2:
print("\nEjercicio 2:")
# Definir las variables simbólicas
a2, a1, a3, d1, d2, d3 = sp.symbols('a2 a1 a3 d1 d2 d3')
# Primera fila de la tabla DH: [α=-90, a=0, θ=90, d=a1+d1]
T1 = dh_matrix(sp.pi/2, a1 + d1, 0, -sp.pi/2)
# Segunda fila de la tabla DH: [α=90, a=0, θ=-90, d=a2+d2]
T2 = dh_matrix(-sp.pi/2, a2 + d2, 0, sp.pi/2)
# Tercera fila de la tabla DH: [α=0, a=0, θ=0, d=a3+d3]
T3 = dh_matrix(0, a3 + d3, 0, 0)
# Matriz final: Producto de las transformaciones
T = T1 @ T2 @ T3
# Mostrar las matrices individuales y el producto final, redondeado a 2 decimales
print("Matriz T1:")
sp.pprint(reemplazar_trigonometria(T1)) # Redondear a 2 decimales
print("\nMatriz T2:")
sp.pprint(reemplazar_trigonometria(T2)) # Redondear a 2 decimales
print("\nMatriz T3:")
sp.pprint(reemplazar_trigonometria(T3)) # Redondear a 2 decimales
print("\nMatriz homogénea total T:")
sp.pprint(reemplazar_trigonometria(sp.simplify(T))) # Redondear a 2 decimales
# Ejercicio 3:
print("\nEjercicio 3:")
# Definir las variables simbólicas
th1, th2, th3, th4, th5, th6, d1, d2, d3, d4, d5, d6 = sp.symbols('th1 th2 th3 th4 th5 th6 d1 d2 d3 d4 d5 d6')
# Primera fila de la tabla DH: [α=-90, a=0, θ=th1, d=d1]
T1 = dh_matrix(th1, d1, 0, -sp.pi/2)
# Segunda fila de la tabla DH: [α=0, a=d2, θ=th2, d=0]
T2 = dh_matrix(th2, 0, d2, 0)
# Tercera fila de la tabla DH: [α=0, a=d3, θ=th3, d=0]
T3 = dh_matrix(th3, 0, d3, 0)
#------------Roptura de robot por no cumplir denavit----------------
# Cuarta fila de la tabla DH: [α=-90, a=0, θ=th4, d=d4]
T4 = dh_matrix(th4, d4, 0, -sp.pi/2)
# Quinta fila de la tabla DH: [α=90, a=d5, θ=th5, d=0]
T5 = dh_matrix(th5, 0, d5, sp.pi/2)
# Sexta fila de la tabla DH: [α=0, a=0, θ=th6, d=d6]
T6 = dh_matrix(th6, d6, 0, 0)
# Matriz final: Producto de las transformaciones
T_1 = T1 * T2 * T3
T_2 = T4 * T5 * T6
# Mostrar las matrices individuales y el producto final, redondeado a 2 decimales
print("Matriz T1:")
sp.pprint(reemplazar_trigonometria(T1)) # Redondear a 2 decimales
print("\nMatriz T2:")
sp.pprint(reemplazar_trigonometria(T2)) # Redondear a 2 decimales
print("\nMatriz T3:")
sp.pprint(reemplazar_trigonometria(T3)) # Redondear a 2 decimales
print("\nMatriz T4:")
sp.pprint(reemplazar_trigonometria(T4)) # Redondear a 2 decimales
print("\nMatic T5:")
sp.pprint(reemplazar_trigonometria(T5)) # Redondear a 2 decimales
print("\nMatiz T6:")
sp.pprint(reemplazar_trigonometria(T6)) # Redondear a 2 decimales
print("\nMatriz homogénea total T1:")
sp.pprint(reemplazar_trigonometria(T_1)) # Redondear a 2 decimales
print("\nMatriz homogénea total T2:")
sp.pprint(reemplazar_trigonometria(T_2)) # Redondear a 2 decimales
# Juntar las matrices T1 y T2
x_angle = 0 # Rotación en X
y_angle = sp.pi / 2 # Rotación en Y (90°)
z_angle = sp.pi # Rotación en Z (180°)
# Calcular la matriz de rotación combinada
T_rotation = rotation_matrix(x_angle, y_angle, z_angle)
# Matriz total combinada
T_total = T_1 @ T_rotation @ T_2
# Mostrar la matriz de rotación y la matriz total, redondeado a 2 decimales
print("\nMatriz de rotación:")
sp.pprint(reemplazar_trigonometria(T_rotation)) # Redondear a 2 decimales
print("\nMatriz homogénea total T:")
sp.pprint(reemplazar_trigonometria(T_total)) # Redondear a 2 decimales
#Imprimir por columnas la matriz total
print("\n\n Columna 0")
sp.pprint(reemplazar_trigonometria(T_total).col(0))
print("\n\n Columna 2")
sp.pprint(reemplazar_trigonometria(T_total).col(1))
print("\n\n Columna 1")
sp.pprint(reemplazar_trigonometria(T_total).col(2))
print("\n\n Columna 3")
sp.pprint(reemplazar_trigonometria(T_total).col(3))
#Ejercicio 4
print("\nEjercicio 4:")
# Definir las variables simbólicas
th1, th2, th3, th4, th5, th6, d1, d2, d3, d4, d5, d6 = sp.symbols('th1 th2 th3 th4 th5 th6 d1 d2 d3 d4 d5 d6')
# Primera fila de la tabla DH: [α=-90, a=0, θ=th1, d=d1]
T1 = dh_matrix(th1, d1, 0, -sp.pi/2)
# Segunda fila de la tabla DH: [α=0, a=d2, θ=th2, d=0]
T2 = dh_matrix(th2, 0, d2, 0)
# Tercera fila de la tabla DH: [α=-90, a=0, θ=th3, d=d3]
T3 = dh_matrix(th3, d3, 0, -sp.pi/2)
# Cuarta fila de la tabla DH: [α=-90, a=0, θ=th4, d=d4]
T4 = dh_matrix(th4, d4, 0, -sp.pi/2)
# Quinta fila de la tabla DH: [α=90, a=0, θ=th5, d=0]
T5 = dh_matrix(th5, 0, 0, sp.pi/2)
# Sexta fila de la tabla DH: [α=0, a=0, θ=th6, d=d5]
T6 = dh_matrix(th6, d5, 0, 0)
# Matriz final: Producto de las transformaciones
T = T1 @ T2 @ T3 @ T4 @ T5 @ T6
# Mostrar las matrices individuales y el producto final, redondeado a 2 decimales
print("Matriz T1:")
sp.pprint(reemplazar_trigonometria(T1)) # Redondear a 2 decimales
print("\nMatriz T2:")
sp.pprint(reemplazar_trigonometria(T2)) # Redondear a 2 decimales
print("\nMatriz T3:")
sp.pprint(reemplazar_trigonometria(T3)) # Redondear a 2 decimales
print("\nMatriz T4:")
sp.pprint(reemplazar_trigonometria(T4)) # Redondear a 2 decimales
print("\nMatriz T5:")
sp.pprint(reemplazar_trigonometria(T5)) # Redondear a 2 decimales
print("\nMatriz T6:")
sp.pprint(reemplazar_trigonometria(T6)) # Redondear a 2 decimales
print("\nMatriz homogénea total T:")
sp.pprint(reemplazar_trigonometria(sp.simplify(T))) # Redondear a 2 decimales
# Ejercicio 5:
print("\nEjercicio 5:")
# Definir las variables simbólicas
th1, th2, th3, th4, th5, th6, d1, d2, d3, d4, d5, d6 = sp.symbols('th1 th2 th3 th4 th5 th6 d1 d2 d3 d4 d5 d6')
# Primera fila de la tabla DH: [α=-90, a=0, θ=th1, d=d1]
T1 = dh_matrix(th1, d1, 0, -sp.pi/2)
# Segunda fila de la tabla DH: [α=0, a=d2, θ=th2, d=0]
T2 = dh_matrix(th2, 0, d2, 0)
# Tercera fila de la tabla DH: [α=-90, a=0, θ=th3, d=d3]
T3 = dh_matrix(th3, d3, 0, -sp.pi/2)
# Cuarta fila de la tabla DH: [α=0, a=0, θ=th4, d=d4]
T4 = dh_matrix(th4, d4, 0, 0)
# Quinta fila de la tabla DH: [α=-90, a=0, θ=th5, d=d5]
T5 = dh_matrix(th5, d5, 0, -sp.pi/2)
# Sexta fila de la tabla DH: [α=0, a=0, θ=th6, d=d6]
T6 = dh_matrix(th6, d6, 0, 0)
# Matriz final: Producto de las transformaciones
T_1 = T1 @ T2 @ T3 @ T4
T_2 = T5 @ T6
# Juntar las matrices T1 y T2
x_angle = -(sp.pi / 2) # Rotación en X
y_angle = 0
z_angle = 0
# Calcular la matriz de rotación combinada
T_rotation = rotation_matrix(x_angle, y_angle, z_angle)
# Matriz total combinada
T = T_1 @ T_rotation @ T_2
# Mostrar las matrices individuales y el producto final, redondeado a 2 decimales
print("Matriz T1:")
sp.pprint(reemplazar_trigonometria(T1)) # Redondear a 2 decimales
print("\nMatriz T2:")
sp.pprint(reemplazar_trigonometria(T2)) # Redondear a 2 decimales
print("\nMatriz T3:")
sp.pprint(reemplazar_trigonometria(T3)) # Redondear a 2 decimales
print("\nMatriz T4:")
sp.pprint(reemplazar_trigonometria(T4)) # Redondear a 2 decimales
print("\nMatriz T5:")
sp.pprint(reemplazar_trigonometria(T5)) # Redondear a 2 decimales
print("\nMatriz T6:")
sp.pprint(reemplazar_trigonometria(T6)) # Redondear a 2 decimales
print("\nMatriz homogénea total T:")
sp.pprint(reemplazar_trigonometria(sp.simplify(T))) # Redondear a 2 decimales
#Codigo para ejecutar la simulación de los 5 robots
from roboticstoolbox import DHRobot, PrismaticDH, RevoluteDH
from spatialmath import SE3
import numpy as np
import matplotlib.pyplot as plt
Status_block = False
# Robot 1 --------------------------------------------------------------
name = "1_Planar"
# Define the planar robot with 1 revolute joint and 1 prismatic joint
a1 = 0.1 # Desplazamiento para la articulación prismática
a2 = 0.05 # Longitud del eslabón para la articulación revoluta
a3 = 0.1 # Desplazamiento adicional para la articulación prismática
robot = DHRobot(
[
RevoluteDH( alpha=np.pi/2, a=a2, d=0, offset=np.pi/2),
PrismaticDH( offset=a1 + a3 )
],
name=name
)
print("Detalles del Robot: ", name)
print(robot)
# Define the displacement values for each prismatic joint
t_values = [np.radians(0)]
d_values = [0.1] # Example: [X, Y, Z] displacements in meters
q_values = [t_values[0], d_values[0]]
print("Matriz de transformación (Directa):")
T = robot.fkine(q_values) # Forward kinematics
print(T)
robot.plot(q_values, block=Status_block, jointaxes=True, eeframe=True, jointlabels=True)
plt.savefig(f"Actividades/Clase/Tarea_3/CF {name}.png", dpi=600, bbox_inches='tight', pad_inches=0.1)
# Robot 2 --------------------------------------------------------------
name = "2_Cartesiano"
a1 = 0.1 # Desplazamiento para la articulación prismática
a2 = 0.1 # Desplazamiento para la articulación prismática
a3 = 0.1 # Desplazamiento para la articulación prismática
# Define the Cartesian robot with 3 prismatic joints
robot = DHRobot(
[
PrismaticDH(offset=a1, alpha=-np.pi/2, a=0, theta=np.pi/2),
PrismaticDH(offset=a2, alpha=np.pi/2, a=0, theta=-np.pi/2),
PrismaticDH(offset=a3, alpha=0, a=0, theta=0)
],
base = SE3(0, 0, 0), # shift the entire robot 0.5m along Z
name=name
)
print("Detalles del Robot: ", name)
print(robot)
# Define the displacement values for each prismatic joint
t_values = [0]
d_values = [0.1, 0.1, 0.1] # Example: [X, Y, Z] displacements in meters
q_values = [d_values[0], d_values[1], d_values[2]]
print("Matriz de transformación (Directa):")
T = robot.fkine(q_values) # Forward kinematics
print(T)
robot.plot(q_values, block=Status_block, jointaxes=True, eeframe=True, jointlabels=True)
plt.savefig(f"Actividades/Clase/Tarea_3/CF {name}.png", dpi=600, bbox_inches='tight', pad_inches=0.1)
# Robot 3 --------------------------------------------------------------
name = "3_Articulado"
a1 = 0.1
a2 = 0.1
a3 = 0.1
a4 = 0.1
a5 = 0.1
a6 = 0.1
# longitud del eslabón para la articulación revoluta
# Define the articulated robot with 6 revolute joints
robot = DHRobot(
[
RevoluteDH( alpha=-np.pi/2, a=0, d=a1, offset=0),
RevoluteDH( alpha=0, a=a2, d=0, offset=-np.pi/2),
RevoluteDH( alpha=-np.pi/2, a=0, d=0, offset=0),
RevoluteDH( alpha=np.pi/2, a=0, d=a3+a4, offset=np.pi/2),
RevoluteDH( alpha=-np.pi/2, a=0, d=0, offset=0),
RevoluteDH( alpha=0, a=0, d=a5+a6, offset=0)
],
name=name
)
print("Detalles del Robot: ", name)
print(robot)
# Define the displacement values for each revolute joint
t_values = [0, 0, 0, 0, 0, 0]
d_values = [0, 0, 0, 0, 0, 0] # Example: [X, Y, Z] displacements in meters
q_values = [t_values[0], t_values[1], t_values[2], t_values[3], t_values[4], t_values[5]]
print("Matriz de transformación (Directa):")
T = robot.fkine(q_values) # Forward kinematics
print(T)
robot.plot(q_values, block=Status_block, jointaxes=True, eeframe=True, jointlabels=True)
plt.savefig(f"Actividades/Clase/Tarea_3/CF {name}.png", dpi=600, bbox_inches='tight', pad_inches=0.1)
# Robot 4 --------------------------------------------------------------
name = "4_Articulado"
a1 = 0.2
a2 = 0.1
a3 = 0.1
a4 = 0.1
a5 = 0.1
a6 = 0.1
# Define the articulated robot with 6 revolute joints
robot = DHRobot(
[
RevoluteDH( alpha=-np.pi/2, a=0, d=a1, offset=0),
RevoluteDH( alpha=-np.pi/2, a=a2, d=0, offset=0),
RevoluteDH( alpha=-np.pi/2, a=0, d=0, offset=0),
RevoluteDH( alpha=np.pi/2, a=0, d=a3+a4, offset=0),
RevoluteDH( alpha=-np.pi/2, a=0, d=0, offset=0),
RevoluteDH( alpha=0, a=0, d=a5+a6, offset=0)
],
name=name
)
print("Detalles del Robot: ", name)
print(robot)
# Define the displacement values for each revolute joint
t_values = [0, 0, 0, 0, 0, 0]
d_values = [0, 0, 0, 0, 0, 0] # Example: [X, Y, Z] displacements in meters
q_values = [t_values[0], t_values[1], t_values[2], t_values[3], t_values[4], t_values[5]]
print("Matriz de transformación (Directa):")
T = robot.fkine(q_values) # Forward kinematics
print(T)
robot.plot(q_values, block=Status_block, jointaxes=True, eeframe=True, jointlabels=True)
plt.savefig(f"Actividades/Clase/Tarea_3/CF {name}.png", dpi=600, bbox_inches='tight', pad_inches=0.1)
# Robot 5 --------------------------------------------------------------
name = "5_Articulado"
a1 = 0.1
a2 = 0.1
a3 = 0.1
a4 = 0.1
a5 = 0.1
a6 = 0.1
# Define the articulated robot with 6 revolute joints
robot = DHRobot(
[
RevoluteDH( alpha=-np.pi/2, a=0, d=a1, offset=0),
RevoluteDH( alpha=0, a=a2, d=0, offset=-np.pi/2),
RevoluteDH( alpha=np.pi/2, a=0, d=0, offset=0),
RevoluteDH( alpha=-np.pi/2, a=0, d=a3+a4, offset=0),
RevoluteDH( alpha=np.pi/2, a=0, d=0, offset=0),
RevoluteDH( alpha=0, a=0, d=a5+a6, offset=0)
],
name=name
)
print("Detalles del Robot: ", name)
print(robot)
# Define the displacement values for each revolute joint
t_values = [0, 0, 0, 0, 0, 0]
d_values = [0, 0, 0, 0, 0, 0] # Example: [X, Y, Z] displacements in meters
q_values = [t_values[0], t_values[1], t_values[2], t_values[3], t_values[4], t_values[5]]
print("Matriz de transformación (Directa):")
T = robot.fkine(q_values) # Forward kinematics
print(T)
robot.plot(q_values, block=Status_block, jointaxes=True, eeframe=True, jointlabels=True)
plt.savefig(f"Actividades/Clase/Tarea_3/CF {name}.png", dpi=600, bbox_inches='tight', pad_inches=0.1)
Se presentan 5 robots: 1 Planar, 2 Cartesiano, 3 Articulado, 4 Articulado, 5 Articulado