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()