def logMapSO3()

in differentiable_robot_model/se3_so3_util.py [0:0]

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

def logMapSO3(R, epsilon=1.0e-14):
    assert R.shape[0] == 3
    assert R.shape[1] == 3
    assert (
        torch.abs(torch.abs(torch.det(R)) - 1.0) < assert_epsilon
    ), "det(R) = %f" % torch.det(R)
    half_traceR_minus_one = (torch.trace(R) - 1.0) / 2.0
    if half_traceR_minus_one < -R.new_ones(1):
        print("Warning: half_traceR_minus_one = %f < -1.0" % half_traceR_minus_one)
        half_traceR_minus_one = -R.new_ones(1)
    if half_traceR_minus_one > 1.0:
        print("Warning: half_traceR_minus_one = %f > 1.0" % half_traceR_minus_one)
        half_traceR_minus_one = R.new_ones(1)

    theta = torch.acos(half_traceR_minus_one)
    omegahat = (R - R.T) / ((2.0 * torch.sin(theta)) + epsilon)
    return theta * omegahat