def quatswap(quat):
mat = transformations.quaternion_matrix(quat)
mat_new = np.c_[mat[:,2],mat[:,1],-mat[:,0],mat[:,3]]
return transformations.quaternion_from_matrix(mat_new)
def quatswap(quat):
mat = transformations.quaternion_matrix(quat)
mat_new = np.c_[mat[:, 2], mat[:, 1], -mat[:, 0], mat[:, 3]]
return transformations.quaternion_from_matrix(mat_new)
def mat2quat(mat33):
mat44 = np.eye(4)
mat44[:3, :3] = mat33
return transformations.quaternion_from_matrix(mat44)
def hmat_to_pose(hmat):
quat = transformations.quaternion_from_matrix(hmat)
trans = hmat[:3, 3]
return trans_rot_to_pose(trans, quat)
def mat2quat(mat33):
mat44 = np.eye(4)
mat44[:3,:3] = mat33
return transformations.quaternion_from_matrix(mat44)
def hmat_to_pose(hmat):
quat = transformations.quaternion_from_matrix(hmat)
trans = hmat[:3,3]
return trans_rot_to_pose(trans, quat)
