def logMapSE3()

in differentiable_robot_model/se3_so3_util.py [0:0]

• 28 lines of code
• 1 McCabe index (conditional complexity)

def logMapSE3(T, epsilon=1.0e-14):
    assert T.shape[0] == 4
    assert T.shape[1] == 4
    assert torch.norm(T[3, :3]) < assert_epsilon
    assert torch.abs(T[3, 3] - 1.0) < assert_epsilon
    R = T[:3, :3]
    omegahat = logMapSO3(R, epsilon)

    omega = getVec3FromSkewSymMat(omegahat, epsilon)
    norm_omega = torch.norm(omega)

    Ainv = (
        torch.eye(3, device=T.device, dtype=T.dtype)
        - (0.5 * omegahat)
        + (
            (
                (
                    (2.0 * torch.sin(norm_omega))
                    - (norm_omega * (1.0 + torch.cos(norm_omega)))
                )
                / ((2 * (norm_omega ** 2) * torch.sin(norm_omega)) + epsilon)
            )
            * torch.matmul(omegahat, omegahat)
        )
    )
    p = T[:3, 3]
    kseehat = T.new_zeros((4, 4))
    kseehat[:3, :3] = omegahat
    kseehat[:3, 3] = torch.matmul(Ainv, p)
    return kseehat