def test_grid(self):
pos = [1, 2, 3]
pos_tol = [NoTolerance()] * 3
rot_tol = [NoTolerance(), NoTolerance(), SymmetricTolerance(np.pi, 3)]
pt = TolEulerPt(pos, Quaternion(), pos_tol, rot_tol)
tf_samples = pt.sample_grid()
assert_almost_equal(tf_samples[0], tf_samples[2])
assert_almost_equal(tf_samples[1][:3, :3], np.eye(3))
fig = plt.figure(figsize=plt.figaspect(1 / 3))
axes = [fig.add_subplot(1, 3, i, projection="3d") for i in [1, 2, 3]]
# all axis show the path point reference frame and have no coordinate axes
for pt, ax in zip([pt_pos, pt_eul, pt_quat], axes):
ax.set_axis_off()
plot_reference_frame(ax, tf=pt.transformation_matrix, arrow_length=0.2)
# here we plot the samples for each specific point
ax1, ax2, ax3 = axes[0], axes[1], axes[2]
plot_reference_frame(ax1, arrow_length=0.3)
for tf in pt_pos.sample_grid():
ax1.scatter(tf[0, 3], tf[1, 3], tf[2, 3], "o", c="black")
for tf in pt_eul.sample_grid():
plot_reference_frame(ax2, tf, arrow_length=0.1)
for tf in pt_quat.sample_incremental(50, SampleMethod.random_uniform):
plot_reference_frame(ax3, tf, arrow_length=0.1)
# here we tweak the view a bit to make it look nice
ax1.view_init(azim=20, elev=38)
ax1.set_xlim3d([-0.5, 0.5])
ax1.set_ylim3d([-0.5, 0.5])
ax1.set_zlim3d([-0.5, 0.5])
ax2.view_init(azim=35, elev=38)
ax3.view_init(azim=35, elev=38)
plt.tight_layout()
plt.savefig("figures/sample_examples.png", dpi=200)