# 用户自定义指令插件

**版本 V1.4 | 更新日期：2026年3月25日**

---

## Language / 语言

- [English](README.en.md) | [中文简体](README.md)

---

## 概述

**CM（Custom Module）**是一个用户自定义指令插件，为捷勃特机器人提供了一系列实用的自定义指令。

---

## 版本说明

本插件提供两个版本，分别对应不同的SDK版本：

### SDK v1.7.1.3 版本
- **目录：** `CoordinateModifier（SDKV1.7.1.3）/`
- **主文件：** `CM_oldsdk.py`
- **兼容SDK：** Python SDK v1.7.1.3
- **兼容机器人软件版本：**
  - Copper v7.6.X.X
  - Bronze v7.6.X.X

### SDK v2.0.0.0 版本
- **目录：** `CoordinateModifier（SDKV2.0.0.0）/`
- **主文件：** `CM.py`
- **兼容SDK：** Python SDK v2.0.0.0
- **兼容机器人软件版本：**
  - Copper v7.7.X.X
  - Bronze v7.7.X.X

### 版本选择建议

- **使用 SDK v1.7.1.3 版本：** 如果您的机器人软件版本为 v7.6.X.X
- **使用 SDK v2.0.0.0 版本：** 如果您的机器人软件版本为 v7.7.X.X 或更高

**注意：** 请根据您的机器人软件版本选择对应的插件版本，确保SDK版本与机器人软件版本兼容。

---

## 打包说明

插件开发完毕后，需要使用捷勃特插件打包工具进行打包。详细的打包与安装说明请参考：

[📦 打包与安装文档](https://dev.sh-agilebot.com/docs/extension/zh/02-development/04-package.html)

---

## 功能列表

插件提供以下13个自定义指令：

1. **SetTF** - 设置工具坐标系参数（直接数值）
2. **SetUF** - 设置用户坐标系参数（直接数值）
3. **SetTF_R** - 从R寄存器读取值设置工具坐标系参数
4. **SetUF_R** - 从R寄存器读取值设置用户坐标系参数
5. **SetTF_PR** - 从PR寄存器读取完整位姿设置工具坐标系
6. **SetUF_PR** - 从PR寄存器读取完整位姿设置用户坐标系
7. **Incr** - R寄存器自增
8. **Decr** - R寄存器自减
9. **Strp** - 拆解字符串数据到PR寄存器
10. **TFShift** - 工具坐标系补正（基于视觉反馈）
11. **DecToHex** - 从十进制转换为十六进制
12. **TurnCountToR** - 将PR位姿回转数写入R寄存器（单轴）
13. **RToTurnCount** - 将R寄存器回写到PR位姿回转数（单轴）

---

## 指令详细说明

### 1. SetTF - 设置工具坐标系参数（直接数值）

直接通过数值设置工具坐标系的指定参数。

**参数：**
- `ID` (int): 工具坐标系ID号（1-30）
- `Pos` (int): 位置参数编号（1-6）
  - 1: X坐标（单位：mm）
  - 2: Y坐标（单位：mm）
  - 3: Z坐标（单位：mm）
  - 4: A角度（单位：度）
  - 5: B角度（单位：度）
  - 6: C角度（单位：度）
- `Value` (float): 参数值

**示例：**
```
CALL_SERVICE CM, Set TF, ID=1, Pos=1, Value=111.11
```

---

### 2. SetUF - 设置用户坐标系参数（直接数值）

直接通过数值设置用户坐标系的指定参数。

**参数：**
- `ID` (int): 用户坐标系ID号（1-30）
- `Pos` (int): 位置参数编号（1-6）
- `Value` (float): 参数值

**示例：**
```
CALL_SERVICE CM, SetUF, ID=1, Pos=1, Value=222.354
```

---

### 3. SetTF_R - 从R寄存器读取值设置工具坐标系参数

从指定的R寄存器读取数值，并设置到工具坐标系的指定参数。

**参数：**
- `ID` (int): 工具坐标系ID号（1-30）
- `Pos` (int): 位置参数编号（1-6）
- `R_ID` (int): R寄存器编号

**示例：**
```
CALL_SERVICE CM, Set TF_R, ID=3, Pos=1, R_ID=1
```

---

### 4. SetUF_R - 从R寄存器读取值设置用户坐标系参数

从指定的R寄存器读取数值，并设置到用户坐标系的指定参数。

**参数：**
- `ID` (int): 用户坐标系ID号（1-30）
- `Pos` (int): 位置参数编号（1-6）
- `R_ID` (int): R寄存器编号

**示例：**
```
CALL_SERVICE CM, SetUF_R, ID=3, Pos=1, R_ID=1
```

---

### 5. SetTF_PR - 从PR寄存器读取完整位姿设置工具坐标系

从指定的PR寄存器读取完整的位姿信息（X、Y、Z、A、B、C），并一次性设置到工具坐标系。

**参数：**
- `ID` (int): 工具坐标系ID号（1-30）
- `PR_ID` (int): PR寄存器编号

**示例：**
```
CALL_SERVICE CM, Set TF_PR, ID=2, PR_ID=1
```

---

### 6. SetUF_PR - 从PR寄存器读取完整位姿设置用户坐标系

从指定的PR寄存器读取完整的位姿信息，并一次性设置到用户坐标系。

**参数：**
- `ID` (int): 用户坐标系ID号（1-30）
- `PR_ID` (int): PR寄存器编号

**示例：**
```
CALL_SERVICE CM, SetUF_PR, ID=2, PR_ID=2
```

---

### 7. Incr - R寄存器自增

将指定R寄存器的值增加指定的步长。

**参数：**
- `R_ID` (int): R寄存器编号
- `Step` (float): 自增步长，默认为1.0

**示例：**
```
CALL_SERVICE CM, Incr, R_ID=2, Step=1
```

---

### 8. Decr - R寄存器自减

将指定R寄存器的值减少指定的步长。

**参数：**
- `R_ID` (int): R寄存器编号
- `Step` (float): 自减步长，默认为1.0

**示例：**
```
CALL_SERVICE CM, Decr, R_ID=1, Step=1
```

---

### 9. Strp - 拆解字符串数据到PR寄存器

从SR寄存器读取字符串数据，拆解后写入PR寄存器。

**参数：**
- `SR_ID` (int): 字符串寄存器编号
- `R_ID_Status` (int): R寄存器编号，用于输出物料检测状态（1=有物料，0=无物料）
- `PR_ID` (int): PR寄存器起始编号
- `R_ID_Error` (int): R寄存器编号，用于输出错误状态码（0=正确，1=错误）

**数据格式：**
- SR寄存器格式：状态位,数据1,数据2,数据3,...
- 状态位：0=无物料，1=有物料
- 数据映射关系：数据1→PR的X坐标，数据2→PR的Y坐标，数据3→PR的C角度
- 支持分隔符：逗号、分号、竖线、制表符、空格等（自动检测）
- 每个PR寄存器存储6个分量（X,Y,Z,A,B,C），其中Z、A、B保留原值不变

**状态码说明：**
- `R_ID_Status`：物料检测状态（1=有物料，0=无物料）
- `R_ID_Error`：错误状态码（0=正确，1=错误）
  - 状态位=0（无物料）→ R_ID_Status=0, R_ID_Error=1（错误）
  - 状态位=1且数据格式正确 → R_ID_Status=1, R_ID_Error=0（正确）
  - 状态位=1但数据格式错误 → R_ID_Status=1, R_ID_Error=1（格式错误）

**示例：**
```
SR[1] = "1,100.5,200.3,45.0"
// 说明：状态位=1（有物料），数据1=100.5（X坐标），数据2=200.3（Y坐标），数据3=45.0（C角度）
// 执行后：PR[1].x=100.5, PR[1].y=200.3, PR[1].c=45.0（Z、A、B保留原值）
CALL_SERVICE CM, Strp, SR_ID=1, R_ID_Status=1, PR_ID=1, R_ID_Error=2
```

---

### 10. TFShift - 工具坐标系补正（基于视觉反馈Eye-to-Hand）

通过读取不同的视觉目标点偏差与基准视觉位置的偏差，来计算工具坐标系的相对偏差，从而将偏差输出在工具坐标系中。

**参数：**
- `InputTF_ID` (int): 基准标定坐标系编号（1-30），默认1
- `ResultTF_ID` (int): 最终算法计算后写入的坐标系编号（1-30），默认3
- `CamPose_ID` (int): 拍照点PR寄存器编号，默认60
- `RefVis_ID` (int): 基准视觉模板数据PR寄存器编号，默认61（需要手动写入）
- `ActVis_ID` (int): 视觉输出的实际坐标数据PR寄存器编号，默认62（需要手动写入）

**示例：**
```
// 假设：
// - TF[1] 为基准标定坐标系
// - PR[60] 存储拍照点位姿
// - PR[61] 存储基准视觉模板数据（需要手动写入）
// - PR[62] 存储实际视觉坐标数据（需要手动写入）
// - TF[3] 为计算结果输出坐标系

CALL_SERVICE CM, TFShift, InputTF_ID=1, ResultTF_ID=3, CamPose_ID=60, RefVis_ID=61, ActVis_ID=62
```

**工作原理：**
1. 读取基准工具坐标系（InputTF_ID）的位姿数据
2. 读取拍照点位姿（CamPose_ID）PR寄存器数据
3. 读取基准视觉模板数据（RefVis_ID）PR寄存器数据
4. 读取实际视觉坐标数据（ActVis_ID）PR寄存器数据
5. 通过坐标变换矩阵计算工具坐标系的相对偏差
6. 将计算结果写入结果工具坐标系（ResultTF_ID）

**注意事项：**
- 所有PR寄存器必须在使用前手动创建并写入正确的位姿数据
- 基准视觉模板数据（RefVis_ID）和实际视觉坐标数据（ActVis_ID）需要根据视觉系统输出手动写入对应的PR寄存器
- 计算结果会自动进行误差验证，并在日志中输出误差分析信息
- 工具坐标系ID必须在1-30之间

---

### 11. DecToHex - 从十进制转换为十六进制

将R寄存器中的十进制数值转换为十六进制字符串，并写入SR寄存器。

**参数：**
- `R_ID` (int): R寄存器编号，包含需要转换的十进制数（支持整数和浮点数）
- `SR_ID` (int): SR寄存器编号，用于保存转换后的十六进制字符串

**转换规则：**
1. 浮点数处理：截断（直接丢弃小数部分，不四舍五入）
2. 数值范围：32位整数（-2147483648 到 2147483647）
3. 负数处理：使用32位补码形式表示
4. 输出格式：固定8位十六进制字符串（大写，不足8位前面补零）

**示例：**
```
// 示例1：正数转换
R[1] = 255
CALL_SERVICE CM, DecToHex, R_ID=1, SR_ID=1
// 结果：SR[1] = "000000FF"

// 示例2：浮点数截断
R[1] = 255.99
CALL_SERVICE CM, DecToHex, R_ID=1, SR_ID=1
// 结果：SR[1] = "000000FF"（截断为255）

// 示例3：负数转换（32位补码）
R[1] = -1
CALL_SERVICE CM, DecToHex, R_ID=1, SR_ID=1
// 结果：SR[1] = "FFFFFFFF"

// 示例4：负数转换
R[1] = -255
CALL_SERVICE CM, DecToHex, R_ID=1, SR_ID=1
// 结果：SR[1] = "FFFFFF01"

// 示例5：零值
R[1] = 0
CALL_SERVICE CM, DecToHex, R_ID=1, SR_ID=1
// 结果：SR[1] = "00000000"
```

**注意事项：**
- R寄存器中的浮点数会被截断为整数（不四舍五入）
- 数值必须在32位整数范围内（-2147483648 到 2147483647）
- 负数使用32位补码形式表示
- 输出始终为8位大写十六进制字符串，不足8位前面补零

---

### 12. TurnCountToR - 将PR位姿回转数写入R寄存器（单轴）

读取指定 `PR_ID` 寄存器中的 `turnCircle`（回转数），并只写入一个指定轴（`Joint_ID`）到 `R[R_ID]`。

**参数：**
- `PR_ID` (int): PR寄存器编号，读取其姿态中的 `turnCircle`
- `R_ID` (int): R寄存器编号（单个R）
- `Joint_ID` (int): 需要写入的关节轴（1=J1, 2=J2, ..., 6=J6）

**数据规则：**
- 仅允许写入值 `-1 / 0 / 1`
- 只替换指定轴的回转标记，不涉及其它轴

**示例：**
```
CALL_SERVICE CM, TurnCountToR, PR_ID=10, R_ID=100, Joint_ID=3
```

---

### 13. RToTurnCount - 将R寄存器回写到PR位姿回转数（单轴）

从 `R[R_ID]` 读取值，校验后只写回 `PR_ID` 的指定轴（`Joint_ID`）回转标记。

**参数：**
- `PR_ID` (int): PR寄存器编号，写入其姿态中的 `turnCircle`
- `R_ID` (int): R寄存器编号（单个R）
- `Joint_ID` (int): 需要回写的关节轴（1=J1, 2=J2, ..., 6=J6）

**数据规则：**
- R寄存器值必须为整数，且仅允许 `-1 / 0 / 1`
- 只替换指定轴，其它轴保持不变

**示例：**
```
CALL_SERVICE CM, RToTurnCount, PR_ID=10, R_ID=100, Joint_ID=3
```

---

## 关键项

### 核心特性

- **长连接机制：**自动管理机器人连接，首次调用时自动连接，已连接时复用现有连接
- **自动寄存器创建：**Strp指令支持自动创建R寄存器（如果不存在），PR寄存器需要手动创建
- **数据验证：**所有指令都包含完整的参数验证和错误处理
- **精度控制：**坐标系参数值自动保留三位小数
- **分隔符自动检测：**Strp指令支持自动检测多种分隔符（逗号、分号、竖线、制表符、空格等）
- **回转数同步：**支持PR `turnCircle` 与R寄存器双向同步（`TurnCountToR` / `RToTurnCount`，单轴替换）

---

## 使用方法

1. 打开 应用 - 插件管理 ，点击右上角的 安装插件 按钮。

![图片](images/Step1_intall.jpg)

2. 安装CM_oldsdk.gbtapp

![图片](images/Step2_findPlugin.jpg)

3. 在 简单服务 类别中启用 CM_oldsdk.gbtapp

![图片](images/Step3_startPlugin.jpg)

4. 在建好的程序test里面插入插件指令，这里以更改TF1的X值举例，运行程序，可以看到TF的X值已被更改，其他的指令参照readme。

![图片](images/Step4_addCommand1.jpg)



![图片](images/Step4_addCommand2.jpg)



![图片](images/Step4_addCommand3.jpg)









## 注意事项

### 重要提示

1. **坐标系ID范围：**ID必须在1-30之间，0是基础坐标系不可修改
2. **参数编号：**位置参数编号必须在1-6之间（1=X, 2=Y, 3=Z, 4=A, 5=B, 6=C）
3. **寄存器存在性：**使用R寄存器或PR寄存器前，建议确保寄存器已创建（Strp指令支持自动创建R寄存器）
4. **连接状态：**确保机器人已正确连接且可访问，插件会自动管理连接
5. **数据类型：**所有数值参数会自动进行类型转换和验证
6. **Strp指令特殊说明：**
   - PR寄存器需要手动创建，使用前请确保PR寄存器已存在
   - R_ID_Status 和 R_ID_Error 寄存器如果不存在会自动创建
   - 状态位为0时，不会进行数据拆解，直接返回错误（R_ID_Status=0, R_ID_Error=1）
   - 写入PR寄存器后会立即验证数据是否正确写入
   - 建议在使用前检查 R_ID_Status 和 R_ID_Error 的值来判断执行结果
7. **错误处理：**所有指令返回字典格式，包含success、message/error字段，建议始终检查success字段
8. **回转数约束：**`TurnCountToR` 与 `RToTurnCount` 仅支持 `-1/0/1`，用于轴回转标记同步

---

## 版本历史

### V1.4 (2026年3月25日)
- 新增 **TurnCountToR** 指令：将PR寄存器中的回转数（turnCircle）写入R寄存器
- 新增 **RToTurnCount** 指令：将R寄存器中的回转数回写到PR寄存器
- 增强回转数数据校验：仅允许 `-1/0/1`
- 将 **TurnCountToR** / **RToTurnCount** 调整为“单轴替换”模式：每次只更新一个轴，新增 `Joint_ID` 指定 J1~J6
- 回转数同步仍仅支持 `-1/0/1`

### V1.3 (2026年1月12日)
- 新增 **DecToHex** 指令：从十进制转换为十六进制
- 支持将R寄存器中的十进制数值转换为32位十六进制字符串
- 支持浮点数截断、负数补码转换
- 输出固定8位大写十六进制格式

### V1.2 (2026年1月9日)
- 新增 **TFShift** 指令：工具坐标系补正（基于视觉反馈）
- 支持通过视觉反馈自动计算和更新工具坐标系
- 提供高精度坐标变换计算，支持误差验证

### V1.1 (2026年1月9日)
- 新增 SDK v2.0.0.0 版本支持
- 更新坐标系接口：使用 `coordinate_system.TF/UF` 子类
- 更新坐标系数据结构：使用 `coordinate.data.x/y/z/a/b/c`
- 更新连接状态检查：`is_connect()` → `is_connected()`
- 更新错误处理：使用 `ret.errmsg` 获取错误信息
- 更新 Extension 使用方式：支持独立实例化获取IP地址

---

**CM 用户自定义指令插件 | 版本 V1.4 | 更新日期：2026年3月25日 | © 2026**
