Isaac relative transform

relative.py

# AprilTag Plane -> XformFix: 180,0,0

import omni
from pxr import Usd, UsdGeom, Gf
import numpy as np
import transforms3d as t3d


def print_xyzq(tf):
    q = t3d.quaternions.mat2quat(tf[:3,:3])
    print(f'{tf[0,3]:.3f},{tf[1,3]:.3f},{tf[2,3]:.3f},{q[1]:.3f},{q[2]:.3f},{q[3]:.3f},{q[0]:.3f}')


def get_tf(primPath):
    stage = omni.usd.get_context().get_stage()
    prim = stage.GetPrimAtPath(primPath)
    tf_isaac = omni.usd.get_world_transform_matrix(prim, Usd.TimeCode.Default()).GetOrthonormalized()
    tf = np.array(tf_isaac).astype(np.float32)
    return tf.transpose()


to_tf = get_tf('/World/pallet_id105/Plane_01/Xform')
from_tf = get_tf('/World/ur5e/tool0')

res_tf = np.linalg.inv(from_tf) @ to_tf
print_xyzq(res_tf)