def quat_to_axisangle()

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

• 15 lines of code
• 3 McCabe index (conditional complexity)

def quat_to_axisangle(quat: np.ndarray) -> np.ndarray:
    """Convert a quaternion to axis-angle representation.

    Args:
        quat: Quaternion [w, x, y, z]

    Returns:
        Axis-angle vector (angle * axis)
    """
    w, x, y, z = quat

    # Normalize quaternion
    norm = np.sqrt(w * w + x * x + y * y + z * z)
    if norm < 1e-10:
        return np.zeros(3)

    w /= norm
    x /= norm
    y /= norm
    z /= norm

    # Compute angle
    angle = 2.0 * np.arccos(np.clip(w, -1.0, 1.0))

    # Compute axis
    sin_half_angle = np.sqrt(1.0 - w * w)

    if sin_half_angle < 1e-10:
        # Small angle, arbitrary axis
        return np.array([angle * x, angle * y, angle * z])

    axis = np.array([x, y, z]) / sin_half_angle

    # Return axis-angle vector
    return angle * axis