def restringe_junta2(v1, v2, i):
qlim = [2.6179, 1.5358, 2.6179, 1.6144, 2.6179, 1.8413, 1.7889]
th = acosr(v1.T @ v2)
v3 = S(v2) @ v1 #vetor ortogonal a v1 e v2
#Se o ângulo é maior que o permitido sature ele em qlim[i] - 0.01
if (th > qlim[i]):
v1 = rotationar_vetor(v2, v3, qlim[i] - 0.01)
return v1[:, 0] / norm(v1)
def FABRIK_angulos(p,D):
x = vetor([1,0,0])
qlim = [2.6179,1.5358,2.6179,1.6144,2.6179,1.8413,1.7889]
q = np.zeros([7,1])
####Conversão da solução gráfica em um vetor de ângulo####
#Primeiro normalizo os vetores, porque as vezes por causa de erro numérico eles chegam
#com norma maior do que 1
for i in range(7):
D[:,i] = D[:,i]/norm(D[:,i])
#Calcula um vetor v que seja ortogonal ao vetor de refência para o cálculo dos ângulos
for i in range(7):
if(i == 0):
vref = x
elif(i == 6):
vref = vetor(p[:,6] - p[:,5])
vref = vref/norm(vref)
else:
vref = vetor(D[:,i-1])
#v é o vetor ortogonal ao vetor de refência para o cálculo dos ângulos
v = rotationar_vetor(vref,vetor(D[:,i]),pi/2)
#cálculo o ângulo
if(i == 5):
vaux = vetor(p[:,6] - p[:,5])
vaux = vaux/norm(vaux)
q[5] = acosr(vaux.T@vref)
if(vaux.T@v < 0): q[5] = - q[5]
elif(i == 6):
vaux = vetor(p[:,7] - p[:,6])
vaux = vaux/norm(vaux)
q[6] = acosr(vaux.T@vref)
if(vaux.T@v < 0): q[6] = - q[6]
else:
q[i] = acosr(D[:,i+1].T@vref)
if(D[:,i+1].T@v < 0): q[i] = - q[i]
#if(np.abs(q[i]) > qlim[i]):
#print('a junta',i+1,'excedeu')
# a = 1
return q
def restringe_junta(p1, p2, p3, i):
qlim = [2.6179, 1.5358, 2.6179, 1.6144, 2.6179, 1.8413, 1.7889]
v1 = vetor(p1 - p2)
v1 = v1 / norm(v1)
v2 = vetor(p3 - p2)
v2 = v2 / norm(v2)
th = acosr(v1.T @ v2)
d = distancia(p1, p2, 3)
v3 = S(v1) @ v2 #vetor ortogonal a v1 e v2
#Se V3 não for o vetor nulo, normalize V3
if (norm(v3) > 0.0001): v3 = v3 / norm(v3)
#Se o ângulo é maior que o permitido sature ele em qlim[i] - 0.1
if (th < pi - qlim[i]):
v1 = rotationar_vetor(v1, v3, th - (pi - qlim[i] + 0.1))
p1 = vetor(p2) + d * v1
else:
p1 = vetor(p1)
return p1[:, 0]
if (i == 1 or i == 3): #Se a junta prev for pivot (2 e 4)
pl[:, i] = pl[:, i + 1] - Dl[:, i + 1] * b[i]
#paux é pl é o ponto da próxima iteração
#eu calculo ele aqui porque como proposto na abordagem FABRIK-R
#Dl(:,i) é cálculo de forma que pl(:,i+1) seja alcancável
paux = iteracao_Fabrik(p[:, i - 1], pl[:, i], b[i],
Dl[:, i + 1])[:, 0]
paux = restringe_junta(paux, pl[0:3, i], pl[0:3, i + 1], i + 1)
v1 = vetor(paux - pl[:, i])
v1 = v1 / norm(v1) #vetor de refência
v2 = vetor(Dl[:, i + 2])
v2 = v2 / norm(v2)
th = np.real(acosr(v1.T @ v2))
#v3 é um vetor ortogonal ao vetor de referência (v1)
v3 = rotationar_vetor(v1, vetor(Dl[:, i + 1]), pi / 2)[:, 0]
if (v3.T @ v2 < 0):
th = -th
Dl[:, i] = rotationar_vetor(v2, vetor(Dl[:, i + 1]),
(pi / 2) - th)[:, 0]
Dl[:, i] = Dl[:, i] / norm(D[:, i])
else: #Se a junta prev for hinge (6)
pl[:, i] = iteracao_Fabrik(p[:, i - 1], pl[:, i + 1], b[i],
Dl[:, i + 1])[:, 0]
pl[:, i] = restringe_junta(pl[0:3, i], pl[0:3, i + 1],
pl[0:3, i + 2], i + 1)
v1 = vetor(pl[:, i] - pl[:, i + 1])
v1 = v1 / norm(v1)
Dl[:, i] = rotationar_vetor(vetor(Dl[:, i + 1]), v1, pi / 2)[:,
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
if (q[i] > qlim[i]):
q[i] = qlim[i]
elif (q[i] < -qlim[i]):
q[i] = -qlim[i]
pontos = Cinematica_Direta(q)
erro_anterior = erro
erro = distancia(pontos[:, 7], posicaod, 3)
if (erro_anterior < erro):
