alpha7 = pi / 2
while not rospy.is_shutdown():
for k in range(K):
# parametros de DH variáveis
theta1 = pi / 2 + q[0]
theta2 = q[1]
theta3 = q[2]
theta4 = q[3]
theta5 = q[4]
theta6 = pi / 2 + q[5]
theta7 = pi / 2 + q[6]
#Encontrando as matrizes homogêneas atuais
A1 = matriz_homogenea(d1, a1, alpha1, theta1)
A2 = matriz_homogenea(d2, a2, alpha2, theta2)
A3 = matriz_homogenea(d3, a3, alpha3, theta3)
A4 = matriz_homogenea(d4, a4, alpha4, theta4)
A5 = matriz_homogenea(d5, a5, alpha5, theta5)
A6 = matriz_homogenea(d6, a6, alpha6, theta6)
A7 = matriz_homogenea(d7, a7, alpha7, theta7)
#Definindo os pontos de interesse em seus sistemas locais
o1_1 = o #origem do SC 1
o2_2 = o #origem do SC 2
o3_3 = o #origem do SC 3
o4_4 = o #origem do SC 4
o5_5 = o #origem do SC 5
o6_6 = o #origem do SC 6
o7_7 = o #origem do SC 7
def update_fuction(self, o,
o2): #Calcula a função de custo/fitness da particula
#(posição,orientacao) da pose desejada
#Parâmetros Físicos do manipulador [m]
base = 0.05 #5 cm
L = 0.075 #distância da ultima junta a extremidade do efetuador
#centro do efetuador em relação ao ultimo sistema de coordenada
p = np.array([0, 0, L, 1]).T
#Parâmetros de Denavit-Hartenberg do manipulado
d1 = 0.075 + base
d2 = 0
d3 = 0.15
d4 = 0
d5 = 0.145
d6 = 0
d7 = 0
a1 = 0
a2 = 0
a3 = 0
a4 = 0
a5 = 0
a6 = 0.075
a7 = 0
alpha1 = pi / 2
alpha2 = -pi / 2
alpha3 = pi / 2
alpha4 = -pi / 2
alpha5 = pi / 2
alpha6 = pi / 2
alpha7 = pi / 2
theta1 = pi / 2 + self.p[0]
theta2 = self.p[1]
theta3 = self.p[2]
theta4 = self.p[3]
theta5 = self.p[4]
theta6 = pi / 2 + self.p[5]
theta7 = pi / 2 + self.p[6]
#Calculo das matrizes homogeneas
A1 = matriz_homogenea(d1, a1, alpha1, theta1)
A2 = matriz_homogenea(d2, a2, alpha2, theta2)
A3 = matriz_homogenea(d3, a3, alpha3, theta3)
A4 = matriz_homogenea(d4, a4, alpha4, theta4)
A5 = matriz_homogenea(d5, a5, alpha5, theta5)
A6 = matriz_homogenea(d6, a6, alpha6, theta6)
A7 = matriz_homogenea(d7, a7, alpha7, theta7)
A = A1 @ (A2 @ (A3 @ (A4 @ (A5 @ (A6 @ A7)))))
#posição do efetuador em relação ao sistema de coordenadas global
p = A @ p
#calculo do erro em módulo da orientacao desejada e da particula
self.o = distancia(orientacao(A), o2, 3)
#calculo da distancia euclidiana da posição do efetuador em relação ao objetivo
self.d = distancia(p.T, o, 3)
#Calculo da função de custo
k1 = 0.1 #orientacao
k2 = 0.9 #posição
self.f = (k1 * self.o) + (k2 * self.d)
if (self.f < self.bf):
self.bf = self.f
self.bp = self.p.copy()