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