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()
