def get_axises(self, pose):
Rz, Ry, Rx = _axis_rot_matrices(pose)
axis_x = np.dot(Rz, np.dot(Ry, [1,0,0]))
# axis_y = np.dot(Rx, [0,1,0])
axis_y = np.dot(Rz, [0,1,0])
# axis_z = np.dot(np.dot(Ry, Rx), [0,0,1])
axis_z = [0,0,1]
# ipdb.set_trace()
# return (axis_z, axis_y, [1,0,0])
return (axis_z, axis_y, axis_x)
def test_obj_pose():
from transformations import rotation_matrix, concatenate_matrices, euler_from_matrix
from utils import _axis_rot_matrices, _ypr_from_rot_matrix
alpha, beta, gamma = 0.123, -1.234, 2.345
origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
# I = identity_matrix()
Rx_tf = rotation_matrix(gamma, xaxis)
Ry_tf = rotation_matrix(beta, yaxis)
Rz_tf = rotation_matrix(alpha, zaxis)
obj = Obj("obj")
Rz, Ry, Rx = _axis_rot_matrices([alpha, beta, gamma, 0., 0., 0.])
assert np.allclose(Rx_tf[:3,:3], Rx)
assert np.allclose(Ry_tf[:3,:3], Ry)
assert np.allclose(Rz_tf[:3,:3], Rz)
R = concatenate_matrices(Rz_tf, Ry_tf, Rx_tf)
rot_mat = np.dot(Rz, np.dot(Ry, Rx))
euler = euler_from_matrix(R, 'sxyz')
assert np.allclose(R[:3,:3], rot_mat)
assert np.allclose([gamma, beta, alpha], euler)
assert np.allclose([alpha, beta, gamma], _ypr_from_rot_matrix(R[:3,:3]))