code: def get_pose_error(target_pose, current_pose):
      
      error = np.zeros(6)
  
      # compute translational error
      target_pos = target_pose[:3, 3]
      current_pos = current_pose[:3, 3]
      pos_err = target_pos - current_pos
  
      # compute rotational error
      r1 = current_pose[:3, 0]
      r2 = current_pose[:3, 1]
      r3 = current_pose[:3, 2]
      r1d = target_pose[:3, 0]
      r2d = target_pose[:3, 1]
      r3d = target_pose[:3, 2]
      rot_err = 0.5 * (np.cross(r1, r1d) + np.cross(r2, r2d) + np.cross(r3, r3d))
  
      error[:3] = pos_err
      error[3:] = rot_err
      return error
docs: Computes the error corresponding to target pose - current pose as a 6-dim vector.
      The first 3 components correspond to translational error while the last 3 components
      correspond to the rotational error.
  
      Args:
          target_pose: a 4x4 homogenous matrix for the target pose
          current_pose: a 4x4 homogenous matrix for the current pose
  
      Returns:
          A 6-dim numpy array for the pose error.