def expMapse3()

in differentiable_robot_model/se3_so3_util.py [0:0]

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

def expMapse3(kseehat, epsilon=1.0e-14):
    assert kseehat.shape[0] == 4
    assert kseehat.shape[1] == 4
    assert torch.norm(kseehat[3, :]) < assert_epsilon
    omegahat = kseehat[:3, :3]
    exp_omegahat = expMapso3(omegahat, epsilon)

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

    A = (
        torch.eye(3, device=kseehat.device, dtype=kseehat.dtype)
        + (((1.0 - torch.cos(norm_omega)) / (norm_omega + epsilon) ** 2) * omegahat)
        + (
            ((norm_omega - torch.sin(norm_omega)) / ((norm_omega + epsilon) ** 3))
            * torch.matmul(omegahat, omegahat)
        )
    )
    v = kseehat[:3, 3]
    exp_kseehat = torch.eye(4, device=kseehat.device, dtype=kseehat.dtype)
    exp_kseehat[:3, :3] = exp_omegahat
    exp_kseehat[:3, 3] = torch.matmul(A, v)
    return exp_kseehat