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)