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_invert_transform_without_check():
"""Test inversion of transformations."""
random_state = np.random.RandomState(0)
for _ in range(5):
A2B = random_state.randn(4, 4)
A2B = A2B + A2B.T
B2A = invert_transform(A2B, check=False)
A2B2 = np.linalg.inv(B2A)
assert_array_almost_equal(A2B, A2B2)
def test_invert_transform():
"""Test inversion of transformations."""
random_state = np.random.RandomState(0)
for _ in range(5):
R = matrix_from(a=random_axis_angle(random_state))
p = random_vector(random_state)
A2B = transform_from(R, p)
B2A = invert_transform(A2B)
A2B2 = np.linalg.inv(B2A)
assert_array_almost_equal(A2B, A2B2)
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_request_inverse_transform():
"""Request an inverse 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)
B2A = tm.get_transform("B", "A")
B2A_2 = invert_transform(A2B)
assert_array_almost_equal(B2A, B2A_2)
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_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)
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
# Ground truth screw motion: linear interpolation of rotation about and
# translation along the screw axis
pose12pose2 = pt.concat(pose2, pt.invert_transform(pose1))
screw_axis, theta = pt.screw_axis_from_exponential_coordinates(
pt.exponential_coordinates_from_transform(pose12pose2))
offsets = np.array([
pt.transform_from_exponential_coordinates(screw_axis * t * theta)
for t in np.linspace(0, 1, n_steps)
])
interpolated_poses = np.array([pt.concat(offset, pose1) for offset in offsets])
# Linear interpolation of dual quaternions
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(
