def movimiento():
global x, y, z
movimiento = FM()
x = float(ejex.get())
y = float(ejey.get())
z = float(ejez.get())
movimiento.data = [x, y, z]
print(movimiento.data)
pub1.publish(movimiento)
x = 0
y = 0
z = 0
rate.sleep()
ex = str(x)
ey = str(y)
ez = str(z)
ejex.set(ex)
ejey.set(ey)
ejez.set(ez)
movimiento.data = [x, y, z]
def conversion():
#Iniciamos el nodo y declaramos las suscripciones y publicaciones de topics
rospy.init_node('conversion', anonymous=True)
pub = rospy.Publisher('/UR3_1/inputs/move_pose', FM, queue_size=10)
rospy.Subscriber("UR3_1/outputs/pose", FM, callback1)
rospy.Subscriber("configuracion", String, callback2)
rospy.Subscriber("movimiento", FM, callback3)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
global iteracion
#Si no hemos recibido ningun dato de movimiento no movemos el robot
if (iteracion == True):
print("NUEVO MOVIMIENTO")
iteracion = False
global j, ro, longitud
Dt = longitud
#-------------Vamos a transformar la orientacion recibida del robot--------------------
#-------------en otra forma de orientacion que podemos manejar mas adelante------------
#Primero calculamos el ANGULO y el EJE del vector de orientacion que recibimos
angle = math.sqrt(ox * ox + oy * oy + oz * oz)
axis = [ox / angle, oy / angle, oz / angle]
#Creamos una matriz de orientacion a partir del angulo y el eje anteriores
a = np.hstack((axis, (angle, )))
Rm = matrix_from_axis_angle(a)
#Obtenemos el Eje Z de nuestra matriz, que es el que necesitamos para los calculos
z = Rm[2]
#Guardamos la posicion del robot en la variable P
P = [xi, yi, zi]
#-------------Una vez tenemos posicion y orientacion vamos a calcular la nueva--------------------
#-------------posicion y orientacion segun el movimiento que queremos para la herramienta------------
#Para la primera vez que realizamos las operaciones vamos a tomar
#una serie de valores determinados
if (j == 0):
fi = float(
fulcro / Dt
) #Calculamos fi con el valor inicial recibido por la interfaz
Pf = P + fi * Dt * z #El punto de fulcro lo calculamos al principio y no cambia
print(Pf)
#La posicion inicial de la herramienta se calcula a partir de los valores de posicion del robot mas la orientacion
Pt = P + Dt * z
#Nueva posicion de la punta con el incremento del Phantom
Ptn = [Pt[0] + Ph1, Pt[1] + Ph2, Pt[2] + Ph3]
#Nueva direccion en el eje z de la herramienta
zn = Pf - Ptn
#Distancia del punto de fulcro a la nueva posicion de la pinza
Mzn = math.sqrt(zn[0] * zn[0] + zn[1] * zn[1] + zn[2] * zn[2])
ro = Dt - Mzn
#Nueva posicion del efector final del robot
Pn = Pf + ro * zn / Mzn
# El eje Z ya lo tnemos, pero lo hacemos unitario
znn = [-zn[0] / Mzn, -zn[1] / Mzn, -zn[2] / Mzn]
#-------------Para calcular la orientacion vamos a calcular los angulos de Euler--------------------
#-------------a partir del eje Z de la matriz, el cual ya tenemos, y una vez tengamos---------------
#-------------los amgulos, llamamos a la funcion que nos calcula la matriz de orientacion-----------
b = math.atan2(math.sqrt(znn[0]**2 + znn[1]**2), znn[2])
a = 0
g = math.atan2((znn[1] / math.sin(b)), (-znn[0] / math.sin(b)))
M = eulerZYZ([a, b, g])
#Pasamos la orientacion a axis-angle, que es la que entiende nuestro simulador
a = axis_angle_from_matrix(M)
orientacion = [a[0] * a[3], a[1] * a[3], a[2] * a[3]]
r = Pn - Ptn
print("Incremento", [Ph1, Ph2, Ph3])
print(" ")
print('Nueva posicion efector Pn', Pn)
print('Nueva posicion herramienta Ptn', Ptn)
print(' ')
print("Comprobaciones")
print(' ')
print('Actual posicion herramienta Pt', Pt)
print('Longitud herramienta Dt',
math.sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]))
print(' ')
print(' ')
#Creamos la variable que contiene la posicion y orientacion que le vamos a dar a nuestro robot
pose = FM()
pose.data = [
Pn[0], Pn[1], Pn[2], orientacion[0], orientacion[1],
orientacion[2]
]
#pose.data = [Pn[0], Pn[1], Pn[2], ox, oy, oz]
#Publicamos la pose
pub.publish(pose)
#Aumentamos la iteracion
j = j + 1
rate.sleep()
rospy.spin()
def conversion():
rospy.init_node('conversion', anonymous=True)
pub = rospy.Publisher('/UR3_1/inputs/move_pose', FM, queue_size=10)
rospy.Subscriber("UR3_1/outputs/pose", FM, callback1)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
#Introducimos el incremento de posiscion que queremos en cada eje
Ph1 = float(raw_input("Introduce Ph1: "))
Ph2 = float(raw_input("Introduce Ph2: "))
Ph3 = float(raw_input("Introduce Ph3: "))
#Ph = 0.05
#Esto es para que se realicen los calculos solo en caso de introducir valores
if not (Ph1 == 0):
rate.sleep()
#Convertimos la orientacion de la herramienta en matriz de orientacion para obtener
#la direccion en el eje z
angle = math.sqrt(ox * ox + oy * oy + oz * oz)
axis = [ox / theta, oy / theta, oz / theta]
a = np.hstack((axis, (angle, )))
Rm = matrix_from_axis_angle(a)
x = Rm[0]
y = Rm[1]
z = Rm[2]
#Obtenemos el punto de fulcro y la posicion de la punta
P = [xi, yi, zi]
Pf = P + fi * Dt * z
Pt = P + Dt * z
#Nueva posicion de la punta con el incremento del Phantom
Pd = Rh * Ks * np.array([[Ph1], [0], [0]])
#Ptn = Pt + np.transpose(Pd)
Ptn = Pt + [Ph1, Ph2, Ph3]
#Nueva direccion en el eje z de la herramienta
zn = Pf - Ptn
#Distancia del punto de fulcro a la nueva posicion de la pinza
Mzn = math.sqrt(zn[0] * zn[0] + zn[1] * zn[1] + zn[2] * zn[2])
ro = Dt - Mzn
#Nueva posicion del efector finalcdel robot
Pn = Pf + ro * zn / Mzn
Rm[2] = -zn
r = Pn - Ptn
print(math.sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]))
#a = axis_angle_from_matrix(Rm)
#orientacion = [a[0]*a[3], a[1]*a[3], a[2]*a[3]]
pose = FM()
pose.data = [Pn[0], Pn[1], Pn[2], zn[0], zn[1], zn[2]]
pub.publish(pose)
rate.sleep()
rospy.spin()