def test_axis_angle_from_matrix2(self, expected_axis, expected_angle):
m = rotation_matrix(expected_angle, expected_axis)
actual_axis = w.compile_and_execute(
lambda x: w.axis_angle_from_matrix(x)[0], [m])
actual_angle = w.compile_and_execute(
lambda x: w.axis_angle_from_matrix(x)[1], [m])
expected_angle, expected_axis, _ = rotation_from_matrix(m)
compare_axis_angle(actual_angle, actual_axis, expected_angle,
expected_axis)
def test_axis_angle_from_quaternion(self, q):
axis2, angle2 = quat2axangle([q[-1], q[0], q[1], q[2]])
axis = w.compile_and_execute(
lambda x, y, z, w_: w.axis_angle_from_quaternion(x, y, z, w_)[0],
q)
angle = w.compile_and_execute(
lambda x, y, z, w_: w.axis_angle_from_quaternion(x, y, z, w_)[1],
q)
compare_axis_angle(angle, axis, angle2, axis2, 2)
def test_distance_point_to_line_segment3(self):
p = np.array([0, 1, 2])
start = np.array([0, 0, 0])
end = np.array([0, 0, 1])
distance = w.compile_and_execute(
lambda a, b, c: w.distance_point_to_line_segment(a, b, c)[0],
[p, start, end])
nearest = w.compile_and_execute(
lambda a, b, c: w.distance_point_to_line_segment(a, b, c)[1],
[p, start, end])
assert distance == 1.4142135623730951
def test_rpy_from_matrix2(self, q):
matrix = quaternion_matrix(q)
roll = w.compile_and_execute(lambda m: w.rpy_from_matrix(m)[0],
[matrix])
pitch = w.compile_and_execute(lambda m: w.rpy_from_matrix(m)[1],
[matrix])
yaw = w.compile_and_execute(lambda m: w.rpy_from_matrix(m)[2],
[matrix])
r1 = w.compile_and_execute(w.rotation_matrix_from_rpy,
[roll, pitch, yaw])
self.assertTrue(np.isclose(r1, matrix).all(),
msg='{} != {}'.format(r1, matrix))
def test_distance_point_to_line_segment1(self):
p = np.array([0, 0, 0])
start = np.array([0, 0, 0])
end = np.array([0, 0, 1])
distance = w.compile_and_execute(
lambda a, b, c: w.distance_point_to_line_segment(a, b, c)[0],
[p, start, end])
nearest = w.compile_and_execute(
lambda a, b, c: w.distance_point_to_line_segment(a, b, c)[1],
[p, start, end])
assert distance == 0
assert nearest[0] == 0
assert nearest[1] == 0
assert nearest[2] == 0
def test_axis_angle_from_rpy(self, roll, pitch, yaw):
angle2, axis2, _ = rotation_from_matrix(euler_matrix(roll, pitch, yaw))
axis = w.compile_and_execute(
lambda r, p, y: w.axis_angle_from_rpy(r, p, y)[0],
[roll, pitch, yaw])
angle = w.compile_and_execute(
lambda r, p, y: w.axis_angle_from_rpy(r, p, y)[1],
[roll, pitch, yaw])
if angle < 0:
angle = -angle
axis = [-x for x in axis]
if angle2 < 0:
angle2 = -angle2
axis2 *= -1
compare_axis_angle(angle, axis, angle2, axis2)
def test_rpy_from_matrix(self, q):
matrix = quaternion_matrix(q)
roll = w.compile_and_execute(lambda m: w.rpy_from_matrix(m)[0],
[matrix])
pitch = w.compile_and_execute(lambda m: w.rpy_from_matrix(m)[1],
[matrix])
yaw = w.compile_and_execute(lambda m: w.rpy_from_matrix(m)[2],
[matrix])
roll2, pitch2, yaw2 = euler_from_matrix(matrix)
self.assertTrue(np.isclose(roll, roll2),
msg='{} != {}'.format(roll, roll2))
self.assertTrue(np.isclose(pitch, pitch2),
msg='{} != {}'.format(pitch, pitch2))
self.assertTrue(np.isclose(yaw, yaw2),
msg='{} != {}'.format(yaw, yaw2))
def test_rot_of(self, x, y, z, q):
r1 = w.compile_and_execute(
lambda *args: w.rotation_of(w.frame_quaternion(*args)),
[x, y, z, q[0], q[1], q[2], q[3]])
r2 = quaternion_matrix(q)
self.assertTrue(np.isclose(r1, r2).all(),
msg='\n{} != \n{}'.format(r1, r2))
def test_quaternion_from_matrix(self, q):
matrix = quaternion_matrix(q)
q2 = quaternion_from_matrix(matrix)
q1_2 = w.compile_and_execute(w.quaternion_from_matrix, [matrix])
self.assertTrue(np.isclose(q1_2, q2).all()
or np.isclose(q1_2, -q2).all(),
msg='{} != {}'.format(q, q1_2))
def test_scale(self, v, a):
if np.linalg.norm(v) == 0:
r2 = [0, 0, 0]
else:
r2 = v / np.linalg.norm(v) * a
np.testing.assert_array_almost_equal(
w.compile_and_execute(w.scale, [v, a]), r2)
def test_slerp2(self, q1, q2, t):
r1 = w.compile_and_execute(w.quaternion_slerp, [q1, q2, t])
r2 = quaternion_slerp(q1, q2, t)
self.assertTrue(np.isclose(r1, r2, atol=1e-3).all()
or np.isclose(r1, -r2, atol=1e-3).all(),
msg='q1={} q2={} t={}\n{} != {}'.format(
q1, q2, t, r1, r2))
def test_dot(self, vectors):
u, v = vectors
u = np.array(u, ndmin=2)
v = np.array(v, ndmin=2)
result = w.compile_and_execute(w.dot, [u, v.T])
if not np.isnan(result):
self.assertTrue(np.isclose(result, np.dot(u, v.T)))
def test_entrywise_product(self, q1, q2):
# TODO use real matrices
m1 = quat2mat(q1)
m2 = quat2mat(q2)
r1 = w.compile_and_execute(w.entrywise_product, [m1, m2])
r2 = m1 * m2
np.testing.assert_array_almost_equal(r1, r2)
def test_shorted_angular_distance(self, angle1, angle2):
try:
expected = shortest_angular_distance(angle1, angle2)
except ValueError:
expected = np.nan
actual = w.compile_and_execute(w.shortest_angular_distance,
[angle1, angle2])
self.assertTrue(np.isclose(actual, expected, equal_nan=True))
def test_frame3_rpy(self, x, y, z, roll, pitch, yaw):
r2 = euler_matrix(roll, pitch, yaw)
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
np.testing.assert_array_almost_equal(
w.compile_and_execute(w.frame_rpy, [x, y, z, roll, pitch, yaw]),
r2)
def test_frame3_quaternion(self, x, y, z, q):
r2 = quaternion_matrix(q)
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
np.testing.assert_array_almost_equal(
w.compile_and_execute(w.frame_quaternion, [x, y, z] + q.tolist()),
r2)
def test_frame3_axis_angle(self, x, y, z, axis, angle):
r2 = rotation_matrix(angle, np.array(axis))
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
np.testing.assert_array_almost_equal(
w.compile_and_execute(w.frame_axis_angle, [x, y, z, axis, angle]),
r2)
def test_translation3(self, x, y, z):
r1 = w.compile_and_execute(w.translation3, [x, y, z])
r2 = np.identity(4)
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
self.assertTrue(np.isclose(r1, r2).all(),
msg='{} != {}'.format(r1, r2))
def test_trans_of(self, x, y, z, q):
r1 = w.compile_and_execute(
lambda *args: w.translation_of(w.frame_quaternion(*args)),
[x, y, z, q[0], q[1], q[2], q[3]])
r2 = np.identity(4)
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
for i in range(r2.shape[0]):
for j in range(r2.shape[1]):
self.assertAlmostEqual(float(r1[i, j]), r2[i, j])
def test_rotation_distance(self, q1, q2):
m1 = quaternion_matrix(q1)
m2 = quaternion_matrix(q2)
actual_angle = w.compile_and_execute(w.rotation_distance, [m1, m2])
expected_angle, _, _ = rotation_from_matrix(m1.T.dot(m2))
expected_angle = expected_angle
try:
self.assertAlmostEqual(shortest_angular_distance(
actual_angle, expected_angle),
0,
places=3)
except AssertionError:
self.assertAlmostEqual(shortest_angular_distance(
actual_angle, -expected_angle),
0,
places=3)
def test_inverse_frame(self, x, y, z, q):
f = quaternion_matrix(q)
f[0, 3] = x
f[1, 3] = y
f[2, 3] = z
r2 = PyKDL.Frame()
r2.M = PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3])
r2.p[0] = x
r2.p[1] = y
r2.p[2] = z
r2 = r2.Inverse()
r2 = pykdl_frame_to_numpy(r2)
self.assertTrue(
np.isclose(w.compile_and_execute(w.inverse_frame, [f]),
r2,
atol=1.e-4,
rtol=1.e-4).all())
def test_fmod(self, a, b):
ref_r = np.fmod(a, b)
self.assertTrue(
np.isclose(w.compile_and_execute(w.fmod, [a, b]),
ref_r,
equal_nan=True))
def test_sum_column(self, m):
actual_sum = w.compile_and_execute(w.sum_column, [m])
expected_sum = np.sum(m, axis=1)
self.assertTrue(np.all(np.isclose(actual_sum, expected_sum)))
def test_sum(self, m):
actual_sum = w.compile_and_execute(w.sum, [m])
expected_sum = np.sum(m)
self.assertTrue(np.isclose(actual_sum, expected_sum))
def test_quaternion_diff(self, q1, q2):
q3 = quaternion_multiply(quaternion_conjugate(q1), q2)
q4 = w.compile_and_execute(w.quaternion_diff, [q1, q2])
self.assertTrue(np.isclose(q3, q4).all() or np.isclose(q3, -q4).all(),
msg='{} != {}'.format(q1, q4))
def test_min(self, f1, f2):
self.assertAlmostEqual(w.compile_and_execute(w.Min, [f1, f2]),
min(f1, f2),
places=7)
def test_normalize_angle(self, a):
ref_r = normalize_angle(a)
self.assertAlmostEqual(w.compile_and_execute(w.normalize_angle, [a]),
ref_r,
places=5)
def test_sign(self, f1):
self.assertAlmostEqual(w.compile_and_execute(w.sign, [f1]),
np.sign(f1),
places=7)
def test_if_greater_eq_zero(self, condition, if_result, else_result):
self.assertAlmostEqual(
w.compile_and_execute(w.if_greater_eq_zero,
[condition, if_result, else_result]),
np.float(if_result if condition >= 0 else else_result),
places=7)
def test_normalize_angle_positive(self, a):
expected = normalize_angle_positive(a)
actual = w.compile_and_execute(w.normalize_angle_positive, [a])
self.assertAlmostEqual(shortest_angular_distance(expected, actual),
0.0,
places=5)
