def get_orientation_error(target_orn, current_orn):
    """
    Returns the difference between two quaternion orientations as a 3 DOF numpy array.
    For use in an impedance controller / task-space PD controller.

    Args:
        target_orn: 4-dim iterable, desired orientation as a (x, y, z, w) quaternion
        current_orn: 4-dim iterable, current orientation as a (x, y, z, w) quaternion

    Returns:
        orn_error: 3-dim numpy array for current orientation error, corresponds to
            (target_orn - current_orn)
    """
    current_orn = np.array(
        [current_orn[3], current_orn[0], current_orn[1], current_orn[2]]
    )
    target_orn = np.array([target_orn[3], target_orn[0], target_orn[1], target_orn[2]])

    pinv = np.zeros((3, 4))
    pinv[0, :] = [-current_orn[1], current_orn[0], -current_orn[3], current_orn[2]]
    pinv[1, :] = [-current_orn[2], current_orn[3], current_orn[0], -current_orn[1]]
    pinv[2, :] = [-current_orn[3], -current_orn[2], current_orn[1], current_orn[0]]
    orn_error = 2.0 * pinv.dot(np.array(target_orn))
    return orn_error
Returns the difference between two quaternion orientations as a 3 DOF numpy array.
    For use in an impedance controller / task-space PD controller.

    Args:
        target_orn: 4-dim iterable, desired orientation as a (x, y, z, w) quaternion
        current_orn: 4-dim iterable, current orientation as a (x, y, z, w) quaternion

    Returns:
        orn_error: 3-dim numpy array for current orientation error, corresponds to
            (target_orn - current_orn)