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