import numpy as np
from frankapy import FrankaArm

class ControlUtil:
    
    __fa = FrankaArm(robot_num=2)
    
    DISPLAYTABLE_TO_BASE:np.ndarray = np.asarray([
        [1, 0, 0, 0],
        [0, 1, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
        ])
    
    GRIPPER_TO_CAMERA:np.ndarray = np.asarray([
        [1, 0, 0, 0],
        [0, 1, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
        ])
    
    @staticmethod
    def get_franka_arm() -> FrankaArm:
        return ControlUtil.__fa
    
    @staticmethod
    def get_pose() -> np.ndarray:
        return ControlUtil.__fa.get_pose().matrix
    
    @staticmethod
    def reset() -> None:
        ControlUtil.__fa.reset_joints()
        
# ----------- Debug Test -------------

if __name__ == "__main__":
    print(ControlUtil.get_pose())
    ControlUtil.reset()
    print(ControlUtil.get_pose())