hello_str.name = ['One_joint', 'Two_joint', 'Three_joint', 'Four_joint'\
,'Five_joint','Six_joint','Seven_joint','L_F_joint','R_F_joint']
hello_str.position = [0, 0, 0, 0, 0, 0, 0, 0, 0]
hello_str.velocity = []
hello_str.effort = []
#variaveis do método
#passo maximo entre atualizacoes das juntas
qmax = 0.1
K = 1000 #número máximo de iterações
#valor maximo que a junta pode assumir
qlim = [2.6179, 1.5358, 2.6179, 1.6144, 2.6179, 1.8413, 1.7889]
#Objetivos
[posicaod, orientd] = random_pose()
rpyd = orientacao(orientd)
#vetores colunas do sistema de coordenadas global
z = np.array([[0, 0, 1, 1]]).T
o = np.array([[0, 0, 0, 1]]).T #origem
#Inicialização dos angulos de juntas
q = np.zeros([7, 1])
for a in range(np.size(q)):
q[a] = random.uniform(-qlim[a], qlim[a])
#Parâmetros Físicos do manipulador [m]
base = 0.05 #5 cm
L = 0.075 #distância da ultima junta a extremidade do efetuador
#parametros de DH constantes
d1 = 0.075 + base
hello_str.effort = []
n = 7 #numero de juntas
x = vetor([1,0,0])
y = vetor([0,1,0])
z = vetor([0,0,1])
repeticoes = 1000
convergencia = 0
excedeu = 0
ktotal = 0
for rep in range(repeticoes):
#Gera uma pose aleatória desejada (posição + orientação)
posicaod, orientd = random_pose()
destino = posicaod
orientd2 = orientacao(orientd)
#tamanho dos elos
b = np.array([0.05,0.075,0.075,0.0725,0.0725,0.075,0.075])
#modulo dos limites de juntas
qlim = [2.6179,1.5358,2.6179,1.6144,2.6179,1.8413,1.7889]
#Confifuração do robô nula
q = np.zeros([7,1])
#Gera uma configuração aleatória
for i in range(np.size(q)):
q[i] = random.uniform(-qlim[i],qlim[i])
#Calcula os pontos e os vetores de atuação do robô atuais
p,D = Cinematica_Direta2(q)
#No Backward a junta 1 é sempre iniciada no mesmo lugar
def update_fuction(self, o,
o2): #Calcula a função de custo/fitness da particula
#(posição,orientacao) da pose desejada
#Parâmetros Físicos do manipulador [m]
base = 0.05 #5 cm
L = 0.075 #distância da ultima junta a extremidade do efetuador
#centro do efetuador em relação ao ultimo sistema de coordenada
p = np.array([0, 0, L, 1]).T
#Parâmetros de Denavit-Hartenberg do manipulado
d1 = 0.075 + base
d2 = 0
d3 = 0.15
d4 = 0
d5 = 0.145
d6 = 0
d7 = 0
a1 = 0
a2 = 0
a3 = 0
a4 = 0
a5 = 0
a6 = 0.075
a7 = 0
alpha1 = pi / 2
alpha2 = -pi / 2
alpha3 = pi / 2
alpha4 = -pi / 2
alpha5 = pi / 2
alpha6 = pi / 2
alpha7 = pi / 2
theta1 = pi / 2 + self.p[0]
theta2 = self.p[1]
theta3 = self.p[2]
theta4 = self.p[3]
theta5 = self.p[4]
theta6 = pi / 2 + self.p[5]
theta7 = pi / 2 + self.p[6]
#Calculo das matrizes homogeneas
A1 = matriz_homogenea(d1, a1, alpha1, theta1)
A2 = matriz_homogenea(d2, a2, alpha2, theta2)
A3 = matriz_homogenea(d3, a3, alpha3, theta3)
A4 = matriz_homogenea(d4, a4, alpha4, theta4)
A5 = matriz_homogenea(d5, a5, alpha5, theta5)
A6 = matriz_homogenea(d6, a6, alpha6, theta6)
A7 = matriz_homogenea(d7, a7, alpha7, theta7)
A = A1 @ (A2 @ (A3 @ (A4 @ (A5 @ (A6 @ A7)))))
#posição do efetuador em relação ao sistema de coordenadas global
p = A @ p
#calculo do erro em módulo da orientacao desejada e da particula
self.o = distancia(orientacao(A), o2, 3)
#calculo da distancia euclidiana da posição do efetuador em relação ao objetivo
self.d = distancia(p.T, o, 3)
#Calculo da função de custo
k1 = 0.1 #orientacao
k2 = 0.9 #posição
self.f = (k1 * self.o) + (k2 * self.d)
if (self.f < self.bf):
self.bf = self.f
self.bp = self.p.copy()
#variaveis do método
#passo maximo entre atualizacoes das juntas
qmax = 0.1
#Constante alpha para melhorar a aproximação
alfa = 0.1
#Constante de amortecimento
lbd = 0.005
I = np.eye(6)
K = 1000 #número máximo de iterações
#valor maximo que a junta pode assumir
qlim = [2.6179, 1.5358, 2.6179, 1.6144, 2.6179, 1.8413, 1.7889]
#Objetivos
[posicaod, orientd] = random_pose()
rpyd = orientacao(orientd) #angulos de Roll Pitch Yaw desejados
#vetores colunas do sistema de coordenadas global
z = np.array([[0, 0, 1, 1]]).T
o = np.array([[0, 0, 0, 1]]).T #origem
#Inicialização dos angulos de juntas
q = np.zeros([7, 1])
for a in range(np.size(q)):
q[a] = random.uniform(-qlim[a], qlim[a])
#Parâmetros Físicos do manipulador [m]
base = 0.05 #5 cm
L = 0.075 #distância da ultima junta a extremidade do efetuador
#parametros de DH constantes
print(f)
return qbest
#Main
#configurando o Rviz
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(100) # 10hz
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['One_joint', 'Two_joint', 'Three_joint', 'Four_joint'\
,'Five_joint','Six_joint','Seven_joint','L_F_joint','R_F_joint']
hello_str.position = [0, 0, 0, 0, 0, 0, 0, 0, 0]
hello_str.velocity = []
hello_str.effort = []
#objetivo
[posicaod, orientd] = random_pose()
orientd = orientacao(orientd)
numero_particulas = 200
dimensao = 7 #dimensão do robô
#restrições de cada ângulo
L = [2.6179, 1.5358, 2.6179, 1.6144, 2.6179, 1.8413, 1.7889]
while not rospy.is_shutdown():
solucao = PSO(posicaod, orientd, numero_particulas, dimensao, L)
print('Solução q = ', solucao)
break
