def test_axis_angle_from_quaternion2(self):
q = [0,0,0,1.0000001]
axis2, angle2 = quat2axangle([q[-1],q[0],q[1],q[2]])
x,y,z, angle = speed_up_and_execute(spw.axis_angle_from_quaternion, list(q))
axis = [x,y,z]
angle = float(angle)
compare_axis_angle(angle, axis, angle2, axis2, 2)
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_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_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_axis_angle_from_rpy(self, roll, pitch, yaw):
axis2, angle2 = euler2axangle(roll, pitch, yaw)
assume(abs(angle2) > SMALL_NUMBER)
axis, angle = spw.axis_angle_from_rpy(roll, pitch, yaw)
angle = float(angle)
axis = np.array(axis).astype(float).T[0]
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_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)
