def test_scale_transform():
"""Test scaling of transforms."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
A2B_scaled1 = scale_transform(A2B, s_xt=0.5, s_yt=0.5, s_zt=0.5)
A2B_scaled2 = scale_transform(A2B, s_t=0.5)
assert_array_almost_equal(A2B_scaled1, A2B_scaled2)
A2B_scaled1 = scale_transform(A2B, s_xt=0.5, s_yt=0.5, s_zt=0.5, s_r=0.5)
A2B_scaled2 = scale_transform(A2B, s_t=0.5, s_r=0.5)
A2B_scaled3 = scale_transform(A2B, s_d=0.5)
assert_array_almost_equal(A2B_scaled1, A2B_scaled2)
assert_array_almost_equal(A2B_scaled1, A2B_scaled3)
A2B_scaled = scale_transform(A2B, s_xr=0.0)
a_scaled = axis_angle_from_matrix(A2B_scaled[:3, :3])
assert_array_almost_equal(a_scaled[0], 0.0)
A2B_scaled = scale_transform(A2B, s_yr=0.0)
a_scaled = axis_angle_from_matrix(A2B_scaled[:3, :3])
assert_array_almost_equal(a_scaled[1], 0.0)
A2B_scaled = scale_transform(A2B, s_zr=0.0)
a_scaled = axis_angle_from_matrix(A2B_scaled[:3, :3])
assert_array_almost_equal(a_scaled[2], 0.0)
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()