o2_0 = T2@o2_2
o3_0 = T3@o3_3
o4_0 = T4@o4_4
o5_0 = T5@o5_5
o6_0 = T6@o6_6
o7_0 = T7@o7_7
p_0 = T7@p_7
#atualiaza os angulos do manipulador no Rviz
hello_str.header.stamp = rospy.Time.now()
hello_str.position = [q[0,0],q[1,0],q[2,0],q[3,0],q[4,0],q[5,0],q[6,0],0,0]
pub.publish(hello_str)
rate.sleep()
#Calcula a distancia entre o efetuador a o objetivo(Posição)
erro = distancia(p_0,posicaod,3)
#Condição de parada
if(erro < 0.01):
break
#os vetores z serao transformados em vetores no R^3
z0_0 = z[0:3]
z1_0 = (T1@z)[0:3]
z2_0 = (T2@z)[0:3]
z3_0 = (T3@z)[0:3]
z4_0 = (T4@z)[0:3]
z5_0 = (T5@z)[0:3]
z6_0 = (T6@z)[0:3]
#z7_0 = (T7@z)[0:3] nao eh usado
o4_0 = T4 @ o4_4
o5_0 = T5 @ o5_5
o6_0 = T6 @ o6_6
o7_0 = T7 @ o7_7
p_0 = T7 @ p_7
#atualiza os angulos do manipulador no Rviz
hello_str.header.stamp = rospy.Time.now()
hello_str.position = [
q[0, 0], q[1, 0], q[2, 0], q[3, 0], q[4, 0], q[5, 0], q[6, 0], 0, 0
]
pub.publish(hello_str)
rate.sleep()
#Condição de parada
errop = distancia(p_0, posicaod, 3) #erro de posiçao
rpy = orientacao(T7[0:3, 0:3]) #angulos Roll, Pitch Yall
erroa = distancia(rpy, rpyd, 3) #erro angular
c1 = 1 #posição
c2 = 0.1 #orientação
erro = c1 * errop + c2 * erroa #erro de pose
erro = distancia(p_0, posicaod, 3)
if (erro < 0.001):
print('Solucao q: \n', q, '\nNumero de iteracoes:', k)
break
#os vetores z serao transformados em vetores no R^3
z0_0 = z[0:3]
z1_0 = (T1 @ z)[0:3]
z2_0 = (T2 @ z)[0:3]
z3_0 = (T3 @ z)[0:3]
#Gera uma configuração aleatória
#for i in range(np.size(q)):
# q[i] = random.uniform(-qlim[i],qlim[i])
#Calcula os pontos e os vetores de atuação do robô atuais
p, D = Cinematica_Direta2(q)
#No Backward a junta 1 é sempre iniciada no mesmo lugar
pcte = p[:, 0].copy()
#pl e Dl são p_linha e D_linha respectivamente
pl = p.copy()
Dl = D.copy()
#Calculo o erro inicial (distância euclidiana)
erro = distancia(p[:, n], destino, 3)
print('erro inicial:', erro)
K = 100 #número máximo de iterações
k = 0 #iteração inicial
erromin = 10**-4 #erro minimo usado como um dos critérios de parada
while (erro > erromin and k < K):
#Forward
for i in range(n - 1, 0, -1):
if (i == 6): #Se for a junta 7
pl[:, i + 1] = destino[:, 0] #coloca o efetuador no destino
v1 = vetor(pl[:, i + 1] - p[:, i]) #p6 -> p7' (efetuador)
v1 = v1 / norm(v1)
v2 = vetor(p[:, i - 1] - p[:, i]) #p6 -> p5
v2 = v2 / norm(v2)
#Gera uma configuração aleatória
for i in range(np.size(q)):
q[i] = random.uniform(-qlim[i],qlim[i])
#Calcula os pontos e os vetores de atuação do robô atuais
p,D = Cinematica_Direta2(q)
#No Backward a junta 1 é sempre iniciada no mesmo lugar
pcte = p[:,0].copy()
#pl e Dl são p_linha e D_linha respectivamente
pl = p.copy()
Dl = D.copy()
#Calculo o erro inicial (distância euclidiana)
erro = distancia(p[:,n],destino,3)
K = 50 #número máximo de iterações
k = 0 #iteração inicial
erromin = 10**-3 #erro minimo usado como um dos critérios de parada
while(erro > erromin and k < K):
#Forward
for i in range(n-1,0,-1):
if(i == 6): #Se for a junta 7
pl[:,i+1] = destino[:,0]#coloca o efetuador no destino
#v1 = vetor(pl[:,i+1] - p[:,i])#p6 -> p7' (efetuador)
#v1 = v1/norm(v1)
#v2 = vetor(p[:,i-1] - p[:,i])#p6 -> p5
#v2 = v2/norm(v2)
#naux = (S(v1)@v2)#produto vetorial
T1 = A1
T2 = T1 @ A2
T3 = T2 @ A3
T4 = T3 @ A4
T5 = T4 @ A5
T6 = T5 @ A6
T7 = T6 @ A7
#vetores de atuação das juntas
Vy = np.array([
T1[0:3, 1], T2[0:3, 1], T3[0:3, 1], T4[0:3, 1], T5[0:3, 1], T6[0:3, 1],
T7[0:3, 1]
]).T
pontos = Cinematica_Direta(q)
erro = distancia(pontos[:, 7], posicaod, 3)
for i in range(n - 1, -1, -1):
pontos = Cinematica_Direta(q)
proj = projecao_ponto_plano(vetor(Vy[:, i]), pontos[:, i], posicaod[:])
va = proj - vetor(pontos[:, i])
va = va / norm(va)
proj = projecao_ponto_plano(vetor(Vy[:, i]), pontos[:, i],
vetor(pontos[:, 7]))
vb = proj - vetor(pontos[:, i])
vb = vb / norm(vb)
th = acosr(va.T @ vb)
j = i + 1
if (j == 4 or j == 2): #Se for a junta 2 ou 4
v = rotationar_vetor(va, vetor(Vy[:, i]), pi / 2)
else:
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()
