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_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_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_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_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_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_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_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)
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 +
np.linspace(0, 1, n_steps)[:, np.newaxis] * Stheta2)
interpolates_poses_from_ecs = ptr.transforms_from_exponential_coordinates(