""" Este script genera la matriz de transformación para cada eslabón siguiendo la metodología de Denavit Hartenberg, coloca como cadena de texto d_i si es una articulación prizmática o a theta_i si es rotacional dh_i = [a_i, alpha_i, d_i, theta_i] """ from typing import List import numpy as np # dh_i = [a_i, alpha_i, d_i, theta_i] def make_transformation(dh_i: List): if type(dh_i[2]) == str and type(dh_i[3]) != str: def transformation(q_i: float): n = np.matrix([[np.cos(dh_i[3])], [np.sin(dh_i[3])], [0]]) s = np.matrix([[-np.sin(dh_i[3])*np.cos(dh_i[1])], [np.cos(dh_i[3])*np.cos(dh_i[1])], [np.sin(dh_i[1])]]) a = np.matrix([[np.sin(dh_i[3])*np.sin(dh_i[1])], [-np.cos(dh_i[3])*np.sin(dh_i[1])], [np.cos(dh_i[1])]]) d = np.matrix([[dh_i[0]*np.cos(dh_i[3])], [dh_i[0]*np.sin(dh_i[3])], [q_i]]) t11_33 = np.concatenate((n, s, a, d),axis=1) t41_44 = np.matrix([0, 0, 0, 1]) return np.matrix(np.concatenate((t11_33,t41_44),axis=0)) elif type(dh_i[3]) == str: def transformation(q_i: float): n = np.matrix([[np.cos(q_i)], [np.sin(q_i)], [0]]) s = np.matrix([[-np.sin(q_i)*np.cos(dh_i[1])], [np.cos(q_i)*np.cos(dh_i[1])], [np.sin(dh_i[1])]]) a = np.matrix([[np.sin(q_i)*np.sin(dh_i[1])], [-np.cos(q_i)*np.sin(dh_i[1])], [np.cos(dh_i[1])]]) d = np.matrix([[dh_i[0]*np.cos(q_i)], [dh_i[0]*np.sin(q_i)], [dh_i[2]]]) t11_33 = np.concatenate((n, s, a, d),axis=1) t41_44 = np.matrix([0, 0, 0, 1]) #print(type(np.matrix(np.concatenate((t11_33,t41_44),axis=0)))) return np.matrix(np.concatenate((t11_33,t41_44),axis=0)) else: raise Exception("Sorry, I can only handle prismatic or rotating joints, try to express your joint has two or more joints") return transformation ## Ejemplo de uso link_1 = [0., np.pi/2, 0., 'theta_1'] print(link_1) a_01 = make_transformation(link_1) print(type(a_01)) q_1 = 10 print(type(a_01(q_1))) print(np.size(a_01(q_1),0),'x',np.size(a_01(q_1),1)) print('A^0_1 =',a_01(q_1))

# Links link_1 = [0., np.pi/2, 0., 'theta_1'] link_2 = [30., 0., 0., 'theta_2'] link_3 = [30., 0., 0., 'theta_3']

a_01 = make_transformation(link_1) a_12 = make_transformation(link_2) a_23 = make_transformation(link_3)

a_01_0_grados = np.round(a_01(0),2) print('A^0_1 =',a_01_0_grados) print(type(a_01_0_grados)) # Si usamos redonde se pierde el tipo matriz y se trata como array.

a_01_0_grados = np.matrix(np.round(a_01(0),2)) a_12_0_grados = np.matrix(np.round(a_12(0),2)) a_23_0_grados = np.matrix(np.round(a_23(0),2)) print(type(a_01_0_grados)) print(a_01_0_grados) print(type(a_12_0_grados)) print(a_12_0_grados) print(type(a_23_0_grados)) print(a_23_0_grados) a_03_0_0_0_grados = a_23_0_grados*a_12_0_grados*a_01_0_grados print(type(a_03_0_0_0_grados)) print(a_03_0_0_0_grados)

# Generamos vectores de ángulos para las tres articulaciones paso = 1000 angulos1 = [x*1/4*np.pi/paso for x in range(0,paso)] angulos2 = [x*1*np.pi/paso for x in range(0,paso)] angulos3 = [-x*1/2*np.pi/paso for x in range(0,paso)] posición1 = np.concatenate([a_01(x)[[0,1,2],3] for x in angulos1],1) posición2 = np.concatenate([(a_01(x)*a_12(y))[[0,1,2],3] for x,y in zip(angulos1,angulos2)],1) posición3 = np.concatenate([(a_01(x)*a_12(y)*a_23(z))[[0,1,2],3] for x,y,z in zip(angulos1,angulos2,angulos3)],1) #print(type(np.asarray(posición1[1,:]))) #print(np.squeeze(np.asarray(posición3[0,:]))) #print(np.squeeze(np.asarray(posición3[1,:]))) #print(np.squeeze(np.asarray(posición3[2,:])))

# Imprimiendo posición eslabon 1 from mpl_toolkits import mplot3d import matplotlib.pyplot as plt fig = plt.figure() ax = plt.axes(projection='3d') ax.plot3D(np.squeeze(np.asarray(posición1[0,:])), np.squeeze(np.asarray(posición1[1,:])), np.squeeze(np.asarray(posición1[2,:])), 'green') ax.plot3D(np.squeeze(np.asarray(posición2[0,:])), np.squeeze(np.asarray(posición2[1,:])), np.squeeze(np.asarray(posición2[2,:])), 'blue') ax.plot3D(np.squeeze(np.asarray(posición3[0,:])), np.squeeze(np.asarray(posición3[1,:])), np.squeeze(np.asarray(posición3[2,:])), 'red') ax.set_title('Posición'); ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z')

# Imprimiendo posición eslabon 2 from mpl_toolkits import mplot3d import matplotlib.pyplot as plt fig = plt.figure() ax = plt.axes(projection='3d') ax.plot3D(np.squeeze(np.asarray(posición2[0,:])), np.squeeze(np.asarray(posición2[1,:])), np.squeeze(np.asarray(posición2[2,:])), 'blue')

# Imprimiendo posición eslabon 1 from mpl_toolkits import mplot3d import matplotlib.pyplot as plt fig = plt.figure() ax = plt.axes(projection='3d') ax.plot3D(np.squeeze(np.asarray(posición3[0,:])), np.squeeze(np.asarray(posición3[1,:])), np.squeeze(np.asarray(posición3[2,:])), 'red')

# Constrsuyendo el Jacobiano import numpy as np def jacobiano (matrixes): """matrixes is a list of transformation matrixes from link 1 to n-1""" # adding a_00 to matrixes a_00 = np.matrix(np.eye(4)) #print('a_00 =',a_00) matrixes.insert(0,a_00) #print('matrixes =',matrixes) #i = 1 j = np.matrix([[],[],[]]) for matrix in matrixes[0:-1]: z_previous = np.transpose(matrix[[0,1,2],2]) #print('z_',i-1,'=',z_previous) origin_diference = np.transpose(matrixes[-1][[0,1,2],-1]-matrix[[0,1,2],-1]) #print('origin_diference_', i-1, '=', origin_diference) ji = np.transpose(np.cross(z_previous,origin_diference)) #print('j_',i-1,'=',ji) j = np.concatenate((j,ji),1) #i += 1 return np.matrix(np.squeeze(j)) # example angulos_ininciales = np.array([0,0,0]) posicion_deseada = np.array([40,40,40]) posicion_actual = [a_01(angulos_ininciales[0])*a_12(angulos_ininciales[1])*a_23(angulos_ininciales[2])] m1 = a_01(angulos_ininciales[0]) m2 = a_01(angulos_ininciales[0])*a_12(angulos_ininciales[1]) m3 = a_01(angulos_ininciales[0])*a_12(angulos_ininciales[1])*a_23(angulos_ininciales[2]) matrixes = [m1, m2, m3] j = jacobiano(matrixes) print(j) print(type(j))

# Calculando siguiente q def next_q(q_actual, x_deseada, step): # Calculando matrices de transformación m1 = a_01(q_actual[0]) m2 = a_01(q_actual[0])*a_12(q_actual[1]) m3 = a_01(q_actual[0])*a_12(q_actual[1])*a_23(q_actual[2]) # Calculando Jacobiano matrixes = [m1, m2, m3] #print('matrixes =',matrixes) j = jacobiano(matrixes) #print('jacobiano =',j) #print('jacobiano tipo=',type(j)) jt = np.matrix(np.transpose(j)) #print('jacobiano transpuesto =',jt) # Calculando posición actual del eslabón final x_actual = np.matrix(m3[[0,1,2],3]) # Calculando siguiente q #print('jacobiano transpuesto tipo =',type(jt)) #print('x_actual-x_deseada =',x_actual-x_deseada,type(x_actual-x_deseada)) q = np.matrix(q_actual-step*jt*(x_actual-x_deseada)) return q # Ejemplo q_actual = np.matrix([[0], [0], [0]]) x_deseada = np.matrix([[15],[15],[15]]) step = 0.001 q_actual = next_q(q_actual, x_deseada, step) print(q_actual) #print(np.squeeze(q_actual)) #print('q_actual =',q_actual.ndim)