def quat_diff_active()

in gym_hil/controllers/opspace.py [0:0]

• 9 lines of code
• 1 McCabe index (conditional complexity)

def quat_diff_active(source_quat: np.ndarray, target_quat: np.ndarray) -> np.ndarray:
    """Compute the quaternion difference from source to target.

    Args:
        source_quat: Source quaternion [w, x, y, z]
        target_quat: Target quaternion [w, x, y, z]

    Returns:
        Quaternion representing rotation from source to target
    """
    # q_diff = q_target * q_source^(-1)
    # For unit quaternions, inverse is conjugate
    source_conj = np.array([source_quat[0], -source_quat[1], -source_quat[2], -source_quat[3]])

    # Quaternion multiplication: q1 * q2
    w1, x1, y1, z1 = target_quat
    w2, x2, y2, z2 = source_conj

    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2

    return np.array([w, x, y, z])