import json
from os import PathLike
from pathlib import Path

from scipy.spatial.transform import RigidTransform, Rotation
import numpy as np


def relative_transform(world_to_a: RigidTransform, world_to_b: RigidTransform) -> RigidTransform:
    """ Computes the relative transform between two poses represented by a transform from the same 
        world coordinates

    Args:
        world_to_a (RigidTransform): transform from world origin to the first pose
        world_to_b (RigidTransform): transform from world origin to the second pose

    Returns:
        RigidTransform: transform to second pose from first pose b_to_a
    """

    Twa = world_to_a.as_matrix()
    Tbw = np.linalg.inv(world_to_b.as_matrix())
    Tab = np.linalg.matmul(Tbw,Twa)

    return RigidTransform.from_matrix(Tab)