def test_dual_quaternion_sclerp():
random_state = np.random.RandomState(22)
pose1 = random_transform(random_state)
pose2 = random_transform(random_state)
dq1 = dual_quaternion_from_transform(pose1)
dq2 = dual_quaternion_from_transform(pose2)
n_steps = 100
# Ground truth: screw linear interpolation
pose12pose2 = concat(pose2, invert_transform(pose1))
screw_axis, theta = screw_axis_from_exponential_coordinates(
exponential_coordinates_from_transform(pose12pose2))
offsets = np.array([
transform_from_exponential_coordinates(screw_axis * t * theta)
for t in np.linspace(0, 1, n_steps)
])
interpolated_poses = np.array(
[concat(offset, pose1) for offset in offsets])
# Dual quaternion ScLERP
sclerp_interpolated_dqs = np.vstack([
dual_quaternion_sclerp(dq1, dq2, t)
for t in np.linspace(0, 1, n_steps)
])
sclerp_interpolated_poses_from_dqs = np.array(
[transform_from_dual_quaternion(dq) for dq in sclerp_interpolated_dqs])
for t in range(n_steps):
assert_array_almost_equal(interpolated_poses[t],
sclerp_interpolated_poses_from_dqs[t])
def test_dual_quaternion_concatenation():
random_state = np.random.RandomState(1000)
for _ in range(5):
A2B = random_transform(random_state)
B2C = random_transform(random_state)
A2C = concat(A2B, B2C)
dq1 = dual_quaternion_from_transform(A2B)
dq2 = dual_quaternion_from_transform(B2C)
dq3 = concatenate_dual_quaternions(dq2, dq1)
A2C2 = transform_from_dual_quaternion(dq3)
assert_array_almost_equal(A2C, A2C2)
def test_transform_not_added():
"""Test request for transforms that have not been added."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
C2D = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
tm.add_transform("C", "D", C2D)
assert_raises_regexp(KeyError, "Unknown frame", tm.get_transform, "A", "G")
assert_raises_regexp(KeyError, "Unknown frame", tm.get_transform, "G", "D")
assert_raises_regexp(KeyError, "Cannot compute path", tm.get_transform,
"A", "D")
def test_update_transform():
"""Update an existing transform."""
random_state = np.random.RandomState(0)
A2B1 = random_transform(random_state)
A2B2 = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B1)
tm.add_transform("A", "B", A2B2)
A2B = tm.get_transform("A", "B")
# Hack: test depends on internal member
assert_array_almost_equal(A2B, A2B2)
assert_equal(len(tm.i), 1)
assert_equal(len(tm.j), 1)
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)
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_conversions_between_exponential_coordinates_and_transform():
A2B = np.eye(4)
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
A2B2 = transform_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B = translate_transform(np.eye(4), [1.0, 5.0, 0.0])
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0])
A2B2 = transform_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B = rotate_transform(np.eye(4), active_matrix_from_angle(2, 0.5 * np.pi))
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0])
A2B2 = transform_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
random_state = np.random.RandomState(52)
for _ in range(5):
A2B = random_transform(random_state)
Stheta = exponential_coordinates_from_transform(A2B)
A2B2 = transform_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
def test_dual_quaternion_applied_to_point():
random_state = np.random.RandomState(1000)
for _ in range(5):
p_A = random_vector(random_state, 3)
A2B = random_transform(random_state)
dq = dual_quaternion_from_transform(A2B)
p_B = dq_prod_vector(dq, p_A)
assert_array_almost_equal(p_B,
transform(A2B, vector_to_point(p_A))[:3])
def test_request_added_transform():
"""Request an added transform from the transform manager."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
A2B_2 = tm.get_transform("A", "B")
assert_array_almost_equal(A2B, A2B_2)
def test_whitelist():
"""Test correct handling of whitelists for plotting."""
random_state = np.random.RandomState(2)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
nodes = tm._whitelisted_nodes(None)
assert_equal(set(["A", "B"]), nodes)
nodes = tm._whitelisted_nodes("A")
assert_equal(set(["A"]), nodes)
assert_raises_regexp(KeyError, "unknown nodes", tm._whitelisted_nodes, "C")
def test_check_consistency():
"""Test correct detection of inconsistent graphs."""
random_state = np.random.RandomState(2)
tm = TransformManager()
A2B = random_transform(random_state)
tm.add_transform("A", "B", A2B)
B2A = random_transform(random_state)
tm.add_transform("B", "A", B2A)
assert_false(tm.check_consistency())
tm = TransformManager()
A2B = random_transform(random_state)
tm.add_transform("A", "B", A2B)
assert_true(tm.check_consistency())
C2D = random_transform(random_state)
tm.add_transform("C", "D", C2D)
assert_true(tm.check_consistency())
B2C = random_transform(random_state)
tm.add_transform("B", "C", B2C)
assert_true(tm.check_consistency())
A2D_over_path = tm.get_transform("A", "D")
A2D = random_transform(random_state)
tm.add_transform("A", "D", A2D)
assert_false(tm.check_consistency())
tm.add_transform("A", "D", A2D_over_path)
assert_true(tm.check_consistency())
def test_request_concatenated_transform():
"""Request a concatenated transform from the transform manager."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
B2C = random_transform(random_state)
F2A = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
tm.add_transform("B", "C", B2C)
tm.add_transform("D", "E", np.eye(4))
tm.add_transform("F", "A", F2A)
A2C = tm.get_transform("A", "C")
assert_array_almost_equal(A2C, concat(A2B, B2C))
C2A = tm.get_transform("C", "A")
assert_array_almost_equal(
C2A, concat(invert_transform(B2C), invert_transform(A2B)))
F2B = tm.get_transform("F", "B")
assert_array_almost_equal(F2B, concat(F2A, A2B))
def test_transform():
"""Test transformation of points."""
PA = np.array([[1, 2, 3, 1], [2, 3, 4, 1]])
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
PB = transform(A2B, PA)
p0B = transform(A2B, PA[0])
p1B = transform(A2B, PA[1])
assert_array_almost_equal(PB, np.array([p0B, p1B]))
assert_raises_regexp(ValueError,
"Cannot transform array with more than 2 dimensions",
transform, A2B, np.zeros((2, 2, 4)))
def test_concat():
"""Test concatenation of transforms."""
random_state = np.random.RandomState(0)
for _ in range(5):
A2B = random_transform(random_state)
assert_transform(A2B)
B2C = random_transform(random_state)
assert_transform(B2C)
A2C = concat(A2B, B2C)
assert_transform(A2C)
p_A = np.array([0.3, -0.2, 0.9, 1.0])
p_C = transform(A2C, p_A)
C2A = invert_transform(A2C)
p_A2 = transform(C2A, p_C)
assert_array_almost_equal(p_A, p_A2)
C2A2 = concat(invert_transform(B2C), invert_transform(A2B))
p_A3 = transform(C2A2, p_C)
assert_array_almost_equal(p_A, p_A3)
def test_normalize_dual_quaternion():
dq = [1, 0, 0, 0, 0, 0, 0, 0]
dq_norm = check_dual_quaternion(dq)
assert_unit_dual_quaternion(dq_norm)
assert_array_almost_equal(dq, dq_norm)
dq = [0, 0, 0, 0, 0, 0, 0, 0]
dq_norm = check_dual_quaternion(dq)
assert_unit_dual_quaternion(dq_norm)
assert_array_almost_equal([1, 0, 0, 0, 0, 0, 0, 0], dq_norm)
random_state = np.random.RandomState(999)
for _ in range(5):
A2B = random_transform(random_state)
dq = random_state.randn() * dual_quaternion_from_transform(A2B)
dq_norm = check_dual_quaternion(dq)
assert_unit_dual_quaternion(dq_norm)
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_conversions_between_transform_and_transform_log():
A2B = np.eye(4)
transform_log = transform_log_from_transform(A2B)
assert_array_almost_equal(transform_log, np.zeros((4, 4)))
A2B2 = transform_from_transform_log(transform_log)
assert_array_almost_equal(A2B, A2B2)
random_state = np.random.RandomState(84)
A2B = transform_from(np.eye(3), p=random_vector(random_state, 3))
transform_log = transform_log_from_transform(A2B)
A2B2 = transform_from_transform_log(transform_log)
assert_array_almost_equal(A2B, A2B2)
for _ in range(5):
A2B = random_transform(random_state)
transform_log = transform_log_from_transform(A2B)
A2B2 = transform_from_transform_log(transform_log)
assert_array_almost_equal(A2B, A2B2)
def test_pickle():
"""Test if a transform manager can be pickled."""
random_state = np.random.RandomState(1)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
_, filename = tempfile.mkstemp(".pickle")
try:
pickle.dump(tm, open(filename, "wb"))
tm2 = pickle.load(open(filename, "rb"))
finally:
if os.path.exists(filename):
try:
os.remove(filename)
except WindowsError:
pass # workaround for permission problem on Windows
A2B2 = tm2.get_transform("A", "B")
assert_array_almost_equal(A2B, A2B2)
def test_transforms_from_exponential_coordinates():
A2B = np.eye(4)
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
A2B2 = transforms_from_exponential_coordinates([Stheta])[0]
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0]
assert_array_almost_equal(A2B, A2B2)
A2B = translate_transform(np.eye(4), [1.0, 5.0, 0.0])
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0])
A2B2 = transforms_from_exponential_coordinates([Stheta])[0]
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0]
assert_array_almost_equal(A2B, A2B2)
A2B = rotate_transform(np.eye(4), active_matrix_from_angle(2, 0.5 * np.pi))
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0])
A2B2 = transforms_from_exponential_coordinates([Stheta])[0]
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0]
assert_array_almost_equal(A2B, A2B2)
random_state = np.random.RandomState(53)
for _ in range(5):
A2B = random_transform(random_state)
Stheta = exponential_coordinates_from_transform(A2B)
A2B2 = transforms_from_exponential_coordinates([Stheta])[0]
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0,
0]
assert_array_almost_equal(A2B, A2B2)
def test_adjoint_of_transformation():
random_state = np.random.RandomState(94)
for _ in range(5):
A2B = random_transform(random_state)
theta_dot = 3.0 * random_state.rand()
S = random_screw_axis(random_state)
V_A = S * theta_dot
adj_A2B = adjoint_from_transform(A2B)
V_B = adj_A2B.dot(V_A)
S_mat = screw_matrix_from_screw_axis(S)
V_mat_A = S_mat * theta_dot
V_mat_B = A2B.dot(V_mat_A).dot(invert_transform(A2B))
S_B, theta_dot2 = screw_axis_from_exponential_coordinates(V_B)
V_mat_B2 = screw_matrix_from_screw_axis(S_B) * theta_dot2
assert_almost_equal(theta_dot, theta_dot2)
assert_array_almost_equal(V_mat_B, V_mat_B2)
def test_dual_quaternion_from_screw_parameters():
q = np.zeros(3)
s_axis = np.array([1, 0, 0])
h = np.inf
theta = 0
dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta)
assert_array_almost_equal(dq, np.array([1, 0, 0, 0, 0, 0, 0, 0]))
q = np.zeros(3)
s_axis = norm_vector(np.array([2.3, 2.4, 2.5]))
h = np.inf
theta = 3.6
dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta)
pq = pq_from_dual_quaternion(dq)
assert_array_almost_equal(pq, np.r_[s_axis * theta, 1, 0, 0, 0])
q = np.zeros(3)
s_axis = norm_vector(np.array([2.4, 2.5, 2.6]))
h = 0
theta = 4.1
dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta)
pq = pq_from_dual_quaternion(dq)
assert_array_almost_equal(pq[:3], [0, 0, 0])
assert_array_almost_equal(axis_angle_from_quaternion(pq[3:]),
norm_axis_angle(np.r_[s_axis, theta]))
random_state = np.random.RandomState(1001)
for _ in range(5):
A2B = random_transform(random_state)
Stheta = exponential_coordinates_from_transform(A2B)
S, theta = screw_axis_from_exponential_coordinates(Stheta)
q, s_axis, h = screw_parameters_from_screw_axis(S)
dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta)
assert_unit_dual_quaternion(dq)
dq_expected = dual_quaternion_from_transform(A2B)
assert_unit_dual_quaternion_equal(dq, dq_expected)
def test_check_transform():
"""Test input validation for transformation matrix."""
A2B = np.eye(3)
assert_raises_regexp(
ValueError, "Expected homogeneous transformation matrix with shape",
check_transform, A2B)
A2B = np.eye(4, dtype=int)
A2B = check_transform(A2B)
assert_equal(type(A2B), np.ndarray)
assert_equal(A2B.dtype, np.float64)
A2B[:3, :3] = np.array([[1, 1, 1], [0, 0, 0], [2, 2, 2]])
assert_raises_regexp(ValueError, "rotation matrix", check_transform, A2B)
A2B = np.eye(4)
A2B[3, :] = np.array([0.1, 0.0, 0.0, 1.0])
assert_raises_regexp(ValueError, "homogeneous transformation matrix",
check_transform, A2B)
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
A2B2 = check_transform(A2B)
assert_array_almost_equal(A2B, A2B2)
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)
===============================================
In this example, we will demonstrate how to use the TransformManager.
We will add several transforms to the manager and plot all frames in
two reference frames ('world' and 'A').
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.plot_utils import make_3d_axis
from pytransform3d.transformations import random_transform
from pytransform3d.transform_manager import TransformManager
random_state = np.random.RandomState(0)
A2world = random_transform(random_state)
B2world = random_transform(random_state)
A2C = random_transform(random_state)
D2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "world", A2world)
tm.add_transform("B", "world", B2world)
tm.add_transform("A", "C", A2C)
tm.add_transform("D", "B", D2B)
plt.figure(figsize=(10, 5))
ax = make_3d_axis(3, 121)
ax = tm.plot_frames_in("world", ax=ax, alpha=0.6)
ax.view_init(30, 20)
=============================
This example shows interpolated trajectories between two random poses.
The red line corresponds to an interpolation with exponential coordinates
and the green line corresponds to an interpolation with dual quaternions.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
import pytransform3d.transformations as pt
import pytransform3d.trajectories as ptr
import pytransform3d.plot_utils as ppu
random_state = np.random.RandomState(21)
pose1 = pt.random_transform(random_state)
pose2 = pt.random_transform(random_state)
dq1 = pt.dual_quaternion_from_transform(pose1)
dq2 = -pt.dual_quaternion_from_transform(pose2)
Stheta1 = pt.exponential_coordinates_from_transform(pose1)
Stheta2 = pt.exponential_coordinates_from_transform(pose2)
n_steps = 100
interpolated_dqs = (np.linspace(1, 0, n_steps)[:, np.newaxis] * dq1 +
np.linspace(0, 1, n_steps)[:, np.newaxis] * dq2)
# renormalization (not required here because it will be done with conversion)
interpolated_dqs /= np.linalg.norm(interpolated_dqs[:, :4], axis=1)[:,
np.newaxis]
interpolated_poses_from_dqs = np.array(
[pt.transform_from_dual_quaternion(dq) for dq in interpolated_dqs])
interpolated_ecs = (np.linspace(1, 0, n_steps)[:, np.newaxis] * Stheta1 +
def test_dual_quaternion_sclerp_same_dual_quaternions():
random_state = np.random.RandomState(19)
pose = random_transform(random_state)
dq = dual_quaternion_from_transform(pose)
dq2 = dual_quaternion_sclerp(dq, dq, 0.5)
assert_array_almost_equal(dq, dq2)
