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