code: def pose2mat(pose):
      
      homo_pose_mat = np.zeros((4, 4), dtype=np.float32)
      homo_pose_mat[:3, :3] = quat2mat(pose[1])
      homo_pose_mat[:3, 3] = np.array(pose[0], dtype=np.float32)
      homo_pose_mat[3, 3] = 1.
      return homo_pose_mat
docs: Converts pose to homogeneous matrix.
  
      Args:
          pose: a (pos, orn) tuple where pos is vec3 float cartesian, and
              orn is vec4 float quaternion.
  
      Returns:
          4x4 homogeneous matrix