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_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_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_norm_exponential_coordinates():
Stheta_only_translation = np.array([0.0, 0.0, 0.0, 100.0, 25.0, -23.0])
Stheta_only_translation2 = norm_exponential_coordinates(
Stheta_only_translation)
assert_array_almost_equal(Stheta_only_translation,
Stheta_only_translation2)
random_state = np.random.RandomState(381)
# 180 degree rotation ambiguity
for _ in range(10):
q = random_state.randn(3)
s = norm_vector(random_state.randn(3))
h = random_state.randn()
Stheta = screw_axis_from_screw_parameters(q, s, h) * np.pi
Stheta2 = screw_axis_from_screw_parameters(q, -s, -h) * np.pi
assert_array_almost_equal(
transform_from_exponential_coordinates(Stheta),
transform_from_exponential_coordinates(Stheta2))
assert_array_almost_equal(norm_exponential_coordinates(Stheta),
norm_exponential_coordinates(Stheta2))
for _ in range(10):
Stheta = random_state.randn(6)
# ensure that theta is not within [-pi, pi]
Stheta[random_state.randint(0, 3)] += np.pi + random_state.rand()
Stheta_norm = norm_exponential_coordinates(Stheta)
assert_false(np.all(Stheta == Stheta_norm))
A2B = transform_from_exponential_coordinates(Stheta)
Stheta2 = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta2, Stheta_norm)
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_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)
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(
interpolated_ecs)
s = time_scaling(np.linspace(0.0, 5.0, 50001), 5.0)
start = transform_from(
R=active_matrix_from_angle(1, -0.4 * np.pi),
p=np.array([-1, -2.5, 0])
)
goal1 = transform_from(
R=active_matrix_from_angle(1, -0.1 * np.pi),
p=np.array([-1, 1, 0])
)
goal2 = transform_from(
R=active_matrix_from_angle(2, -np.pi),
p=np.array([-0.65, -0.75, 0])
)
path1 = straight_line_path(
exponential_coordinates_from_transform(start),
exponential_coordinates_from_transform(goal1),
s
)
path2 = straight_line_path(
exponential_coordinates_from_transform(goal1),
exponential_coordinates_from_transform(goal2),
s
)
H = transforms_from_exponential_coordinates(np.vstack((path1, path2)))
ax = make_3d_axis(1.0)
trajectory = Trajectory(H, n_frames=1000, show_direction=False, s=0.3)
trajectory.add_trajectory(ax)
ax.view_init(azim=-95, elev=70)
ax.set_xlim((-2.2, 1.3))
ax.set_ylim((-2.5, 1))
def straight_line_path(start, goal, s):
"""Compute straight line path in exponential coordinates."""
start = np.asarray(start)
goal = np.asarray(goal)
return (start[np.newaxis] * (1.0 - s)[:, np.newaxis] +
goal[np.newaxis] * s[:, np.newaxis])
s = time_scaling(np.linspace(0.0, 5.0, 50001), 5.0)
start = transform_from(R=active_matrix_from_angle(1, -0.4 * np.pi),
p=np.array([-1, -2.5, 0]))
goal1 = transform_from(R=active_matrix_from_angle(1, -0.1 * np.pi),
p=np.array([-1, 1, 0]))
goal2 = transform_from(R=active_matrix_from_angle(2, -np.pi),
p=np.array([-0.65, -0.75, 0]))
path1 = straight_line_path(exponential_coordinates_from_transform(start),
exponential_coordinates_from_transform(goal1), s)
path2 = straight_line_path(exponential_coordinates_from_transform(goal1),
exponential_coordinates_from_transform(goal2), s)
H = transforms_from_exponential_coordinates(np.vstack((path1, path2)))
ax = make_3d_axis(1.0)
trajectory = Trajectory(H, n_frames=1000, show_direction=False, s=0.3)
trajectory.add_trajectory(ax)
ax.view_init(azim=-95, elev=70)
ax.set_xlim((-2.2, 1.3))
ax.set_ylim((-2.5, 1))
remove_frame(ax)
plt.show()