def test_exponential_coordinates_from_transforms_2dims():
random_state = np.random.RandomState(843)
Sthetas = random_state.randn(4, 4, 6)
H = transforms_from_exponential_coordinates(Sthetas)
Sthetas2 = exponential_coordinates_from_transforms(H)
H2 = transforms_from_exponential_coordinates(Sthetas2)
assert_array_almost_equal(H, H2)
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)
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)
ax = pt.plot_transform(A2B=pose1, s=0.3, ax_s=2)
pt.plot_transform(A2B=pose2, s=0.3, ax=ax)
traj_from_dqs = ppu.Trajectory(interpolated_poses_from_dqs, s=0.1, c="g")
traj_from_dqs.add_trajectory(ax)
traj_from_ecs = ppu.Trajectory(interpolates_poses_from_ecs, s=0.1, c="r")
traj_from_ecs.add_trajectory(ax)
plt.show()
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()
