def test_sign(self, f1):
self.assertAlmostEqual(w.compile_and_execute(w.sign, [f1]),
np.sign(f1),
places=7)
self.assertAlmostEqual(w.compile_and_execute(w.diffable_sign, [f1]),
np.sign(f1),
places=7)
def test_if_eq(self, a, b, if_result, else_result):
r1 = w.compile_and_execute(w.diffable_if_eq,
[a, b, if_result, else_result])
r2 = np.float(if_result if a == b else else_result)
self.assertTrue(np.isclose(r1, r2), msg='{} != {}'.format(r1, r2))
self.assertTrue(
np.isclose(
w.compile_and_execute(w.if_eq, [a, b, if_result, else_result]),
np.float(if_result if a == b else else_result)))
def test_if_greater_eq(self, a, b, if_result, else_result):
self.assertAlmostEqual(w.compile_and_execute(
w.diffable_if_greater_eq, [a, b, if_result, else_result]),
np.float(if_result if a >= b else else_result),
places=7)
self.assertAlmostEqual(w.compile_and_execute(
w.if_greater_eq, [a, b, if_result, else_result]),
np.float(if_result if a >= b else else_result),
places=7)
def test_axis_angle_from_quaternion2(self):
q = [0, 0, 0, 1.0000001]
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_if_eq_zero(self, condition, if_result, else_result):
self.assertAlmostEqual(
w.compile_and_execute(w.diffable_if_eq_zero,
[condition, if_result, else_result]),
np.float(if_result if condition == 0 else else_result),
places=7)
self.assertAlmostEqual(
w.compile_and_execute(w.if_eq_zero,
[condition, if_result, else_result]),
np.float(if_result if condition == 0 else else_result),
places=7)
def test_min(self, f1, f2):
self.assertAlmostEqual(w.compile_and_execute(w.diffable_min, [f1, f2]),
min(f1, f2),
places=7)
self.assertAlmostEqual(w.compile_and_execute(w.diffable_min_fast,
[f1, f2]),
min(f1, f2),
places=7)
self.assertAlmostEqual(w.compile_and_execute(w.Min, [f1, f2]),
min(f1, f2),
places=7)
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)
# axis = [x, y, z]
# angle = float(angle)
compare_axis_angle(angle, axis, angle2, axis2, 2)
def test_if_greater_eq_zero(self, condition, if_result, else_result):
result = w.compile_and_execute(w.diffable_if_greater_eq_zero,
[condition, if_result, else_result])
expected = np.float(if_result if condition >= 0 else else_result)
self.assertAlmostEqual(result,
expected,
msg='{} >= 0: {} instead of {}'.format(
condition, result, expected))
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_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_axis_angle_from_rpy(self, roll, pitch, yaw):
axis2, angle2 = euler2axangle(roll, pitch, yaw)
assume(abs(angle2) > SMALL_NUMBER)
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_slerp(self, q1, q2, t):
r1 = w.compile_and_execute(w.diffable_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_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_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_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_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_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_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_shorted_angular_distance(self, f1, f2):
angle1 = np.pi * f1
angle2 = np.pi * f2
ref_distance = shortest_angular_distance(angle1, angle2)
distance = w.compile_and_execute(w.shortest_angular_distance,
[angle1, angle2])
self.assertAlmostEqual(distance, ref_distance, places=7)
assert abs(distance) <= np.pi
def test_normalize_angle_positive(self, a):
a = a * np.pi
ref_r = normalize_angle_positive(a)
sw_r = w.compile_and_execute(w.normalize_angle_positive, [a])
self.assertAlmostEqual(shortest_angular_distance(ref_r, sw_r),
0.0,
places=5)
def test_axis_angle_from_matrix_stable(self, axis, angle):
assume(angle < np.pi - 0.0001)
m = rotation_matrix(angle, axis)
axis2 = w.compile_and_execute(lambda x: w.axis_angle_from_matrix(x)[0],
[m])
angle2 = w.compile_and_execute(
lambda x: w.axis_angle_from_matrix(x)[1], [m])
angle, axis, _ = rotation_from_matrix(m)
if angle < 0:
angle = -angle
axis = [-x for x in axis]
if angle2 < 0:
angle2 = -angle2
axis2 *= -1
self.assertTrue(np.isclose(angle, angle2),
msg='{} != {}'.format(angle, angle2))
self.assertTrue(np.isclose(axis, axis2).all(),
msg='{} != {}'.format(axis, axis2))
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_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])
try:
roll = float(roll.evalf(real=True))
except AttributeError:
pass
try:
pitch = float(pitch.evalf(real=True))
except AttributeError:
pass
try:
yaw = float(yaw.evalf(real=True))
except AttributeError:
pass
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_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_quaternion_multiply(self, q, p):
r1 = w.compile_and_execute(w.quaternion_multiply, [q, p])
r2 = quaternion_multiply(q, p)
self.assertTrue(np.isclose(r1, r2).all() or np.isclose(r1, -r2).all(),
msg='{} != {}'.format(r1, r2))
def test_normalize_angle(self, a):
a = a * np.pi
ref_r = normalize_angle(a)
self.assertAlmostEqual(w.compile_and_execute(w.normalize_angle, [a]),
ref_r,
places=5)
def test_fmod(self, a, b):
assume(b != 0)
ref_r = np.fmod(a, b)
self.assertAlmostEqual(w.compile_and_execute(w.fmod, [a, b]),
ref_r,
places=4)
def test_save_division(self, f1, f2):
self.assertTrue(
np.isclose(w.compile_and_execute(w.save_division, [f1, f2]),
f1 / f2 if f2 != 0 else 0))
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_quaternion_conjugate(self, q):
r1 = w.compile_and_execute(w.quaternion_conjugate, [q])
r2 = quaternion_conjugate(q)
self.assertTrue(np.isclose(r1, r2).all() or np.isclose(r1, -r2).all(),
msg='{} != {}'.format(r1, r2))
