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_conversions_between_screw_axis_and_exponential_coordinates():
S = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0])
theta = 1.0
Stheta = exponential_coordinates_from_screw_axis(S, theta)
S2, theta2 = screw_axis_from_exponential_coordinates(Stheta)
assert_array_almost_equal(S, S2)
assert_almost_equal(theta, theta2)
S = np.zeros(6)
theta = 0.0
S2, theta2 = screw_axis_from_exponential_coordinates(np.zeros(6))
assert_array_almost_equal(S, S2)
assert_almost_equal(theta, theta2)
random_state = np.random.RandomState(33)
for _ in range(5):
S = random_screw_axis(random_state)
theta = random_state.rand()
Stheta = exponential_coordinates_from_screw_axis(S, theta)
S2, theta2 = screw_axis_from_exponential_coordinates(Stheta)
assert_array_almost_equal(S, S2)
assert_almost_equal(theta, theta2)
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)
base2ee = tm.get_transform("robot_arm", "tcp")
mass = 1.0
wrench_in_ee = np.array([0.0, 0.0, 0.0, 0.0, -9.81, 0.0]) * mass
wrench_in_base = np.dot(pt.adjoint_from_transform(base2ee).T, wrench_in_ee)
fig = pv.figure()
fig.plot_graph(tm, "robot_arm", s=0.1, show_visuals=True)
fig.plot_transform(s=0.4)
fig.plot_transform(A2B=ee2base, s=0.1)
mass2base = np.copy(ee2base)
mass2base[2, 3] += 0.075
fig.plot_sphere(radius=0.025, A2B=mass2base)
S, theta = pt.screw_axis_from_exponential_coordinates(wrench_in_base)
q, s, h = pt.screw_parameters_from_screw_axis(S)
plot_screw(fig, q, s, h, theta * 0.05)
S, theta = pt.screw_axis_from_exponential_coordinates(wrench_in_ee)
q, s, h = pt.screw_parameters_from_screw_axis(S)
plot_screw(fig, q, s, h, theta * 0.05, A2B=ee2base)
fig.view_init()
if "__file__" in globals():
fig.show()
else:
fig.save_image("__open3d_rendered_image.jpg")
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(
[pt.transform_from_dual_quaternion(dq) for dq in interpolated_dqs])
