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_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_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 animation_callback(step, cylinder, cylinder_frame, prev_cylinder2world,
Stheta_dot, inertia_inv):
if step == 0: # Reset cylinder state
prev_cylinder2world[:, :] = np.eye(4)
Stheta_dot[:] = 0.0
# Apply constant wrench
wrench_in_cylinder = np.array([0.1, 0.001, 0.001, 0.01, 1.0, 1.0])
dt = 0.0005
Stheta_ddot = np.dot(inertia_inv, wrench_in_cylinder)
Stheta_dot += dt * Stheta_ddot
cylinder2world = transform_from_exponential_coordinates(
dt * Stheta_dot).dot(prev_cylinder2world)
# Update visualization
cylinder_frame.set_data(cylinder2world)
cylinder.set_data(cylinder2world)
prev_cylinder2world[:, :] = cylinder2world
return cylinder_frame, cylinder
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import active_matrix_from_extrinsic_roll_pitch_yaw
from pytransform3d.transformations import (
plot_transform, plot_screw, screw_axis_from_screw_parameters,
transform_from_exponential_coordinates, concat, transform_from)
# Screw parameters
q = np.array([-0.2, -0.1, -0.5])
s_axis = np.array([0, 0, 1])
h = 0.05
theta = 5.5 * np.pi
Stheta = screw_axis_from_screw_parameters(q, s_axis, h) * theta
A2B = transform_from_exponential_coordinates(Stheta)
origin = transform_from(
active_matrix_from_extrinsic_roll_pitch_yaw([0.5, -0.3, 0.2]),
np.array([0.0, 0.1, 0.1]))
ax = plot_transform(A2B=origin, s=0.4)
plot_transform(ax=ax, A2B=concat(A2B, origin), s=0.2)
plot_screw(ax=ax,
q=q,
s_axis=s_axis,
h=h,
theta=theta,
A2B=origin,
s=1.5,
alpha=0.6)
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])
# Screw linear interpolation of dual quaternions (ScLERP)
sclerp_interpolated_dqs = np.vstack([
