How to define "rotation_threshold"?

[GuoPingPan]

If it’s not a bug, please use discussions: https://github.com/NVlabs/curobo/discussions

Please provide the below information in addition to your issue:

1. cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]): isaac sim
2. python version: 3.9
3. Isaac Sim version (if using): 23.0

---

Issue Details

I had found that "rotation_threshold" is the geodesic distance between reached quaternion and target quaternion. But it is hard for me to understand the pose_distance_kernel.cu.

Is there a mathematical definition? Is it the same as the quaternion geodesic distance calculated by the code below?

import torch

def quaternion_geodesic_distance(q1, q2):
    """
    Calculate the geodesic distance between two sets of quaternions.

    Parameters:
    q1: torch.Tensor of shape (N, 4)
    q2: torch.Tensor of shape (N, 4)

    Returns:
    torch.Tensor: Geodesic distances of shape (N,)
    """
    # Normalize quaternions
    q1 = q1 / q1.norm(dim=1, keepdim=True)
    q2 = q2 / q2.norm(dim=1, keepdim=True)

    # Compute the dot product
    dot_product = torch.sum(q1 * q2, dim=1)

    # Clamp dot product to avoid numerical issues with arccos
    dot_product = torch.clamp(dot_product, -1.0, 1.0)

    # Calculate the angle between the quaternions
    angle = 2 * torch.acos(torch.abs(dot_product))

    return angle

# Example usage
q1 = torch.tensor([[0.707, 0, 0.707, 0],
                   [1, 0, 0, 0]])
q2 = torch.tensor([[0, 0.707, 0.707, 0],
                   [0, 1, 0, 0]])

distance = quaternion_geodesic_distance(q1, q2)
print("Geodesic Distances:", distance)