def test_quaternions_from_matrices_no_batch(): random_state = np.random.RandomState(85) for _ in range(5): q = pr.random_quaternion(random_state) R = pr.matrix_from_quaternion(q) q2 = pbr.quaternions_from_matrices(R) pr.assert_quaternion_equal(q, q2) a = np.array([1.0, 0.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 1.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 0.0, 1.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R)
def _rotmat(self, vector, points): """ Rotates a 3xn array of 3D coordinates from the +z normal to an arbitrary new normal vector. """ vector = vg.normalize(vector) axis = vg.perpendicular(vg.basis.z, vector) angle = vg.angle(vg.basis.z, vector, units='rad') a = np.hstack((axis, (angle, ))) R = matrix_from_axis_angle(a) r = Rot.from_matrix(R) rotmat = r.apply(points) return rotmat
def update_lines(step, start, end, n_frames, rot, profile): global velocity global last_a if step == 0: velocity = [] last_a = start if step <= n_frames / 2: t = step / float(n_frames / 2 - 1) a = pr.axis_angle_slerp(start, end, t) else: t = (step - n_frames / 2) / float(n_frames / 2 - 1) a = interpolate_linear(end, start, t) R = pr.matrix_from_axis_angle(a) # Draw new frame rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]]) rot[0].set_3d_properties([0, R[2, 0]]) rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]]) rot[1].set_3d_properties([0, R[2, 1]]) rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]]) rot[2].set_3d_properties([0, R[2, 2]]) # Update vector in frame test = R.dot(np.ones(3) / np.sqrt(3.0)) rot[3].set_data(np.array([test[0] / 2.0, test[0]]), [test[1] / 2.0, test[1]]) rot[3].set_3d_properties([test[2] / 2.0, test[2]]) velocity.append( pr.angle_between_vectors(a[:3], last_a[:3]) + a[3] - last_a[3]) last_a = a profile.set_data(np.linspace(0, 1, n_frames)[:len(velocity)], velocity) return rot
p = np.array([1.0, 1.0, 1.0]) euler = [0, 0, 0] euler[axis] = angle R = pr.active_matrix_from_intrinsic_euler_xyz(euler) pr.plot_basis(ax, R, p) p = np.array([-1.0, 1.0, 1.0]) euler = [0, 0, 0] euler[2 - axis] = angle R = pr.active_matrix_from_intrinsic_euler_zyx(euler) pr.plot_basis(ax, R, p) p = np.array([1.0, 1.0, -1.0]) R = pr.active_matrix_from_angle(axis, angle) pr.plot_basis(ax, R, p) p = np.array([1.0, -1.0, -1.0]) e = [pr.unitx, pr.unity, pr.unitz][axis] a = np.hstack((e, (angle, ))) R = pr.matrix_from_axis_angle(a) pr.plot_basis(ax, R, p) pr.plot_axis_angle(ax, a, p) p = np.array([-1.0, -1.0, -1.0]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_quaternion(q) pr.plot_basis(ax, R, p) plt.show()
""" ==================================================== Axis-Angle Representation from Two Direction Vectors ==================================================== This example shows how we can compute the axis-angle representation of a rotation that transforms a direction given by a vector 'a' to a direction given by a vector 'b'. We show both vectors, the rotation about the rotation axis and the initial and resulting coordinate frame, where the vector 'b' and its corresponding frame after the rotation are represented by shorter lines. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt from pytransform3d.rotations import axis_angle_from_two_directions, matrix_from_axis_angle, plot_axis_angle, plot_basis from pytransform3d.plot_utils import make_3d_axis, plot_vector a = np.array([1.0, 0.0, 0.0]) b = np.array([0.76958075, -0.49039301, -0.40897453]) aa = axis_angle_from_two_directions(a, b) ax = make_3d_axis(ax_s=1) plot_vector(ax, start=np.zeros(3), direction=a, s=1.0) plot_vector(ax, start=np.zeros(3), direction=b, s=0.5) plot_axis_angle(ax, aa) plot_basis(ax) plot_basis(ax, R=matrix_from_axis_angle(aa), s=0.5) plt.show()
Transform Concatenation ======================= In this example, we have a point p that is defined in a frame C, we know the transform C2B and B2A. We can construct a transform C2A to extract the position of p in frame A. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt import pytransform3d.rotations as pyrot import pytransform3d.transformations as pytr p = np.array([0.0, 0.0, -0.5]) a = np.array([0.0, 0.0, 1.0, np.pi]) B2A = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p) p = np.array([0.3, 0.4, 0.5]) a = np.array([0.0, 0.0, 1.0, -np.pi / 2.0]) C2B = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p) C2A = pytr.concat(C2B, B2A) p = pytr.transform(C2A, np.ones(4)) ax = pytr.plot_transform(A2B=B2A) pytr.plot_transform(ax, A2B=C2A) ax.scatter(p[0], p[1], p[2]) plt.show()
start[:3] = pr.norm_vector(np.random.randn(3)) end = np.array([0, 0, 0, np.pi]) end[:3] = pr.norm_vector(np.random.randn(3)) n_frames = 100 fig = plt.figure(figsize=(12, 5)) ax = fig.add_subplot(121, projection="3d") ax.set_xlim((-1, 1)) ax.set_ylim((-1, 1)) ax.set_zlim((-1, 1)) ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") Rs = pr.matrix_from_axis_angle(start) Re = pr.matrix_from_axis_angle(end) rot = [ ax.plot([0, 1], [0, 0], [0, 0], c="r", lw=3)[0], ax.plot([0, 0], [0, 1], [0, 0], c="g", lw=3)[0], ax.plot([0, 0], [0, 0], [0, 1], c="b", lw=3)[0], ax.plot([0, 1], [0, 1], [0, 1], c="gray", lw=3)[0], ax.plot([0, Rs[0, 0]], [0, Rs[1, 0]], [0, Rs[2, 0]], c="r", lw=3, alpha=0.5)[0], ax.plot([0, Rs[0, 1]], [0, Rs[1, 1]], [0, Rs[2, 1]], c="g", lw=3, alpha=0.5)[0],
========================== Plot Transformed Cylinders ========================== Plots surfaces of transformed cylindrical shells. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt from pytransform3d.transformations import transform_from, plot_transform from pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle from pytransform3d.plot_utils import plot_cylinder, remove_frame random_state = np.random.RandomState(42) A2B = transform_from(R=matrix_from_axis_angle(random_axis_angle(random_state)), p=random_state.randn(3)) ax = plot_cylinder(length=1.0, radius=0.3, thickness=0.1, wireframe=False, alpha=0.2) plot_transform(ax=ax, A2B=np.eye(4), s=0.3, lw=3) plot_cylinder(ax=ax, length=1.0, radius=0.3, thickness=0.1, A2B=A2B, wireframe=False, alpha=0.2) plot_transform(ax=ax, A2B=A2B, s=0.3, lw=3)
linear interpolation and the inner circle uses SLERP. You can play around with the value of 'end_angle' in this example. """ import numpy as np import matplotlib.pyplot as plt from pytransform3d.rotations import (matrix_from_axis_angle, quaternion_from_matrix, quaternion_slerp) from pytransform3d.trajectories import plot_trajectory T = np.linspace(0, 1, 1001) sigmoid = 0.5 * (np.tanh(1.5 * np.pi * (T - 0.5)) + 1.0) radius = 0.5 start_angle = np.deg2rad(0.0) end_angle = np.deg2rad(350.0) R1 = matrix_from_axis_angle([0, 0, 1, 0.5 * np.pi]) R2_start = matrix_from_axis_angle([1, 0, 0, start_angle]) R2_end = matrix_from_axis_angle([1, 0, 0, end_angle]) q_start = quaternion_from_matrix(R1.dot(R2_start)) q_end = quaternion_from_matrix(R1.dot(R2_end)) Y = np.zeros((len(T), 7)) Y[:, 0] = radius * np.cos(np.deg2rad(90) - end_angle * sigmoid) Y[:, 2] = radius * np.sin(np.deg2rad(90) - end_angle * sigmoid) if end_angle > np.pi: q_end *= -1.0 Y[:, 3:] = (1.0 - T)[:, np.newaxis] * q_start + T[:, np.newaxis] * q_end Y_slerp = np.zeros((len(T), 7)) Y_slerp[:, 0] = 0.7 * radius * np.cos(np.deg2rad(90) - end_angle * sigmoid) Y_slerp[:, 2] = 0.7 * radius * np.sin(np.deg2rad(90) - end_angle * sigmoid)
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()
""" ======== Plot Box ======== """ print(__doc__) import numpy as np import pytransform3d.visualizer as pv from pytransform3d.transformations import transform_from from pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle random_state = np.random.RandomState(42) fig = pv.figure() A2B = transform_from( R=matrix_from_axis_angle(random_axis_angle(random_state)), p=np.zeros(3)) fig.plot_box(size=[0.2, 0.5, 1], A2B=A2B) fig.plot_transform(A2B=A2B) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg")
""" ===================================== Axis-Angle Representation of Rotation ===================================== Any rotation can be represented with a single rotation about some axis. Here we see a frame that is rotated in multiple steps around a rotation axis. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt from pytransform3d.rotations import (random_axis_angle, matrix_from_axis_angle, plot_basis, plot_axis_angle) original = random_axis_angle(np.random.RandomState(5)) ax = plot_axis_angle(a=original) for fraction in np.linspace(0, 1, 50): a = original.copy() a[-1] = fraction * original[-1] R = matrix_from_axis_angle(a) plot_basis(ax, R, alpha=0.2) plt.subplots_adjust(left=0, right=1, bottom=0, top=1.1) ax.view_init(azim=105, elev=12) plt.show()
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()