def convertQuaternionToAxisAngle()

in differentiable_robot_model/se3_so3_util.py [0:0]

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

def convertQuaternionToAxisAngle(quat, alpha=0.05, epsilon=1.0e-15):
    if not torch.is_tensor(quat):
        quat = torch.Tensor(quat)
    assert quat.shape[0] == 4
    assert (torch.norm(quat) > 1.0 - alpha) and (torch.norm(quat) < 1.0 + alpha)
    quat = quat / torch.norm(quat)
    angle = 2.0 * torch.acos(quat[3])
    axis = quat[:3] / (torch.sin(angle / 2.0) + epsilon)
    axis_angle = axis * angle
    return axis_angle