def marker_to_robot(self, pose):
    def pose_vector_to_matrix(pose_vec):
        """[X, Y, Z, Rx, Ry, Rz] -> 4x4 变换矩阵"""
        x, y, z, rx, ry, rz = pose_vec
        rotvec = np.array([rx, ry, rz])
        R_mat = R.from_rotvec(rotvec).as_matrix()
        T = np.eye(4)
        T[:3, :3] = R_mat
        T[:3, 3] = [x, y, z]
        return T
    
    def matrix_to_pose_vector(T):
            """4x4 变换矩阵 -> [X, Y, Z, Rx, Ry, Rz]"""
            x, y, z = T[:3, 3]
            rotvec = R.from_matrix(T[:3, :3]).as_rotvec()
            return [x, y, z, rotvec[0], rotvec[1], rotvec[2]]
    
    
    T_marker_world = pose_vector_to_matrix(pose)
    theta2 = (314.5-90) / 360 * 2 * np.pi  #  设置与安装角度匹配
    T = np.array([
        [np.cos(theta2), 0, np.sin(theta2), 0],
        [-np.sin(theta2), 0, np.cos(theta2), 0],
        [0, -1, 0, 0.2],
        [0, 0, 0, 1]
    ])
    T_move = np.array([ # 位移至设置的工作原点
        [1, 0, 0, -0.9],
        [0, 1, 0,    1],
        [0, 0, 1,    0],
        [0, 0, 0,    1]
    ])
    # 转到机器人坐标系
    T_marker_robot = T_move @ T @ T_marker_world  
    return matrix_to_pose_vector(T_marker_robot)
goal = oriented_xyz.get_pose_vector()
goal_marker = goal
goal = self.marker_to_robot(goal)

goal[-3:] = [current_pose[3], current_pose[4] , current_pose[5]] # 设置末端位姿

enable = True
# 前 后 右 左 下 上 
if (not (-0.9 <= goal_marker[0] <= -0.6 and -0.2<= goal_marker[1] <= 0.3 and -0.15 <= goal_marker[2] <= 0.55 )):
    enable = False
#  左 右  仰 俯 前 后
if (not (-1 <= goal_marker[3] <= 1  and 0.5 <= goal_marker[4] <= 3 and -2.6 <= goal_marker[5] <= -1.2 )):
    enable = False

if (self.joy_data.buttons[0]==1 and enable is True):
    ......

goal = oriented_xyz.get_pose_vector()
# 先同步位姿后进行坐标转化
goal[-3:] = [roll, pitch, yaw] 
goal_marker = goal 
goal = self.marker_to_robot(goal)

# 取消原有设置的偏移
def apply_offset(self, pose):
    ......
    pose.orientation.x = rotated[0] # / norm
    pose.orientation.y = rotated[1] # / norm
    pose.orientation.z = rotated[2] # / norm
    pose.orientation.w = rotated[3] # / norm
def callback(self, joy, pose):
    # Publishing the Gripper visualization marker
    
    # Hardcoding the top-down pose of the gripper
    self.ee_marker.pose = self.apply_offset(pose.pose)

if cfg['type'] == 'controller':
	teleop.last_joy = rospy.wait_for_message('/vive_right', Joy)
while not rospy.is_shutdown():
    try:
        if cfg['type'] == 'controller':
            joy = rospy.wait_for_message('/vive_right', Joy)
            pose = rospy.wait_for_message('/right_controller_as_posestamped', PoseStamped)
        else: 
            pose = rospy.wait_for_message('/generic_tracker_0_as_posestamped', PoseStamped)
        teleop.callback(None, pose)

    except KeyboardInterrupt:
        print("Shutting down vive_ur5_teleop")

if ((self.cfg['type'] == 'tracker' or self.joy_data.buttons[0]==1) and enable is True):
    if self.used_movej == 1:
        self.robot.init_realtime_control()
        time.sleep(2)
        self.used_movej = 0
    
    if(self.interpDiff(self.vive_gripper_data) > self.Thres4Interp):
        # Calculate the total number of interpolation steps
        # target_pose = self.vive_gripper_data.pose.position

        # current_pose = self.robot.get_actual_tcp_pose()
        num_steps = int(np.linalg.norm([goal[0], goal[1], goal[2]] - current_pose[:3]) / self.step_size)
        # num_steps = 20
        # print("Current pose: ", current_pose)
        print("Using interpolation | Num steps: ", num_steps)

        # Perform linear interpolation
        interpolated_position = np.linspace(current_pose[:3], [goal[0], goal[1], goal[2]], num_steps)

        for position_intp in interpolated_position:
            # print("position_intp is: ", type(position_intp), " | type goal is: ", type(goal))
            pose_intp = [position_intp , goal[-3:]]
            pose_intp = np.concatenate(pose_intp)
            print("pose_intp",pose_intp)
            self.robot.set_realtime_pose(pose_intp)
            
            self.joint_states_msg.header.stamp = rospy.Time.now() #Testing ONLY
            self.joint_states_msg.position = self.robot.get_actual_joint_positions()
            self.joint_states_pub.publish(self.joint_states_msg)
            
            target_ee_marker.pose.position = self.nd_arr_pose_to_marker(pose_intp)
            # print("Pose from gripper",pose_goal.position.x, pose_goal.position.y, pose_goal.position.z )          
            # print("target_ee_marker.pose: ", target_ee_marker.pose, "\n")
            target_ee_marker.header.stamp = rospy.Time.now()
            # Update the position of target gripper to that of the interpolated pose
            self.target_ee_marker_pub.publish(target_ee_marker)
            self.publish_ee_pose_msg()
            # rospy.sleep(0.05)
            time.sleep(0.10)
        print("\n##########################################################\n")
    
    else:
        self.robot.set_realtime_pose(goal)
        print("Not using interpolation")
        # print("Pose from gripper",pose_goal.position.x, pose_goal.position.y, pose_goal.position.z )          
        # print("target_ee_marker.pose: ", target_ee_marker.pose)
        self.target_ee_marker_pub.publish(target_ee_marker)
        self.publish_ee_pose_msg()
        print("\n##########################################################\n")
    self.used_movej = 0
elif (self.cfg['type'] == 'controller' and self.joy_data.axes[1] > self.tcp_orient_thresh):
    # Rotate the gripper clockwise
    self.used_movej = 1
    q_ee_rotated = self.joint_states_msg.position
    q_ee_rotated[-1] = q_ee_rotated[-1] + math.radians(10)
    self.robot.movej(q=q_ee_rotated, a= self.ACCELERATION_GRIPPER, v= self.VELOCITY_GRIPPER )
    self.publish_ee_pose_msg()
    # robot.init_realtime_control()
elif(self.cfg['type'] == 'controller' and self.cfg['gripper'] == 'true' and self.gripper_opened == 0 and self.joy_data.buttons[4]==1):
    print("Opening the gripper")
    self.gripper_opened = 1
    self.gripper.open(pos=1000)
    time.sleep(1)
elif (self.cfg['type'] == 'controller' and self.cfg['gripper'] == 'true' and self.joy_data.buttons[4]==1 and self.gripper_opened == 1):
    print("Closing the gripper")
    self.gripper_opened = 0
    self.gripper.force_grip(speed=200, force_threshold=600)
    time.sleep(1)

import serial
import time


class Inspire_Gripper:
    def __init__(self, port="/dev/ttyUSB0", baudrate=115200, device_id=1):
        self.ser = serial.Serial(
            port=port,
            baudrate=baudrate,
            bytesize=8,
            parity="N",
            stopbits=1,
            timeout=0.5
        )
        self.device_id = device_id

    def _build_cmd(self, cmd, data: bytes = b""):
        """
        构造完整的指令帧
        帧格式: EB 90 | ID | Len | CMD | Data | Checksum
        """
        frame = bytearray()
        frame.extend([0xEB, 0x90])            # 帧头
        frame.append(self.device_id)          # ID
        frame.append(len(data) + 1)           # Len = CMD + Data
        frame.append(cmd)                     # CMD
        frame.extend(data)                    # Data
        checksum = sum(frame[2:]) & 0xFF      # 校验和(不含帧头)
        frame.append(checksum)
        return bytes(frame)

    def send_cmd(self, cmd, data: bytes = b""):
        """发送命令并读取应答"""
        packet = self._build_cmd(cmd, data)
        self.ser.write(packet)
        resp = self.ser.read(20)  # 最长一帧20字节足够
        return resp

    def open(self, pos=1000):
        """
        张开夹爪到指定开口度 (0~1000, 对应0~70mm)
        默认1000=最大张开
        """
        pos = max(0, min(1000, pos))
        data = pos.to_bytes(2, "little")   # 低字节在前
        resp = self.send_cmd(0x54, data)   # CMD_MC_SEEKPOS
        return resp

    def close(self):
        """闭合夹爪 (开口度=0)"""
        return self.open(0)

    def read_position(self):
        """读取当前开口度"""
        resp = self.send_cmd(0xD9)
        print("开口度:", resp.hex(" ").upper())
        return resp

    def stop(self):
        """急停"""
        resp = self.send_cmd(0x16)
        print("急停:", resp.hex(" ").upper())
        return resp


# if __name__ == "__main__":
#     gripper = Inspire_Gripper("/dev/ttyUSB0", 115200, device_id=1)
#     # 示例操作流程
#     gripper.open(1000)       # 打开到最大
#     time.sleep(2)
#     gripper.force_grip(speed=500, force_threshold=200)  # 力控夹取