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))