def getVec3FromSkewSymMat()

in differentiable_robot_model/se3_so3_util.py [0:0]

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

def getVec3FromSkewSymMat(omegahat, epsilon=1.0e-14):
    assert torch.norm(torch.diag(omegahat)) < assert_epsilon, (
        "omegahat = \n%s" % omegahat
    )
    for i in range(3):
        for j in range(i + 1, 3):
            v1 = omegahat[i, j]
            v2 = omegahat[j, i]
            err = torch.abs(v1 + v2)
            assert err < epsilon, "err = %f >= %f = epsilon" % (err, epsilon)
    omega = omegahat.new_zeros(3)
    omega[0] = 0.5 * (omegahat[2, 1] - omegahat[1, 2])
    omega[1] = 0.5 * (omegahat[0, 2] - omegahat[2, 0])
    omega[2] = 0.5 * (omegahat[1, 0] - omegahat[0, 1])
    return omega