一、简介

二、操作流程

三、常见问题

四、代码和附件

1. cs.DashboardClientInterface().connect(ip, 29999) 连 Dashboard
2. cs.PrimaryClientInterface().connect(ip, 30001) 连 Primary
3. cs.RtsiIOInterface(output_recipe, input_recipe, 100.0).connect(ip) 连 RTSI，100Hz
4. cs.EliteDriver(config) 构造伺服驱动（headless=True）
5. lib.hfdOpen() → hfdInit() → hfdCalibrateDevice() → hfdEnableForce() → hfdEnableDevice() → hfdSetGravityCompensation() 初始化 HFD-6 力反馈摇杆
6. hfdGetButton() 轮询按钮，检测 0→1 上升沿
7. 按下：记录 hfdGetPosition() / hfdGetOrientationRad() 作为原点

• io.getActualTCPPose() 读当前 TCP 位姿 → calculate_xyz / calculate_rxryrz 算基准位姿 V5/V51
• driver.sendExternalControlScript() → driver.isRobotConnected() 轮询等就绪

8. 运行中循环：

• hfdGetPosition() / hfdGetOrientationRad() 读摇杆 → 减去原点得偏移量 pose
• poseMul / userFrameToTcpFrame 做用户坐标系变换，算目标 TCP 位姿
• driver.writeServoj(target_pose, timeout_ms=100, cartesian=True) 笛卡尔伺服下发
• 周期 0.004s（约 250Hz）

9. 再按按钮：driver.writeIdle(timeout_ms=500) → Key_combination_off() 关摇杆力反馈 → 回到待机
10. io.disconnect() / primary.disconnect() / dashboard.disconnect() / driver.stopControl() 清理

HFD-6 摇杆 ──→ [位置/姿态] ──→ 减原点得偏移量
                                    │
                    ┌───────────────┘
                    ▼
             坐标变换链（用户坐标系 → 基座标）
                    │
                    ▼
             writeServoj(cartesian=True) ──→ 机器人实时跟随
                    │
     RTSI ←── getActualTCPPose 实时反馈

# -*- coding: utf-8 -*-
"""
本案例使用序力智能 HFD-6 力反馈设备进行机器人控制

依赖：
    elite-cs-sdk

需要的DLL和配方文件作为附件置于文末，运行时需要放置在同目录下
依赖文件（与本脚本同目录）：
    output_recipe.txt —— 至少包含 actual_joint_positions
    input_recipe.txt —— 标准输入配方（可为默认或自定义）
    HFD_API64.DLL —— 力反馈设备动态链接库
"""

from __future__ import annotations
import ctypes
import time
import os
import elite_cs_sdk as cs


# ============================================================================
# 单位换算、坐标系变换函数
# ============================================================================
import numpy as np
import transformations
from transformations.transformations import euler_from_matrix, inverse_matrix
import copy

def radToDeg(pose:list)->list:
    """
    弧度->角度
    """
    return [pose[0],pose[1],pose[2],pose[3]*57.29578,pose[4]*57.29578,pose[5]*57.29578]

def degToRad(poseList:list)->list:
    """
    角度->弧度
    :return: asd
    """
    poseList[3]/=57.29578
    poseList[4]/=57.29578
    poseList[5]/=57.29578
    return poseList

def mmDegToMRad(pose:list):
    """
    单位换算 mm,deg到m,rad
    :param pose:位姿
    :return:
    """
    nn = copy.deepcopy(pose)
    nn[0] /= 1000
    nn[1] /= 1000
    nn[2] /= 1000
    nn[3] /= 57.2957
    nn[4] /= 57.2957
    nn[5] /= 57.2957
    return nn

def mRadToMmDeg(pose:list):
    """
    单位换算 m rad 转 mm deg
    :param pose: 位姿
    :return: 位姿
    """
    nn = copy.deepcopy(pose)
    nn[0] *= 1000
    nn[1] *= 1000
    nn[2] *= 1000
    nn[3] *= 57.2957
    nn[4] *= 57.2957
    nn[5] *= 57.2957
    return nn

# 将基坐标系下数据转换至用户坐标系的函数
def BaseposeToUserpose(user,basepose):
    #mm °
    return poseMul(poseInv(user),basepose)

def userFrameToTcpFrame(userPose:list,target:list)->list:
    """
    用户坐标转基座标
    :param userPose 用户坐标数据 单位m,rad
    :param target: 用户坐标 单位m,rad
    :return: 基座标 单位m,rad
    """
    out = poseMul(userPose, target)
    return out

def eulerToMatrix(pose:list)->list:
    """
    欧拉角转齐次变换矩阵
    :return: 变化矩阵
    """
    m = transformations.euler_matrix(pose[3], pose[4], pose[5])
    m[0][3] = pose[0]
    m[1][3] = pose[1]
    m[2][3] = pose[2]
    # print(m)
    return m

def matrixToEuler(mat:list)->list:
    """
    齐次变换矩阵转欧拉角
    :param mat: 齐次变换矩阵
    :return: Pose
    """
    x, y, z = mat[0][3], mat[1][3], mat[2][3]
    mat[0][3] = 0
    mat[1][3] = 0
    mat[2][3] = 0
    # 旋转矩阵到弧度的欧拉角
    rx, ry, rz = euler_from_matrix(mat)
    # 由于使用了numpy导致数据类型出现问题，json解析出bug
    return [float(x), float(y), float(z), float(rx), float(ry), float(rz)]

def matMul(pose1:list,pose2:list)->list:
    """
    矩阵的乘
    :param pose1: 变换矩阵1
    :param pose2 变换矩阵2
    :return: 矩阵
    """
    return list(np.matmul(pose1, pose2))

def matInv(pose:list)->list:
    """
    矩阵的逆
    :return: 矩阵
    """
    return inverse_matrix(eulerToMatrix(pose))

def poseInv(pose:list)->list:
    """
    位姿的逆
    :return: 求逆后的欧拉角
    """
    return matrixToEuler(matInv(pose))

def poseMul(pose1:list, pose2:list)->list:
    """
    位姿的乘
    :param pose1: 位姿1
    :param pose2 位姿2
    :return: 相乘后的位姿
    """
    pose = matrixToEuler(matMul(eulerToMatrix(pose1), eulerToMatrix(pose2)))
    return pose



# ============================================================================
# elite-cs-sdk 资源路径
# ============================================================================
SDK_DIR = os.path.dirname(cs.__file__)
SCRIPT_PATH = os.path.join(SDK_DIR, "external_control.script")
OUTPUT_RECIPE = "./output_recipe.txt"
INPUT_RECIPE  = "./input_recipe.txt"

# 机器人 IP（与原版一致）
ROBOT_IP = "172.16.15.114"

# ============================================================================
# HFD 摇杆 DLL
# ============================================================================
lib = ctypes.cdll.LoadLibrary('./HFD_API64.dll')

def hfdOpen():
    return lib.hfdOpen()

def hfdInit():
    lib.hfdInit(-1)
    time.sleep(0.05)

def hfdCalibrateDevice():
    lib.hfdCalibrateDevice(-1)
    time.sleep(0.05)

def hfdGetPosition():
    float_x = ctypes.pointer(ctypes.c_double())
    float_y = ctypes.pointer(ctypes.c_double())
    float_z = ctypes.pointer(ctypes.c_double())
    lib.hfdGetPosition(float_x, float_y, float_z, -1)
    return [float_x.contents.value, float_y.contents.value, float_z.contents.value]

def hfdGetOrientationRad():
    float_x = ctypes.pointer(ctypes.c_double())
    float_y = ctypes.pointer(ctypes.c_double())
    float_z = ctypes.pointer(ctypes.c_double())
    lib.hfdGetOrientationRad(float_x, float_y, float_z, -1)
    return [float_x.contents.value, float_y.contents.value, float_z.contents.value]

def hfdEnableForce():
    lib.hfdEnableForce(True, -1)
    time.sleep(0.05)

def hfdSetForce(X, Y, Z):
    lib.hfdSetForce(ctypes.c_double(X), ctypes.c_double(Y), ctypes.c_double(Z), -1)

def hfdEnableExpertMode():
    lib.hfdEnableExpertMode()
    time.sleep(0.05)

def hfdEnableDevice():
    lib.hfdEnableDevice(True, -1)
    time.sleep(0.05)

def hfdDisableExpertMode():
    lib.hfdDisableExpertMode()
    time.sleep(0.05)

def hfdSetGravityCompensation():
    lib.hfdSetGravityCompensation(True, -1)
    time.sleep(0.05)

def hfdGetButton():
    return lib.hfdGetButton(0, -1)

def Key_combination_on():
    hfdEnableExpertMode()
    lib.hfdEnableDevice(True, -1)
    hfdDisableExpertMode()
    lib.hfdSetGravityCompensation(True, -1)

def Key_combination_off():
    lib.hfdSetGravityCompensation(False, -1)
    hfdEnableExpertMode()
    lib.hfdEnableDevice(False, -1)
    hfdDisableExpertMode()

def Key_combination():
    a1 = hfdOpen()
    hfdInit()
    hfdCalibrateDevice()
    hfdEnableForce()
    hfdEnableExpertMode()
    hfdEnableDevice()
    hfdDisableExpertMode()
    hfdSetGravityCompensation()
    return a1

# ============================================================================
# 40011 明文请求
# ============================================================================
import socket
import ast

def _send_40011(cmd_id: int, statement: str) -> str:
    """向 40011 端口发送请求，返回原始响应字符串"""
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.settimeout(5.0)
    sock.connect((ROBOT_IP, 40011))
    request = f"req {cmd_id} {statement}\n"
    print(request)
    sock.sendall(bytes(str(request), "utf-8"))
    response = sock.recv(1024).decode('utf-8')
    sock.close()
    return response

def _parse_response(resp: str):
    """解析 40011 返回值"""
    resp = resp.strip()
    if '[failure]' in resp:
        return None, resp
    parts = resp.split(' : ', 1)
    if len(parts) == 2:
        val = ast.literal_eval(parts[1].strip())
        # 返回值是双层列表 [[...]]
        if isinstance(val, list) and len(val) == 1:
            return val[0], None
        return val, None
    return None, resp

def Status_request(content: str):
    """
    替代原版 Status_request() —— 发送脚本语句到 40011，
    返回 6 轴 list[float]（位置 m，旋转向量 rad）。
    content 形如："get_actual_tcp_pose()" 或 "get_inverse_kin(...)"
    """
    resp = _send_40011(1, content)
    return _parse_response(resp)

# ============================================================================
# SDK 连接封装
# ============================================================================
class RobotSDK:
    def __init__(self, ip: str):
        self.ip = ip

        # Dashboard — 机器人控制命令（替代 29999 端口）
        self.dashboard = cs.DashboardClientInterface()
        self.dashboard.connect(ip, 29999)

        # Primary — 发送脚本（替代 30001 端口）
        self.primary = cs.PrimaryClientInterface()
        self.primary.connect(ip, 30001)

        # RTSI — 实时 IO / 状态读取（替代 40011 查询）
        self.io = cs.RtsiIOInterface(OUTPUT_RECIPE, INPUT_RECIPE, 100.0)
        self.io.connect(ip)

        # EliteDriver — 实时伺服控制（替代 30020 透传）
        config = cs.EliteDriverConfig()
        config.robot_ip = ip
        config.script_file_path = SCRIPT_PATH
        config.headless_mode = True
        self.driver = cs.EliteDriver(config)

    def disconnect(self):
        self.io.disconnect()
        self.primary.disconnect()
        self.dashboard.disconnect()
        self.driver.stopControl(wait_ms=5000)

    # ---- 30001 脚本（speedl / stopl） ----------------------------------
    def send_script(self, script: str):
        """通过 30001 发送 Elite Script（无返回值）"""
        self.primary.sendScript(script)

    # ---- 40011 明文请求 -----------------------------
    def status_request(self, content: str):
        """通过 40011 发送请求，返回 list[float]"""
        return Status_request(content)

    # ---- RTSI 实时数据 --------------------------------------------------
    def get_actual_tcp_pose(self) -> list:
        """通过 RTSI（30004）获取当前 TCP 位姿 [x,y,z,rx,ry,rz]，单位 m / rad"""
        return self.io.getActualTCPPose()

    def get_actual_joint_positions(self) -> list:
        return self.io.getActualJointPositions()

    def get_speed_scaling(self) -> float:
        """通过 RTSI 获取当前速度百分比（如 0.5 = 50%）"""
        return self.io.getActualSpeedScaling()

    # ---- Dashboard ------------------------------------------------------
    def remote_control_on(self):
        """确保处于远程模式"""
        while True:
            Data = self.dashboard.sendAndReceive("remoteControl -s")
            if 'REMOTE' in Data:
                print('当前处于远程模式')
                break
            else:
                print('当前处于：' + Data)
                self.dashboard.sendAndReceive("remoteControl -on")
            time.sleep(1)

    def power_on(self):
        Data = self.dashboard.sendAndReceive("robotControl -on")
        print(Data)
        if Data == 'Powering on':
            print('上电成功')
        time.sleep(1)

    def brake_release(self):
        while True:
            Data = self.dashboard.sendAndReceive("brakeRelease")
            if 'Brake is released' in Data:
                print('抱闸释放成功')
                break
            else:
                print(Data)
            time.sleep(0.5)

    # ---- 30020 透传 → EliteDriver writeServoj ------------------------
    def servoj(self, q: list, t: float = 0.018,
               lookahead_time: float = 0.20, gain: int = 300):
        """
        替代 interpreter("servoj(...)") —— 直接用 EliteDriver 实时伺服。
        q: 6 关节位置 [j1~j6]，单位 rad
        t: 伺服周期（秒），原版 0.018
        """
        self.driver.writeServoj(q, timeout_ms=0, cartesian=False)

# ============================================================================
# 沿坐标系运动
# ============================================================================
def speedl(content: str):
    """通过 30001 发送 speedl 脚本"""
    trans_speed_map = {
        'x+':  '0.09',  'x-': '-0.09',
        'y+':  '0.09',  'y-': '-0.09',
        'z+':  '0.09',  'z-': '-0.09',
        'Rx+': '0',     'Rx-': '0',
        'Ry+': '0',    'Ry-': '0',
        'Rz+': '0',    'Rz-': '0',
    }
    rotate_speed_map = {
        'x+': '0', 'x-': '0',
        'y+': '0', 'y-': '0',
        'z+': '0', 'z-': '0',
        'Rx+': '0.36',  'Rx-': '-0.36',
        'Ry+': '0.36',  'Ry-': '-0.36',
        'Rz+': '0.36',  'Rz-': '-0.36',
    }
    axis_dir_map = {
        'x+': 'x_dir[0]', 'x-': 'x_dir[0]',
        'y+': 'y_dir[0]', 'y-': 'y_dir[0]',
        'z+': 'z_dir[0]', 'z-': 'z_dir[0]',
        'Rx+': 'x_dir[0]', 'Rx-': 'x_dir[0]',
        'Ry+': 'y_dir[0]', 'Ry-': 'y_dir[0]',
        'Rz+': 'z_dir[0]', 'Rz-': 'z_dir[0]',
    }

    trans_speed  = trans_speed_map.get(content, '0')
    rotate_speed = rotate_speed_map.get(content, '0')
    p0 = axis_dir_map.get(content, 'x_dir[0]')
    p1 = axis_dir_map.get(content, 'x_dir[1]')
    p2 = axis_dir_map.get(content, 'x_dir[2]')

    script = (
        f"   trans_speed = {trans_speed}\n"
        f"   rotate_speed = {rotate_speed}\n"
        "   user_frame = [0,0,0,0,0,0]\n"
        "   cos0 = cos(user_frame[3])\n"
        "   cos1 = cos(user_frame[4])\n"
        "   cos2 = cos(user_frame[5])\n"
        "   sin0 = sin(user_frame[3])\n"
        "   sin1 = sin(user_frame[4])\n"
        "   sin2 = sin(user_frame[5])\n"
        "   x_dir = [1.0, 0.0, 0.0]\n"
        "   y_dir = [0.0, 1.0, 0.0]\n"
        "   z_dir = [0.0, 0.0, 1.0]\n"
        "   x_dir[0] = cos2 * cos1\n"
        "   x_dir[1] = sin2 * cos1\n"
        "   x_dir[2] = -sin1\n"
        "   y_dir[0] = cos2 * sin1 * sin0 - sin2 * cos0\n"
        "   y_dir[1] = sin2 * sin1 * sin0 + cos2 * cos0\n"
        "   y_dir[2] = cos1 * sin0\n"
        "   z_dir[0] = cos2 * sin1 * cos0 + sin2 * sin0\n"
        "   z_dir[1] = sin2 * sin1 * cos0 - cos2 * sin0\n"
        "   z_dir[2] = cos1 * cos0\n"
        f"   speedl([{p0}* trans_speed,{p1}* trans_speed,{p2}* trans_speed,"
        f"{p0}* rotate_speed,{p1}* rotate_speed,{p2}* rotate_speed], 0.6, 100)"
    )
    return script

# ============================================================================
# 计算函数
# ============================================================================
# 注意：mmDegToMRad 期望输入 mm+deg。
# 40011 / getActualTCPPose 返回 m+rad，需要先 mRadToMmDeg 转成 mm+deg。

def calculate_xyz(tcp, pose):
    V2 = poseInv(mmDegToMRad([-pose[0], pose[2], pose[1], 0, 0, 0]))

    print("tcp:"+ str(tcp))
    V1 = BaseposeToUserpose(
        mmDegToMRad([-340.1, -171.1, 311.4, 0, 0, 0]),
        tcp
    )
    V4 = poseMul(V2, [V1[0], V1[1], V1[2], 0, 0, 0])
    return V4

def calculate_rxryrz(tcp, pose):
    V21 = poseInv([0, 0, 0, pose[3], pose[4], pose[5]])
    V1 = BaseposeToUserpose(
        mmDegToMRad([-340.1, -171.1, 311.4, 0, 0, 0]),
        tcp
    )
    V41 = poseMul(V21, [0, 0, 0, V1[3], V1[4], V1[5]])
    return V41

# ============================================================================
# 主程序
# ============================================================================

if __name__ == "__main__":
    # ---- SDK 连接 -------------------------------------------------------
    robot = RobotSDK(ROBOT_IP)

    # ---- 摇杆初始化 ------------------------------------------------------
    a1 = Key_combination()

    # ---- 按钮边沿触发标志 ------------------------------------------------
    running = False          # 当前是否处于力控运动状态
    last_button = hfdGetButton()  # 记录上一帧按钮状态（0 或 1）

    # ---- 主循环 ---------------------------------------------------------
    while True:
        if a1 == 0:
            print("摇杆初始化异常")
            break

        current_button = hfdGetButton()
        # 检测按下边沿（0→1）
        button_pressed = (current_button == 1 and last_button == 0)
        last_button = current_button
        if button_pressed:
            if not running:
                # ---- 记录按下时摇杆的初始位置/姿态作为原点 ----
                init_pos = hfdGetPosition()
                init_ori = hfdGetOrientationRad()

                # ---- 透传初始化：计算基准位姿（此时偏移量为 0） ----
                pose = [0, 0, 0, 0, 0, 0]
                tcp = robot.io.getActualTCPPose()
                V5 = calculate_xyz(tcp, pose)
                V51 = calculate_rxryrz(tcp, pose)

                # ---- 通过 EliteDriver 建立外部控制连接 ----
                robot.driver.sendExternalControlScript()
                _t0 = time.time()
                while not robot.driver.isRobotConnected():
                    if time.time() - _t0 > 15:
                        raise RuntimeError("EliteDriver 连接超时（15s）")
                    time.sleep(0.1)
                print("EliteDriver 已连接，开始运动")
                running = True

            else:
                # ---- 停止 ----
                robot.driver.writeIdle(timeout_ms=500)
                Key_combination_off()
                print("结束运动")
                time.sleep(1)
                Key_combination_on()
                running = False

        # ---- 运行中：实时控制 ----
        if running:
            # 获取摇杆值，减去按下时的初始值，得到相对偏移量
            cur_pos = hfdGetPosition()
            cur_ori = hfdGetOrientationRad()
            pose = [0, 0, 0, 0, 0, 0]
            pose[2], pose[1], pose[0] = (
                cur_pos[2] - init_pos[2],
                cur_pos[1] - init_pos[1],
                cur_pos[0] - init_pos[0],
            )  # X/Y 对调，相对偏移
            pose[3] = cur_ori[0] - init_ori[0]
            pose[4] = cur_ori[1] - init_ori[1]
            pose[5] = cur_ori[2] - init_ori[2]
            # ---- 平移：末端位姿计算 ----
            V6 = poseMul(
                mmDegToMRad([pose[0], pose[2], pose[1], 0, 0, 0]), V5)
            result2 = userFrameToTcpFrame(
                mmDegToMRad([-340.1, -171.1, 311.4, 0, 0, 0]), V6)

            # ---- 旋转：末端位姿计算 ----
            V61 = poseMul([0, 0, 0, pose[3], pose[4], pose[5]], V51)
            result21 = userFrameToTcpFrame(
                mmDegToMRad([-340.1, -171.1, 311.4, 0, 0, 0]), V61)

            # ---- 位姿 ----
            target_pose = [
                round(result2[0],  5),
                round(result2[1],  5),
                round(result2[2],  5),
                3.14,
                0,
                round(result21[5], 5),
            ]

            # ---- 伺服控制 ----
            print(target_pose)
            robot.driver.writeServoj(target_pose, timeout_ms=100, cartesian=True)

        time.sleep(0.004)

    # ---- 清理 -----------------------------------------------------------
    robot.disconnect()