"""
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))
[0.0, 1.5707963267948966, 0.0, 'theta_1']
<class 'function'>
<class 'numpy.matrix'>
4 x 4
A^0_1 = [[-8.39071529e-01 3.33116856e-17 -5.44021111e-01 -0.00000000e+00]
[-5.44021111e-01 -5.13783131e-17 8.39071529e-01 -0.00000000e+00]
[ 0.00000000e+00 1.00000000e+00 6.12323400e-17 0.00000000e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
# 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^0_1 = [[ 1. -0. 0. 0.]
[ 0. 0. -1. 0.]
[ 0. 1. 0. 0.]
[ 0. 0. 0. 1.]]
<class 'numpy.ndarray'>
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)
<class 'numpy.matrix'>
[[ 1. -0. 0. 0.]
[ 0. 0. -1. 0.]
[ 0. 1. 0. 0.]
[ 0. 0. 0. 1.]]
<class 'numpy.matrix'>
[[ 1. -0. 0. 30.]
[ 0. 1. -0. 0.]
[ 0. 0. 1. 0.]
[ 0. 0. 0. 1.]]
<class 'numpy.matrix'>
[[ 1. -0. 0. 30.]
[ 0. 1. -0. 0.]
[ 0. 0. 1. 0.]
[ 0. 0. 0. 1.]]
<class 'numpy.matrix'>
[[ 1. 0. 0. 60.]
[ 0. 0. -1. 0.]
[ 0. 1. 0. 0.]
[ 0. 0. 0. 1.]]
# 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))
[[ 0.0000000e+00 -0.0000000e+00 -0.0000000e+00]
[ 6.0000000e+01 3.6739404e-15 1.8369702e-15]
[ 0.0000000e+00 6.0000000e+01 3.0000000e+01]]
<class 'numpy.matrix'>
# 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)
[[matrix([[0.9]])]
[matrix([[0.9]])]
[matrix([[0.45]])]]