def iteracao_Fabrik(p, p0, d, normal): #normal -> vetor normal ao plano #p0 -> ponto pertecente ao plano #p -> ponto a ser projetado #d -> distância que o ponto que será projetado terá de p0 normal = vetor(normal) p0 = vetor(p0) p = vetor(p) proj = projecao_ponto_plano(normal, p0, p) r = distancia(p0, proj, 3) delta = d / r pnew = (1 - delta) * p0 + delta * proj return pnew
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: v = rotationar_vetor(va, vetor(Vy[:, i]), -pi / 2) if (vb.T @ v < 0): th = -th q[i, 0] = q[i, 0] + th