def convertAxisAngleToQuaternion()

in differentiable_robot_model/se3_so3_util.py [0:0]

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

def convertAxisAngleToQuaternion(axis_angle, epsilon=1.0e-5):
    if not torch.is_tensor(axis_angle):
        axis_angle = torch.Tensor(axis_angle)
    assert axis_angle.shape[0] == 3
    angle = torch.norm(axis_angle)
    if angle > epsilon:
        axis = axis_angle / angle
        quat = axis_angle.new_zeros(4)
        quat[:3] = axis * torch.sin(angle / 2.0)
        quat[3] = torch.cos(angle / 2.0)
    else:
        quat = torch.tensor(
            [0.0, 0.0, 0.0, 1.0], device=axis_angle.device, dtype=axis_angle.dtype
        )
    quat = quat / torch.norm(quat)
    return quat