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]
#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)
naux = (S(v1) @ v2) #produto vetorial
if (norm(naux) > 0.00001): #Se p7',p6 e p5 não forem colineares
Dl[:, i] = naux[:, 0] / norm(naux)
else: #Caso não seja mantém o vetor de direção da iteração anterior
Dl[:, i] = D[:, i].copy()
pl[:, i] = iteracao_Fabrik(p[:, i], pl[:, i + 1], b[i],
Dl[:, i])[:, 0]
elif (i == 1 or i == 3 or i == 5): #Se atual for junta Hinge (2,4 e 6)
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]
Dl[:,i] = orientd[:,1]
#pl[:,i] = iteracao_Fabrik(p[:,i],pl[:,i+1],b[i],Dl[:,i])[:,0]
pl[:,i] = pl[:,i+1] - orientd[:,2]*b[i]
elif(i == 1 or i == 3 or i == 5): #Se atual for junta Hinge (2,4 e 6)
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)
Dl = D.copy()
#Calculo o erro inicial (distância euclidiana)
erro = distancia(p[:, n], destino, 3)
K = 100 #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
if (norm(naux) >
0.00001): #Se p7',p6 e p5 não forem colineares
Dl[:, i] = naux[:, 0] / norm(naux)
else: #Caso não seja mantém o vetor de direção da iteração anterior
Dl[:, i] = D[:, i].copy()
pl[:, i] = iteracao_Fabrik(p[:, i], pl[:, i + 1], b[i],
Dl[:, i])[:, 0]
elif (i == 1 or i == 3
or i == 5): #Se atual for junta Hinge (2,4 e 6)