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 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]
break
#os vetores z serao transformados em vetores no R^3
z0_0 = z[0:3]
z1_0 = (T1 @ z)[0:3]
z2_0 = (T2 @ z)[0:3]
z3_0 = (T3 @ z)[0:3]
z4_0 = (T4 @ z)[0:3]
z5_0 = (T5 @ z)[0:3]
z6_0 = (T6 @ z)[0:3]
#z7_0 = (T7@z)[0:3] nao eh usado
#cálculo do Jacobiano geometrico
J = np.zeros([6, 7])
#produto vetorial de Z0_0 por (o7_0 - o)
J[0:3, 0] = S(z0_0) @ (o7_0[0:3] - o[0:3])[:, 0]
J[3:6, 0] = z0_0[:, 0]
J[0:3, 1] = S(z1_0) @ (o7_0[0:3] - o1_0[0:3])[:, 0]
J[3:6, 1] = z1_0[:, 0]
J[0:3, 2] = S(z2_0) @ (o7_0[0:3] - o2_0[0:3])[:, 0]
J[3:6, 2] = z2_0[:, 0]
J[0:3, 3] = S(z3_0) @ (o7_0[0:3] - o3_0[0:3])[:, 0]
J[3:6, 3] = z3_0[:, 0]
J[0:3, 4] = S(z4_0) @ (o7_0[0:3] - o4_0[0:3])[:, 0]
J[3:6, 4] = z4_0[:, 0]
J[0:3, 5] = S(z5_0) @ (o7_0[0:3] - o5_0[0:3])[:, 0]
J[3:6, 5] = z5_0[:, 0]
J[0:3, 6] = S(z6_0) @ (o7_0[0:3] - o6_0[0:3])[:, 0]
J[3:6, 6] = z6_0[:, 0]
#Calculo da erro
break
#os vetores z serao transformados em vetores no R^3
z0_0 = z[0:3]
z1_0 = (T1@z)[0:3]
z2_0 = (T2@z)[0:3]
z3_0 = (T3@z)[0:3]
z4_0 = (T4@z)[0:3]
z5_0 = (T5@z)[0:3]
z6_0 = (T6@z)[0:3]
#z7_0 = (T7@z)[0:3] nao eh usado
#cálculo do Jacobiano geométrico
J = np.zeros([3,7])
#produto vetorial de Z0_0 por (o7_0 - o)
J[:,0] = S(z0_0)@(o7_0[0:3] - o[0:3])[:,0]
J[:,1] = S(z1_0)@(o7_0[0:3] - o1_0[0:3])[:,0]
J[:,2] = S(z2_0)@(o7_0[0:3] - o2_0[0:3])[:,0]
J[:,3] = S(z3_0)@(o7_0[0:3] - o3_0[0:3])[:,0]
J[:,4] = S(z4_0)@(o7_0[0:3] - o4_0[0:3])[:,0]
J[:,5] = S(z5_0)@(o7_0[0:3] - o5_0[0:3])[:,0]
J[:,6] = S(z6_0)@(o7_0[0:3] - o6_0[0:3])[:,0]
#erro
f = posicaod - p_0[0:3]
#Equação do DLS
dq = alfa*(([email protected]([email protected] + lbd*I))@f)
#limitando o delta q
for i in range(np.size(dq)):
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]
#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