def test_conversions_between_screw_axis_and_parameters():
random_state = np.random.RandomState(98)
q = random_vector(random_state, 3)
s_axis = norm_vector(random_vector(random_state, 3))
h = np.inf
screw_axis = screw_axis_from_screw_parameters(q, s_axis, h)
assert_array_almost_equal(screw_axis, np.r_[0, 0, 0, s_axis])
q2, s_axis2, h2 = screw_parameters_from_screw_axis(screw_axis)
assert_array_almost_equal(np.zeros(3), q2)
assert_array_almost_equal(s_axis, s_axis2)
assert_array_almost_equal(h, h2)
for _ in range(10):
s_axis = norm_vector(random_vector(random_state, 3))
# q has to be orthogonal to s_axis so that we reconstruct it exactly
q = perpendicular_to_vector(s_axis)
h = random_state.rand() + 0.5
screw_axis = screw_axis_from_screw_parameters(q, s_axis, h)
q2, s_axis2, h2 = screw_parameters_from_screw_axis(screw_axis)
assert_array_almost_equal(q, q2)
assert_array_almost_equal(s_axis, s_axis2)
assert_array_almost_equal(h, h2)
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_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")
