def test_batch_concatenate_quaternions_1d():
random_state = np.random.RandomState(230)
q1 = pr.random_quaternion(random_state)
q2 = pr.random_quaternion(random_state)
q12 = np.empty(4)
pbr.batch_concatenate_quaternions(q1, q2, out=q12)
assert_array_almost_equal(
q12, pr.concatenate_quaternions(q1, q2))
def test_quaternion_slerp_batch():
random_state = np.random.RandomState(229)
q_start = pr.random_quaternion(random_state)
q_end = pr.random_quaternion(random_state)
t = np.linspace(0, 1, 101)
Q = pbr.quaternion_slerp_batch(q_start, q_end, t)
for i in range(len(t)):
qi = pr.quaternion_slerp(q_start, q_end, t[i])
pr.assert_quaternion_equal(Q[i], qi)
def test_transforms_from_pqs_0dims():
random_state = np.random.RandomState(0)
pq = np.empty(7)
pq[:3] = random_state.randn(3)
pq[3:] = random_quaternion(random_state)
A2B = transforms_from_pqs(pq, False)
assert_array_almost_equal(A2B, transform_from_pq(pq))
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 test_screw_parameters_from_dual_quaternion():
dq = np.array([1, 0, 0, 0, 0, 0, 0, 0])
q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq)
assert_array_almost_equal(q, np.zeros(3))
assert_array_almost_equal(s_axis, np.array([1, 0, 0]))
assert_true(np.isinf(h))
assert_almost_equal(theta, 0)
dq = dual_quaternion_from_pq(np.array([1.2, 1.3, 1.4, 1, 0, 0, 0]))
q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq)
assert_array_almost_equal(q, np.zeros(3))
assert_array_almost_equal(s_axis, norm_vector(np.array([1.2, 1.3, 1.4])))
assert_true(np.isinf(h))
assert_almost_equal(theta, np.linalg.norm(np.array([1.2, 1.3, 1.4])))
random_state = np.random.RandomState(1001)
quat = random_quaternion(random_state)
a = axis_angle_from_quaternion(quat)
dq = dual_quaternion_from_pq(np.r_[0, 0, 0, quat])
q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq)
assert_array_almost_equal(q, np.zeros(3))
assert_array_almost_equal(s_axis, a[:3])
assert_array_almost_equal(h, 0)
assert_array_almost_equal(theta, a[3])
for _ in range(5):
A2B = random_transform(random_state)
dq = dual_quaternion_from_transform(A2B)
Stheta = exponential_coordinates_from_transform(A2B)
S, theta = screw_axis_from_exponential_coordinates(Stheta)
q, s_axis, h = screw_parameters_from_screw_axis(S)
q2, s_axis2, h2, theta2 = screw_parameters_from_dual_quaternion(dq)
assert_screw_parameters_equal(q, s_axis, h, theta, q2, s_axis2, h2,
theta2)
def test_png_export():
"""Test if the graph can be exported to PNG."""
random_state = np.random.RandomState(0)
ee2robot = transform_from_pq(
np.hstack((np.array([0.4, -0.3,
0.5]), random_quaternion(random_state))))
cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id)))
object2cam = transform_from(
active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, 0.5])),
np.array([0.5, 0.1, 0.1]))
tm = TransformManager()
tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera", "robot", cam2robot)
tm.add_transform("object", "camera", object2cam)
_, filename = tempfile.mkstemp(".png")
try:
tm.write_png(filename)
assert_true(os.path.exists(filename))
except ImportError:
raise SkipTest("pydot is required for this test")
finally:
if os.path.exists(filename):
try:
os.remove(filename)
except WindowsError:
pass # workaround for permission problem on Windows
def test_quaternions_from_matrices_4d():
random_state = np.random.RandomState(84)
for _ in range(5):
q = pr.random_quaternion(random_state)
R = pr.matrix_from_quaternion(q)
q2 = pbr.quaternions_from_matrices([[R, R], [R, R]])
pr.assert_quaternion_equal(q, q2[0, 0])
pr.assert_quaternion_equal(q, q2[0, 1])
pr.assert_quaternion_equal(q, q2[1, 0])
pr.assert_quaternion_equal(q, q2[1, 1])
def test_matrices_from_pos_quat():
"""Test conversion from positions and quaternions to matrices."""
P = np.empty((10, 7))
random_state = np.random.RandomState(0)
P[:, :3] = random_state.randn(10, 3)
for t in range(10):
P[t, 3:] = random_quaternion(random_state)
H = matrices_from_pos_quat(P)
for t in range(10):
assert_array_almost_equal(P[t, :3], H[t, :3, 3])
assert_quaternion_equal(P[t, 3:], quaternion_from_matrix(H[t, :3, :3]))
def test_batch_concatenate_q_conj():
random_state = np.random.RandomState(231)
Q = np.array([pr.random_quaternion(random_state)
for _ in range(10)])
Q = Q.reshape(2, 5, 4)
Q_conj = pbr.batch_q_conj(Q)
Q_Q_conj = pbr.batch_concatenate_quaternions(Q, Q_conj)
assert_array_almost_equal(
Q_Q_conj.reshape(-1, 4),
np.array([[1, 0, 0, 0]] * 10))
def test_matrices_from_quaternions():
random_state = np.random.RandomState(83)
for _ in range(5):
q = pr.random_quaternion(random_state)
R = pbr.matrices_from_quaternions([q], normalize_quaternions=False)[0]
q2 = pr.quaternion_from_matrix(R)
pr.assert_quaternion_equal(q, q2)
for _ in range(5):
q = random_state.randn(4)
R = pbr.matrices_from_quaternions([q], normalize_quaternions=True)[0]
q2 = pr.quaternion_from_matrix(R)
pr.assert_quaternion_equal(q / np.linalg.norm(q), q2)
def test_transform_point_cloud_trans_quat(self):
from pytransform3d.rotations import random_quaternion
frame_name = 'test_frame'
for cloud_msg in TestCloudProcessing._cloud_messages:
for _ in range(TestCloudProcessing._transform_num):
translation = np.random.rand(3) * 1.0 # scale to 1 meter
rotation = random_quaternion() # quaternion
transformed_cloud = transform_point_cloud_trans_quat(
cloud_msg, translation, rotation, frame_name)
self.assertIs(
type(transformed_cloud), PointCloud2,
"'transform_point_cloud_trans_quat' does not return type 'sensor_msgs/PointCloud2'"
)
self.assertTrue(
transformed_cloud.header.frame_id == frame_name)
def test_conversions_between_dual_quternion_and_transform():
random_state = np.random.RandomState(1000)
for _ in range(5):
A2B = random_transform(random_state)
dq = dual_quaternion_from_transform(A2B)
A2B2 = transform_from_dual_quaternion(dq)
assert_array_almost_equal(A2B, A2B2)
dq2 = dual_quaternion_from_transform(A2B2)
assert_unit_dual_quaternion_equal(dq, dq2)
for _ in range(5):
p = random_vector(random_state, 3)
q = random_quaternion(random_state)
dq = dual_quaternion_from_pq(np.hstack((p, q)))
A2B = transform_from_dual_quaternion(dq)
dq2 = dual_quaternion_from_transform(A2B)
assert_unit_dual_quaternion_equal(dq, dq2)
A2B2 = transform_from_dual_quaternion(dq2)
assert_array_almost_equal(A2B, A2B2)
def test_quaternion_slerp_batch_zero_angle():
random_state = np.random.RandomState(228)
q = pr.random_quaternion(random_state)
Q = pbr.quaternion_slerp_batch(q, q, [0.5])
pr.assert_quaternion_equal(q, Q[0])
In this example, we will use the transform manager to infer a transformation
automatically.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import (random_quaternion, matrix_from_euler_xyz,
q_id)
from pytransform3d.transformations import transform_from_pq, transform_from
from pytransform3d.transform_manager import TransformManager
random_state = np.random.RandomState(0)
ee2robot = transform_from_pq(
np.hstack((np.array([0.4, -0.3, 0.5]), random_quaternion(random_state))))
cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id)))
object2cam = transform_from(matrix_from_euler_xyz(np.array([0.0, 0.0, 0.5])),
np.array([0.5, 0.1, 0.1]))
tm = TransformManager()
tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera", "robot", cam2robot)
tm.add_transform("object", "camera", object2cam)
ee2object = tm.get_transform("end-effector", "object")
ax = tm.plot_frames_in("robot", s=0.1)
ax.set_xlim((-0.25, 0.75))
ax.set_ylim((-0.5, 0.5))
ax.set_zlim((0.0, 1.0))
def test_batch_q_conj_1d():
random_state = np.random.RandomState(230)
q = pr.random_quaternion(random_state)
assert_array_almost_equal(pr.q_conj(q), pbr.batch_q_conj(q))
# 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]])
return rot
if __name__ == "__main__":
random_state = np.random.RandomState(3)
start = pr.random_quaternion(random_state)
n_frames = 1000
dt = 0.01
angular_accelerations = np.empty((n_frames, 3))
for i in range(n_frames):
angular_accelerations[i] = pr.random_compact_axis_angle(random_state)
# Integrate angular accelerations to velocities
angular_velocities = np.vstack((np.zeros(
(1, 3)), np.cumsum(angular_accelerations * dt, axis=0)))
# Integrate angular velocities to quaternions
Q = pr.quaternion_integrate(angular_velocities, q0=start, dt=dt)
fig = plt.figure(figsize=(4, 3))
ax = fig.add_subplot(111, projection="3d")
ax.set_xlim((-1, 1))
