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)