Initial import of linkerhand calibration programs
This commit is contained in:
@@ -0,0 +1,69 @@
|
||||
# 手部标定
|
||||
|
||||
这个仓库有两套标定,都在界面里操作。URDF 标定量出关节行程,并写出一份新的 URDF。行程标定量出每条指令实际能到达的范围,并写出结果文件。
|
||||
|
||||
先做完「准备」,再按对应的步骤启动。下面的命令都在仓库根目录、已经 source 过 ROS 2 的终端里执行。
|
||||
|
||||
## 准备
|
||||
|
||||
1. 安装界面依赖:
|
||||
|
||||
```bash
|
||||
pip install numpy scipy pyyaml PyQt5
|
||||
```
|
||||
|
||||
2. 编译相机包并 source 本仓库:
|
||||
|
||||
```bash
|
||||
colcon build --packages-select camera_parameters
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
3. 把下面三个文件里的 `setup` 改成这台机器上的路径:
|
||||
|
||||
- `src/urdf_calibration/hands/o30/config.yaml`:O30 驱动的 `install/setup.bash`
|
||||
- `src/range_calibration/hands/o30/config.yaml`:同一份 O30 驱动
|
||||
- `src/camera_parameters/config/cameras.yaml`:能启动海康相机的 `setup.bash`
|
||||
|
||||
4. 三台相机的内外参放在仓库根目录 `output_camera/`。两套标定都使用其中时间最新的一份。
|
||||
|
||||
5. 填写工装位置:
|
||||
|
||||
- URDF 标定:在 `src/urdf_calibration/hands/o30/config.yaml` 里填写 `fixture_from_hand_base_link` 和 `front_from_fixture`。缺了这两项时,点「开始标定」会停住。
|
||||
- 行程标定:在 `src/range_calibration/hands/o30/config.yaml` 里把这两项改成这套工装的实测值。
|
||||
|
||||
## URDF 标定
|
||||
|
||||
1. 启动界面。窗口打开后会拉起三台相机。进度栏显示「相机没有拉起」时,回到准备第 3 步检查相机的 `setup`。
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/urdf_calibration python3 -m gui
|
||||
```
|
||||
|
||||
2. 型号选 O30,左右手选右手,点「连接」。左手目前连不上,界面会说明原因。连接成功并收到关节反馈后,「开始标定」和滑块才可用。
|
||||
|
||||
3. 看三路图像。上面相机看全工装板上四张角贴纸后,画面上会画出工装坐标。需要手动摆手指时,拖动对应滑块,手会跟着动;在数值框里输入后按回车。点「同步当前目标」把滑块对齐到当前反馈。
|
||||
|
||||
4. 点「开始标定」。进度栏会显示当前阶段和当前关节。标定过程中滑块停用,等它自己跑完。
|
||||
|
||||
5. 通过后,结果写在 `output_urdf_calibration/`,同一时间会有一份 `.json` 和一份同名 `.urdf`。源 URDF 仍在 `src/urdf_calibration/hands/o30/urdf/`。关掉窗口后,这次拉起的相机和驱动会一起退出。
|
||||
|
||||
## 行程标定
|
||||
|
||||
1. 启动界面。窗口打开后会拉起三台相机和贴纸检测。顶部显示「相机没有拉起」时,回到准备第 3 步检查相机的 `setup`。
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/range_calibration python3 -m gui
|
||||
```
|
||||
|
||||
2. 型号选 O30,选左手或右手,点「连接」。连接成功并收到关节反馈后,滑块和标定按钮才可用。
|
||||
|
||||
3. 先保存全手基础姿态。拖动滑块把整只手摆到基础位置,等运动完成、反馈稳定。下拉框选「全手基础姿态」,点「保存当前已到达姿态」。状态变成「所需示教已完成」后才能开始。避让姿态已经写在型号配置里,保存基础姿态即可。
|
||||
|
||||
4. 点「开始标定」,按顺序测完全部任务。只重测一项时,在下拉框里选中该任务,点「试标定 / 重测所选任务」。
|
||||
|
||||
5. 扫描过程中可以点「暂停」。点「继续(重做当前任务)」会把当前这项重新测一遍。点「取消 / 保持目标」会停在当前目标。
|
||||
|
||||
6. 完成后,界面下方会显示结果目录,文件在 `output_range_calibration/`。表格里「指令曲线」是这次记下的指令。关掉窗口后,这次拉起的相机和驱动会一起退出。
|
||||
|
||||
「下发行程偏移到设备」只接受旧的 min/max 结果。这次写出的曲线文件交给嵌入式烧录。「清除行程偏移」会把设备上的偏移恢复成每个关节 0~255。
|
||||
+295
@@ -0,0 +1,295 @@
|
||||
# URDF 标定工程实现说明
|
||||
|
||||
日期:2026-10-11。本文是 `src/urdf_calibration` 的实现说明。后续验证出了问题,按这里的数据流和拒绝原因往下查。
|
||||
|
||||
O30 右手标定做两件事:转轴用源 URDF 里已经写好的 `<axis xyz>`,角度用活动贴纸在图像里转过的边。全部关节通过后先写成 JSON,再只读这份 JSON 修改 URDF 的 `limit`。不改 `<axis xyz>`,也不改 `origin.rpy`。不从贴纸的三维朝向里估计一根新轴,也不做两套三维姿态的家族选择。
|
||||
|
||||
启动:
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/urdf_calibration python3 -m gui
|
||||
```
|
||||
|
||||
## 1. 角度怎么算
|
||||
|
||||
准备运动不再从活动贴纸的三维朝向里求转轴。转轴用 URDF 的 `<axis xyz>`,沿父连杆的 `origin` 从手掌算到该关节,再乘手掌相对工装、工装相对相机的位姿,得到这根轴在负责拍摄的那台相机里的方向。相机光轴和这根轴的夹角因此是已知量。
|
||||
|
||||
画面只读活动贴纸在图像里转过的角度。这个读数是绕相机光轴的分量。把它除以轴与光轴的点积,得到绕 URDF 轴的关节角。符号用右手定则,和 URDF 正方向一致。
|
||||
|
||||
### 坐标系
|
||||
|
||||
每一级都是刚体变换。点从右边的坐标系变到左边的坐标系。实现是 `calibration/acquisition/angle.py` 的 `axis_in_camera` 和 `point_in_camera`:
|
||||
|
||||
```
|
||||
轴_相机 = R_相机←正面 × R_正面←工装 × R_工装←手掌 × 轴_手掌
|
||||
```
|
||||
|
||||
- 轴在手掌中:`UrdfKinematicModel.axis_line(关节名, zero_offsets=..., joint_angles={})` 的第一个返回值,是这根轴在 `hand_base_link` 里的方向。第二个返回值是轴上一点。`joint_frame` 用同一套父子链,另外给出关节坐标系的旋转,画面投影用它。
|
||||
- 已经测完的祖先放进 `zero_offsets`,单位弧度。正在保持在基准指令上的祖先,`joint_angles` 填 0。不要把零位和关节角各加一次。本关节自己的零位绕自己的轴转,不改变自己的轴方向,算方向时填 0。
|
||||
- 这次写入 URDF 的 origin offset 固定为 0,所以内存里的祖先零位数值也是 0。键必须存在,用来表示这一节已经测完。缺键就是 `parent_zero_missing`,不要用「没写进字典」假装 CAD 已经对齐。
|
||||
- 手掌到工装:`hands/o30/config.yaml` 的 `fixture_from_hand_base_link`,`xyz` 单位米,`rpy` 单位弧度,绕固定轴 X-Y-Z。缺了失败,不用单位阵。
|
||||
- 工装到正面相机:同一文件的 `front_from_fixture`,4×4。标定只用左上角 3×3。缺了以 `fixture_pose_missing` 停下,并且在真机还没动之前就停。
|
||||
- 正面相机到侧面、上面:`output_camera/extrinsics` 里时间戳最新的一份。字段 `front_from_view`,四元数 `xyzw`。`front_from_view.side` 把点从侧面相机变到正面相机。侧面相机里的轴是 `R_正面←侧面` 的逆乘上去。上面相机同样。正面那一项是单位变换。内参必须对上该文件记录的指纹,由 `calibration/cameras/files.py` 的 `load_camera_files` 检查。
|
||||
|
||||
相机用 OpenCV 约定:x 向右,y 向下,z 沿光轴指向场景。光轴单位向量是 `(0, 0, 1)`。
|
||||
|
||||
不从手掌贴纸 0、3、16 反解手掌位姿。单张贴纸的两个朝向不能用来定手掌。工装板贴纸 20、21、28、29 只画工装坐标,不参与关节角。
|
||||
|
||||
### 图像角
|
||||
|
||||
活动贴纸只提供四个角点。检测器是 AprilTag `tag36h11`,角点顺序用 `lb-rb-rt-lt`,整段运动不换起点。实现在 `calibration/session/live.py` 的 `_detect`。
|
||||
|
||||
每一帧:
|
||||
|
||||
1. `边 = 角点1 - 角点0`。
|
||||
2. `φ = atan2(边的 y, 边的 x)`,单位弧度。函数是 `edge_angle`。
|
||||
3. 按时间顺序展开,避免在 ±π 处跳变。函数是 `unwrap_edge_angle`。相邻帧跳变超过 35° 丢掉这一帧,原因 `corner_jump`,不当成姿态翻转后的另一个解。
|
||||
4. 零位参考帧是基准指令、返回程、贴纸停稳时的展开角 `φ零位`。`calibration/session/runner.py` 的 `_joint_samples` 取该关节观测里第一次出现的这一帧。
|
||||
|
||||
`φ` 的正方向和绕相机 +z 的右手定则一致。图像里 x 向右、y 向下,绕 +z 的正转是从 x 转向 y,看起来是顺时针。
|
||||
|
||||
读的是正方形边的方向,不是贴纸中心在画面里划过的弧。中心离开转轴时会平动,那不是关节角。角点整体平移而边的方向不变时,`φ` 不变。
|
||||
|
||||
### 还原成关节角
|
||||
|
||||
```
|
||||
c = 轴在该相机中的单位向量 · (0, 0, 1)
|
||||
θ = (φ - φ零位) / c
|
||||
```
|
||||
|
||||
`θ` 是绕 URDF 轴、从零位参考帧量起的关节角,单位弧度。`c` 带符号,轴朝向相机还是背向相机都会自动反号。
|
||||
|
||||
数值例子:轴与光轴夹角 10°,`c = cos(10°) ≈ 0.9848`。图像里贴纸比零位多转了 0.52 rad。关节角是 `0.52 / 0.9848 ≈ 0.528 rad`。用实测点积,不写死 10°。
|
||||
|
||||
`|c| < cos(20°)`(约 0.9397)时拒绝,原因 `axis_camera_angle_too_large`。刚好小于拒绝,刚好大于接受。20° 时夹角再错 1°,角度缩放大约再错 0.6%。超过这个范围,图像转角不再是关节角的稳定投影。
|
||||
|
||||
视觉 `θ` 只按 URDF 右手定则,不乘 `command.sdk_to_joint_direction`。食指、中指、无名指、小指侧摆在这个数组里是 `-1`,其余是 `1`。这个符号留给指令和电机的对应关系,避免乘两次。
|
||||
|
||||
入口是 `reading_from_corners`。它不读 ROS,不读文件。输入是角点、相机名、URDF 模型、零位 offset、工装变换和外参。输出是 `θ`、轴在相机中的单位向量、轴在手掌中的方向、`c`、`φ`、拒绝原因。`reason` 为空才算通过。
|
||||
|
||||
| 原因 | 条件 |
|
||||
| --- | --- |
|
||||
| `parent_zero_missing` | 会改变这根轴方向的祖先还没测完 |
|
||||
| `fixture_pose_missing` | 手掌到工装或工装到正面相机缺失,或这一帧工装位姿无效 |
|
||||
| `tag_not_visible` | 没有四个有效角点 |
|
||||
| `corner_jump` | 相对上一帧展开时跳变超过 35°。这一帧丢掉,不因此结束关节 |
|
||||
| `axis_camera_angle_too_large` | `|c| < cos(20°)` |
|
||||
| `zero_reference_missing` | 返回程没有采到基准指令 |
|
||||
| `missing_camera_extrinsic:<相机>` | 外参里没有这台相机 |
|
||||
| `camera_axis_degenerate` | 变到相机后的轴长度接近 0 |
|
||||
| `tag_mount_unresolved` | 零位帧估计不出贴纸安装 |
|
||||
| `reprojection_exceeded` | 用冻结的安装重投影,均方根超过 1.5 px |
|
||||
| `sample_axis_disagrees_with_urdf` | 样本里的轴和 `axis_line` 夹角超过约 3° |
|
||||
| `device_uid_invalid` | 设备 UID 为空,或含有路径字符 |
|
||||
| `command_not_ready` | 关节指令还不能发 |
|
||||
| `command_not_settled` | 反馈在超时内没有靠近目标指令 |
|
||||
| `fixed_reference_drift` | 基准贴纸帧数不够,或角点漂移超过 5 px |
|
||||
| `apriltag_missing` | 当前环境没有 AprilTag 库 |
|
||||
|
||||
另外,样本门限没达到时原因直接用缺的那一项:`minimum_valid_samples`、`directional_samples_incomplete`、`endpoint_missing`、`bins_unobserved`、`first_cycle_missing`、`first_cycle_span`、`repeat_span`、`steady_nodes`。
|
||||
|
||||
通过后的检查是重投影,实现在 `calibration/acquisition/reprojection.py`。零位那一帧用贴纸边长做一次位姿,得到四个角在相机里的位置,作为安装。之后每一帧只绕已知轴转已知 `θ`,再投回图像。安装是常量,扫行程时不再估。均方根超过 1.5 px 表示贴纸不是在绕这根轴转,例如别的关节在动、贴纸松了或手掌滑了。轴和每一帧的 `θ` 不再自由优化。
|
||||
|
||||
有效样本写成绕掌心中这根 URDF 轴的旋转:`quaternion_about_axis`,`Rotation.from_rotvec(轴_手掌 * θ)`,scipy 四元数顺序 `xyzw`。`assert_sample_axis` 再把这个轴和 `axis_line` 比一次。
|
||||
|
||||
## 2. 零位和行程
|
||||
|
||||
图像角从基准指令那一帧量起,所以这一帧的 `θ` 是 0。这个 0 是尺子的起点,不是这只手相对工厂模型的安装误差。
|
||||
|
||||
贴纸在连杆上怎么贴是未知的,准备段只把它估成一个常量,用来做重投影。这套相对转角看不到「基准姿势和 CAD 零位差多少度」。因此:
|
||||
|
||||
- 源 URDF 的 `origin.rpy` 和 `<axis xyz>` 保持原文字。`write_authorized_urdf` 在 offset 绝对值不超过 `1e-12` 时不改写 `origin` 那一行。
|
||||
- `thumb_ip`、`index_dip`、`middle_dip`、`ring_dip`、`pinky_dip` 的授权里本来就没有 `origin.rpy`。它们同样只改 `limit`。
|
||||
- 行程取去程、回程各自的最小和最大,用两侧外沿。两个方向不平均。同一条指令上两个方向的角度可以不同,那是间隙和弹性。
|
||||
- 拟合调用 `directional_envelope`。它用返回程、基准指令处的最后一个 `θ` 做一次对齐。这个数接近 0。不要在它之外再减一次,也不要把它写进 `corrected_origin_rpy`。写 URDF 时每个关节的 origin offset 传 0。
|
||||
|
||||
四指指根侧摆的基准指令不是数值 0:食指 255,中指 196,无名指 113,小指 52。零位是这些指令对应的姿势。
|
||||
|
||||
反馈字节只用于指令到位和停稳。角度只来自图像。
|
||||
|
||||
## 3. 配置
|
||||
|
||||
路径都相对 `src/urdf_calibration`,外参和结果在仓库根目录。
|
||||
|
||||
| 文件 | 内容 |
|
||||
| --- | --- |
|
||||
| `hands/o30/config.yaml` | 这台机器的驱动 `setup`、`fixture_from_hand_base_link`、`front_from_fixture` |
|
||||
| `hands/o30/profile.yaml` | 指令名、基准指令、测量顺序、贴纸、避让、样本门限、授权字段 |
|
||||
| `hands/o30/product.yaml` | 型号、左右手、源 URDF 相对路径 |
|
||||
| `hands/o30/sdk.yaml` | 驱动可执行文件和三个话题 |
|
||||
| `src/camera_parameters/config/cameras.yaml` | 相机环境、帧率、曝光、贴纸边长、工装板四张角贴纸 |
|
||||
| `hands/o30/urdf/linkerhand_O30i_right-V2_0819.urdf` | 源 URDF。结果另存,不覆盖这里 |
|
||||
| `output_camera/` | 最新外参,以及指纹对得上的内参 |
|
||||
|
||||
贴纸编号只留在 `profile.yaml` 的 `vision.views`。每个机位下有 role、`id`、边长、连杆。`measurement.measurements.<关节>.child_role` 指向活动贴纸,`view` 指向相机。`hands/o30/motion.py` 的 `tag_for_joint` 按关节名做这次查找。不要把编号复制到 `config.yaml`,也不要在 Python 里写死。
|
||||
|
||||
`fixed_reference: true` 的是手掌上的基准贴纸:正面 0、侧面 3、上面 16。它们只检查图像里没漂移,不参与求关节角。
|
||||
|
||||
关节名和 URDF 铰链名相同,见 `command.urdf_joint_by_joint`。写出时按这个名字改对应的 `<joint>` 块。
|
||||
|
||||
## 4. 关节、轴、相机和贴纸
|
||||
|
||||
轴的方向以源 URDF、祖先处于基准指令时为准。`1 0 0` 是手掌 X,垂直于手掌,朝向正面相机。`0 1 0` 是手掌 Y,横过手指,朝向侧面相机。`0 0 -1` 沿手指,朝向上面相机。
|
||||
|
||||
测量顺序是 `zero.spatial.axis_order`。会改变下一节轴方向的祖先用 `axis_parent_joint` 向上走。`phase_parent_joint` 绕同一根轴,不改变下一节轴的方向,不放进「缺了就不能测」的名单,但测量顺序仍按 `axis_order`。
|
||||
|
||||
`required_ancestors` 的规则:本关节若在 `axis_parent_joint` 里,就沿这条链向上。本关节若只在 `phase_parent_joint` 里,先沿着相位父节跳,直到碰到一个有 `axis_parent_joint` 的关节,再沿那条链向上。相位父节自己不放进祖先名单。
|
||||
|
||||
| 关节 | 轴 | 相机 | 活动贴纸 role(tag id) | 基准指令 | 必须先测完的祖先 |
|
||||
| --- | --- | --- | --- | --- | --- |
|
||||
| `thumb_cmc_roll` | 手掌 X | front | `thumb_mcp`(1) | 0 | 无 |
|
||||
| `index_mcp_roll` | 手掌 X | front | `index_mcp_roll`(15) | 255 | 无 |
|
||||
| `middle_mcp_roll` | 手掌 X | front | `middle_mcp_roll`(14) | 196 | 无 |
|
||||
| `ring_mcp_roll` | 手掌 X | front | `ring_mcp_roll`(13) | 113 | 无 |
|
||||
| `pinky_mcp_roll` | 手掌 X | front | `pinky_mcp_roll`(12) | 52 | 无 |
|
||||
| `thumb_cmc_yaw` | 手掌 -Z | top | `thumb_cmc_yaw`(17) | 0 | `thumb_cmc_roll` |
|
||||
| `thumb_mcp` | 手掌 X | front | `thumb_mcp`(1) | 0 | `thumb_cmc_yaw` |
|
||||
| `thumb_ip` | 与 `thumb_mcp` 同向 | front | `thumb_ip`(2) | 0 | `thumb_cmc_yaw` |
|
||||
| `index_mcp_pitch` | 手掌 Y | side | `index_pip`(10) | 0 | `index_mcp_roll` |
|
||||
| `index_pip` | 与食指指根弯曲同向 | side | `index_pip`(10) | 0 | `index_mcp_roll` |
|
||||
| `index_dip` | 同上 | side | `index_dip`(11) | 0 | `index_mcp_roll` |
|
||||
| `middle_mcp_pitch` | 手掌 Y | side | `middle_pip`(8) | 0 | `middle_mcp_roll` |
|
||||
| `middle_pip` | 同上 | side | `middle_pip`(8) | 0 | `middle_mcp_roll` |
|
||||
| `middle_dip` | 同上 | side | `middle_dip`(9) | 0 | `middle_mcp_roll` |
|
||||
| `ring_mcp_pitch` | 手掌 Y | side | `ring_pip`(6) | 0 | `ring_mcp_roll` |
|
||||
| `ring_pip` | 同上 | side | `ring_pip`(6) | 0 | `ring_mcp_roll` |
|
||||
| `ring_dip` | 同上 | side | `ring_dip`(7) | 0 | `ring_mcp_roll` |
|
||||
| `pinky_mcp_pitch` | 手掌 Y | side | `pinky_pip`(4) | 0 | `pinky_mcp_roll` |
|
||||
| `pinky_pip` | 同上 | side | `pinky_pip`(4) | 0 | `pinky_mcp_roll` |
|
||||
| `pinky_dip` | 同上 | side | `pinky_dip`(5) | 0 | `pinky_mcp_roll` |
|
||||
|
||||
`thumb_ip` 的祖先名单是 `thumb_cmc_roll`、`thumb_cmc_yaw`。`thumb_mcp` 是相位父节,绕同一根 X,不放进这份名单,但 `axis_order` 仍把它排在 `thumb_ip` 前面。四指的中节、末节同样:必须先测完的是该指的指根侧摆,指根弯曲和中节不因为相位关系阻塞下一节。
|
||||
|
||||
`thumb_mcp` 的 `origin rpy` 是 `-0.33161 0 0`,绕 X,不把轴从父连杆的 X 上甩开。中指、无名指、小指与食指相同:指根侧摆轴 `1 0 0` 且父连杆是手掌,弯曲、中节、末节轴 `0 1 0`。
|
||||
|
||||
拇指顺序:`thumb_cmc_roll`,`thumb_cmc_yaw`,`thumb_mcp`,`thumb_ip`。同一根手指:先侧摆,侧摆之后停在它的基准指令上,再测弯曲、中节、末节。
|
||||
|
||||
`measurement_order` 按 `axis_order` 逐个关节生成 `JointMeasurement`。关节属于某任务,只要名字出现在该任务的 `joints` 里。四指侧摆共用 `fingers_mcp_roll_front`,食指、中指、无名指、小指都单独取样,各自使用自己的贴纸、`baseline_u8` 和样本。避让仍走该任务的 `preparation_groups` 和 `auxiliary_commands`。被测通道是这个关节自己的 `command_index_by_joint`,不是任务里 `joints[0]` 的下标。
|
||||
|
||||
## 5. 一场标定怎么跑
|
||||
|
||||
界面在 `gui/window.py`。点「连接」之后 `O30Hand.open` 拉起 SDK,并等 `/cb_<左右手>_hand_info` 里的型号、左右手、20 个关节和 `uid` 都对上。没有 `uid` 时原因是 `device_uid_missing`,连接失败。
|
||||
|
||||
点「开始标定」调用 `calibration/session/live.py` 的 `run_live`。滑块在标定期间停用。
|
||||
|
||||
1. 先检查工装变换和 UID,真机还没动。`fixture_from_hand_base_link` 缺失、`front_from_fixture` 缺失,或 UID 不是由字母、数字、下划线、连字符组成,直接停下。
|
||||
2. 按 `axis_order` 逐个关节。祖先不在已完成名单里则 `parent_zero_missing`。
|
||||
3. `avoidance_waypoints` 生成避让路径。反馈在 `stall_timeout_seconds`(profile 里是 4 秒,大步长会按全行程 13.35 秒按比例加长)内没有靠近目标,原因 `command_not_settled`。到位容差是 2 个指令单位。
|
||||
4. 每个关节扫两个循环。每个循环先去程再回程。指令从 `start_u8` 到 `end_u8` 均匀取大约 40 个整数,并保证起点、终点和基准指令都在序列里。回程是把这条序列反过来。
|
||||
5. 每个指令点等待活动贴纸的四个角。稳态点是这条指令序列上均匀取出的 9 个,等待 1 秒;其余点等待 0.5 秒。看不见则 `tag_not_visible`。
|
||||
6. 同时看这台相机上的基准贴纸。整个关节里少于 10 帧,或相对第一帧的角点位移超过 5 px,原因 `fixed_reference_drift`。
|
||||
7. 本关节立刻做角度还原、重投影和样本门限。通过后才测下一关节。内存里把该关节的 origin offset 记为 0,并交给界面重画坐标轴。
|
||||
8. 20 个关节都通过后,`run_calibration` 再用同一批角点算一次样本,写入 JSON,然后只读这份 JSON 写出 URDF。
|
||||
|
||||
反馈只用来判断到位。关节角只来自角点。
|
||||
|
||||
## 6. 样本门限
|
||||
|
||||
数字在 `profile.yaml` 的 `acquisition`,检查函数是 `calibration/acquisition/gates.py` 的 `check_samples`。
|
||||
|
||||
- 至少 40 个有效样本。
|
||||
- 指令范围分成 32 个箱,空箱比例不超过 6.25%,也就是 32 个箱里最多空 2 个。
|
||||
- 去程和回程都要有。返回程要采到基准指令。
|
||||
- `start_u8` 和 `end_u8` 都要出现。
|
||||
- 第 0 个循环的指令跨度至少是全行程的 85%。另一个循环的跨度至少是第 0 个循环的 90%。
|
||||
- 标记为稳态的样本至少 9 个。
|
||||
|
||||
未达到则该关节失败。此时还没有 JSON,也没有新的 URDF。
|
||||
|
||||
## 7. JSON 和拟合
|
||||
|
||||
全部关节通过后,文件写到仓库根目录:
|
||||
|
||||
```
|
||||
output_urdf_calibration/o30_<左右手>_<设备UID>_<YYYYMMDD_HHMMSS>.json
|
||||
```
|
||||
|
||||
左右手是 `right` 或 `left`。时间是这次写入时的本地时间,精确到秒。例如 `o30_right_A1B2C3_20261011_223015.json`。UID 不合法时失败,不退回 `o30_right.json`。profile 里的 `o30_right_{serial_number}_calibration.json` 不使用。
|
||||
|
||||
JSON 的结构:
|
||||
|
||||
```json
|
||||
{
|
||||
"model": "o30",
|
||||
"side": "right",
|
||||
"device_uid": "A1B2C3",
|
||||
"finished_at": "2026-10-11T22:30:15",
|
||||
"joints": {
|
||||
"thumb_cmc_roll": {
|
||||
"baseline_u8": 0,
|
||||
"samples": [
|
||||
{
|
||||
"joint": "thumb_cmc_roll",
|
||||
"command_u8": 0,
|
||||
"direction": "decreasing",
|
||||
"cycle": 0,
|
||||
"view": "front",
|
||||
"theta_rad": 0.0,
|
||||
"axis_palm_xyz": [1.0, 0.0, 0.0],
|
||||
"relative_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
|
||||
"steady": true
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
`direction` 只有 `increasing` 和 `decreasing`。基准指令那一帧的 `theta_rad` 是 0。两个方向都保留。
|
||||
|
||||
拟合函数是 `calibration/fitting/json_fit.py` 的 `fit_urdf_from_json`。它只打开这一份 JSON,不读相机,也不再使用内存里的角点。对每个关节调用 `directional_envelope`,得到相对那个近 0 零位的 `lower` 和 `upper`。然后调用 `write_authorized_urdf`,origin offset 全部传 0。
|
||||
|
||||
写出的 URDF 与 JSON 同名,扩展名是 `.urdf`,放在同一目录。不覆盖 `hands/o30/urdf/linkerhand_O30i_right-V2_0819.urdf`。`<axis xyz>` 写完后与源文件逐字比较,变了就拒绝写出。`limit` 用弧度。
|
||||
|
||||
任一关节失败时,`run_calibration` 还没执行到写文件。采集中途失败同样不写。
|
||||
|
||||
## 8. 画面
|
||||
|
||||
三路图像由 `calibration/cameras/rig.py` 接收。最新一帧留在 `latest_frame`,标定线程读角点;界面队列用来刷新画面。
|
||||
|
||||
上面相机看全 `cameras.yaml` 里 `fixture_plate.tag_ids`(20、21、28、29)后,`fixture_overlay.py` 把工装坐标投到每台相机。三根轴线宽都是 3。缺一张就不画。
|
||||
|
||||
工装变换齐全时,`joint_overlay.py` 把 `axis_order` 里的每个关节投到三路画面。原点是关节在该相机里的位置,三根轴是关节坐标系的 X、Y、Z,颜色是红、绿、蓝。原点在相机背后时这一路不画这个关节。投影带畸变,内参和外参与工装坐标相同。
|
||||
|
||||
旋转轴是 URDF `<axis xyz>` 在关节坐标系里绝对值最大的那一根分量。画面上这根线宽 6,另外两根线宽 3。轴是 `(0, 0, -1)` 时加粗的仍是 Z,不另画一根反向轴。
|
||||
|
||||
缺 `front_from_fixture` 或手掌到工装的变换时不画关节坐标。标定进行中,界面用内存里的 `zero_offsets` 重算,使画出的轴和正在用来算角度的轴是同一套父子链。
|
||||
|
||||
## 9. 代码落在哪
|
||||
|
||||
| 路径 | 作用 |
|
||||
| --- | --- |
|
||||
| `calibration/acquisition/angle.py` | `edge_angle`、`unwrap_edge_angle`、`axis_in_camera`、`point_in_camera`、`reading_from_corners`、`quaternion_about_axis` |
|
||||
| `calibration/acquisition/reprojection.py` | 零位帧估计安装,扫行程时重投影 |
|
||||
| `calibration/acquisition/gates.py` | 样本门限和基准贴纸漂移 |
|
||||
| `calibration/acquisition/fixture.py` | 读取手掌到工装。四元数必须是单位四元数 |
|
||||
| `calibration/acquisition/sample.py` | 把 `θ` 写成带四元数的样本 |
|
||||
| `calibration/urdf_model.py` | `UrdfKinematicModel.axis_line`、`joint_frame`、`corrected_origin_rpy` |
|
||||
| `calibration/cameras/files.py` | 最新外参和指纹匹配的内参 |
|
||||
| `calibration/cameras/joint_overlay.py` | 关节坐标系投影,并标出要加粗的轴 |
|
||||
| `calibration/cameras/fixture_overlay.py` | 工装板坐标系 |
|
||||
| `calibration/fitting/envelope.py` | 去程回程外沿,以及样本轴是否就是模型轴 |
|
||||
| `calibration/fitting/json_fit.py` | 文件名、写 JSON、只读 JSON 拟合 |
|
||||
| `calibration/fitting/urdf_out.py` | 只改授权的 `limit`;offset 为 0 时不改 `origin.rpy` |
|
||||
| `calibration/session/live.py` | 避让、扫两个循环、读角点 |
|
||||
| `calibration/session/runner.py` | 角点变成样本,通过后写 JSON 再拟合 |
|
||||
| `hands/o30/motion.py` | 测量顺序、祖先、贴纸编号 |
|
||||
| `hands/o30/avoidance.py` | 进入该关节之前的指令路径 |
|
||||
| `gui/window.py` | 三路画面、滑块、开始标定 |
|
||||
| `gui/jog.py` | 把 20 路目标发给已连接的手 |
|
||||
|
||||
单元测试在 `src/urdf_calibration/tests/test_urdf_calibration.py`。它检查 20 个关节都在测量顺序里、贴纸编号来自 profile、祖先规则、坏 UID 被拒绝、拟合后 `origin.rpy` 和 `<axis xyz>` 原文不变而 `limit` 改变,以及缺工装时不写文件。
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/urdf_calibration python3 -m unittest tests.test_urdf_calibration -v
|
||||
```
|
||||
|
||||
## 10. 验证时怎样对照
|
||||
|
||||
- 调用栈里没有铰链求解,也没有两套三维姿态的家族选择。角度只经过 `reading_from_corners`。
|
||||
- 贴纸编号来自 `profile.yaml` 的 `vision.views`。`config.yaml` 里没有关节贴纸表。
|
||||
- 工装变换缺失、UID 不合法,或会改变轴方向的祖先还没测完时,进度栏是第 1 节的原因。目录里没有新的 JSON,也没有新的 URDF。
|
||||
- 通过后的 JSON 文件名包含 `o30`、左右手、设备 UID 和到秒的时间。同一个 `command_u8` 上能看到 `increasing` 和 `decreasing` 两条 `theta_rad`。
|
||||
- 同名 URDF 的 `limit` 等于对这份 JSON 调用 `directional_envelope` 的外沿。拟合不打开相机话题。
|
||||
- 写出的 URDF 里,每个被改关节的 `<axis xyz>` 与源文件逐字相同,所有关节的 `origin.rpy` 与源文件逐字相同。
|
||||
- 工装变换齐全时,三路画面能看到关节坐标系。旋转轴比该坐标系里另外两根轴更粗。工装板三根轴仍然一样粗。缺工装变换时只有图像,没有关节坐标。
|
||||
@@ -0,0 +1 @@
|
||||
"""三相机内参读写与棋盘格外参标定。"""
|
||||
@@ -0,0 +1,217 @@
|
||||
"""Map camera frame clocks to host time before pairing images with feedback.
|
||||
|
||||
The MVS frame counter timestamps exposure start. USB delivery/publication is
|
||||
later. Clock latches bracket the device/host correspondence independently of
|
||||
hand motion, images, or fitted calibration curves.
|
||||
"""
|
||||
|
||||
from dataclasses import asdict, dataclass
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
import time
|
||||
|
||||
|
||||
CAMERA_TIMING_POLICY = "mvs_latched_exposure_time_v1"
|
||||
CLOCK_DRIFT_TOLERANCE_NS = 1_000_000
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ClockLatch:
|
||||
device_tick: int
|
||||
before_ns: int
|
||||
after_ns: int
|
||||
raw_before_ns: int | None = None
|
||||
raw_after_ns: int | None = None
|
||||
|
||||
@property
|
||||
def midpoint_ns(self):
|
||||
return (self.before_ns+self.after_ns)//2
|
||||
|
||||
@property
|
||||
def half_width_ns(self):
|
||||
return (self.after_ns-self.before_ns)/2
|
||||
|
||||
|
||||
class DeviceFrameClock:
|
||||
"""A nominal device clock with short, measured host-time brackets.
|
||||
|
||||
A new latch is checked against the previous one before it can change the
|
||||
mapping. This rejects wrong counter units, resets and host clock jumps.
|
||||
Periodic updates bound accumulated drift; none depend on image delivery.
|
||||
"""
|
||||
|
||||
def __init__(self, ticks_per_second):
|
||||
if not math.isfinite(ticks_per_second) or ticks_per_second <= 0:
|
||||
raise ValueError("invalid camera clock frequency")
|
||||
self.ticks_per_second = ticks_per_second
|
||||
self.anchor = None
|
||||
self.last_frame_tick = None
|
||||
self.last_stamp_ns = None
|
||||
self.refresh_interval_seconds = .5
|
||||
|
||||
def elapsed_ns(self, ticks):
|
||||
return round(ticks*1_000_000_000/self.ticks_per_second)
|
||||
|
||||
def update(self, latches):
|
||||
if not latches or any(x.device_tick <= 0 or x.before_ns <= 0
|
||||
or not 0 <= x.after_ns-x.before_ns <= 2_000_000 for x in latches):
|
||||
raise ValueError("camera clock latch is invalid or too uncertain")
|
||||
previous = self.anchor or latches[0]
|
||||
if self.anchor is None and latches[-1].before_ns-latches[0].after_ns < 50_000_000:
|
||||
raise ValueError("camera clock units need an independent elapsed-time check")
|
||||
for current in latches:
|
||||
elapsed = self.elapsed_ns(current.device_tick-previous.device_tick)
|
||||
host_elapsed = current.midpoint_ns-previous.midpoint_ns
|
||||
tolerance = current.half_width_ns+previous.half_width_ns+CLOCK_DRIFT_TOLERANCE_NS
|
||||
if elapsed < 0 or abs(elapsed-host_elapsed) > tolerance:
|
||||
raise ValueError("camera/host clock discontinuity or counter unit mismatch")
|
||||
# Refresh before measured phase drift spends the existing 1 ms
|
||||
# continuity allowance. A fixed 0.5 s period repeatedly invalidates a
|
||||
# smooth slewing host clock. Scheduling changes; admission never does.
|
||||
current = max(latches, key=lambda x: x.midpoint_ns)
|
||||
host_elapsed = current.midpoint_ns-previous.midpoint_ns
|
||||
if host_elapsed >= 50_000_000:
|
||||
drift = abs(host_elapsed-self.elapsed_ns(current.device_tick-previous.device_tick))
|
||||
rate = drift/host_elapsed
|
||||
self.refresh_interval_seconds = max(.02, min(.5,
|
||||
.25*CLOCK_DRIFT_TOLERANCE_NS/1e9/max(rate, 1e-12)))
|
||||
self.anchor = min(latches, key=lambda x: x.half_width_ns)
|
||||
|
||||
def timestamp(self, tick, *, received_ns, exposure_us):
|
||||
if (self.anchor is None or not 0 <= received_ns-self.anchor.midpoint_ns <= 2_000_000_000
|
||||
or not math.isfinite(exposure_us) or exposure_us < 0):
|
||||
raise ValueError("camera frame has no fresh clock or valid exposure")
|
||||
if self.last_frame_tick is not None and tick <= self.last_frame_tick:
|
||||
raise ValueError("camera frame counter reset or duplicate frame")
|
||||
stamp = self.anchor.midpoint_ns+self.elapsed_ns(tick-self.anchor.device_tick)+round(exposure_us*500)
|
||||
if stamp > received_ns+2_000_000 or (self.last_stamp_ns is not None and stamp <= self.last_stamp_ns):
|
||||
raise ValueError("camera image time is future or out of order")
|
||||
self.last_frame_tick, self.last_stamp_ns = tick, stamp
|
||||
return stamp
|
||||
|
||||
|
||||
class MvsCameraTiming:
|
||||
"""The camera-specific clock latch and its optional append-only journal."""
|
||||
|
||||
def __init__(self, camera, mvs, clock_ns, *, exposure_us, journal_path="", raw_clock_ns=None):
|
||||
self.camera, self.mvs, self.clock_ns = camera, mvs, clock_ns
|
||||
# CLOCK_MONOTONIC_RAW is independent of wall-clock synchronization.
|
||||
# It is diagnostic evidence only, never a replacement ROS timestamp.
|
||||
self.raw_clock_ns = raw_clock_ns or (lambda: time.clock_gettime_ns(time.CLOCK_MONOTONIC_RAW))
|
||||
self.exposure_us = exposure_us
|
||||
self.frame_clock = None
|
||||
self.last_stamp_ns = None
|
||||
self.next_refresh = 0.
|
||||
self.journal = None
|
||||
if journal_path:
|
||||
path = Path(journal_path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
self.journal = path.open("a", encoding="utf-8")
|
||||
|
||||
def _integer(self, name):
|
||||
value = self.mvs.MVCC_INTVALUE_EX()
|
||||
status = self.camera.MV_CC_GetIntValueEx(name, value)
|
||||
if status != 0:
|
||||
raise ValueError(f"camera clock cannot read {name}:0x{status:08x}")
|
||||
return int(value.nCurValue)
|
||||
|
||||
def _latch(self):
|
||||
raw_before = self.raw_clock_ns()
|
||||
before = self.clock_ns()
|
||||
status = self.camera.MV_CC_SetCommandValue("DeviceTimestampLatch")
|
||||
if status != 0:
|
||||
raise ValueError(f"camera clock latch failed:0x{status:08x}")
|
||||
tick = self._integer("DeviceTimestamp")
|
||||
after = self.clock_ns()
|
||||
return ClockLatch(tick, before, after, raw_before, self.raw_clock_ns())
|
||||
|
||||
def _write(self, row):
|
||||
if self.journal is not None:
|
||||
self.journal.write(json.dumps(row, separators=(",", ":"))+"\n")
|
||||
|
||||
def start(self):
|
||||
# The installed USB camera firmware returns ticks/second for this
|
||||
# feature (despite its XML unit label). Verify that interpretation
|
||||
# against elapsed host time; do not assume it for another firmware.
|
||||
self.frame_clock = DeviceFrameClock(self._integer("DeviceTimestampIncrement"))
|
||||
latches = []
|
||||
for index in range(40):
|
||||
if index:
|
||||
time.sleep(.02)
|
||||
latches.append(self._latch())
|
||||
valid = [x for x in latches if 0 <= x.after_ns-x.before_ns <= 2_000_000]
|
||||
if len(valid) >= 8 and valid[-1].before_ns-valid[0].after_ns >= 50_000_000:
|
||||
self._update(latches)
|
||||
return
|
||||
raise ValueError("camera clock has no bounded-latency startup latches")
|
||||
|
||||
def _update(self, latches):
|
||||
valid = [x for x in latches if 0 <= x.after_ns-x.before_ns <= 2_000_000]
|
||||
if not valid:
|
||||
# A delayed USB control transaction provides no new clock evidence.
|
||||
# Keep the original two-second freshness bound while trying again.
|
||||
self._write(dict(kind="camera_clock_latch_rejected", latches=[asdict(x) for x in latches]))
|
||||
self.next_refresh = time.monotonic()+.02
|
||||
return
|
||||
try:
|
||||
self.frame_clock.update(valid)
|
||||
except ValueError as error:
|
||||
# Invalidation alone loses the transaction that failed. Preserve
|
||||
# both clocks and the old anchor so drift, jumps and bad device
|
||||
# counters can be distinguished without weakening the check.
|
||||
self._write(dict(kind="camera_clock_sync_rejected", policy=CAMERA_TIMING_POLICY,
|
||||
reason=str(error), ticks_per_second=self.frame_clock.ticks_per_second,
|
||||
previous_anchor=asdict(self.frame_clock.anchor) if self.frame_clock.anchor else None,
|
||||
latches=[asdict(x) for x in latches]))
|
||||
if self.journal is not None:
|
||||
self.journal.flush()
|
||||
raise
|
||||
self._write(dict(kind="camera_clock_sync", policy=CAMERA_TIMING_POLICY,
|
||||
ticks_per_second=self.frame_clock.ticks_per_second,
|
||||
refresh_interval_seconds=self.frame_clock.refresh_interval_seconds,
|
||||
latches=[asdict(x) for x in latches], anchor=asdict(self.frame_clock.anchor)))
|
||||
if self.journal is not None:
|
||||
self.journal.flush()
|
||||
# Startup may select a precise latch near the beginning of its window.
|
||||
# That anchor has already spent part of the refresh budget.
|
||||
anchor_age = (max(x.after_ns for x in latches)-self.frame_clock.anchor.midpoint_ns)/1e9
|
||||
self.next_refresh = time.monotonic()+max(0., self.frame_clock.refresh_interval_seconds-anchor_age)
|
||||
|
||||
def refresh_if_due(self):
|
||||
if self.frame_clock is None:
|
||||
self.start()
|
||||
elif time.monotonic() >= self.next_refresh:
|
||||
self._update([self._latch() for _ in range(3)])
|
||||
|
||||
def invalidate(self, reason):
|
||||
"""Stop using an uncertain mapping until a new startup check succeeds."""
|
||||
self.frame_clock = None
|
||||
self.next_refresh = 0.
|
||||
self._write(dict(kind="camera_clock_invalidated", reason=str(reason),
|
||||
last_stamp_ns=self.last_stamp_ns))
|
||||
if self.journal is not None:
|
||||
self.journal.flush()
|
||||
|
||||
def timestamp(self, info, received_ns):
|
||||
if self.frame_clock is None:
|
||||
raise ValueError("camera frame has no synchronized clock")
|
||||
tick = (int(info.nDevTimeStampHigh)<<32)|int(info.nDevTimeStampLow)
|
||||
# Auto-exposure preview uses the exact exposure-start timestamp when
|
||||
# the SDK supplies no per-frame duration. Calibration fixes exposure.
|
||||
measured_exposure = float(info.fExposureTime)
|
||||
exposure = measured_exposure if measured_exposure > 0 else self.exposure_us
|
||||
stamp = self.frame_clock.timestamp(tick, received_ns=received_ns, exposure_us=exposure)
|
||||
if self.last_stamp_ns is not None and stamp <= self.last_stamp_ns:
|
||||
raise ValueError("camera exposure time is not increasing after clock synchronization")
|
||||
self.last_stamp_ns = stamp
|
||||
self._write(dict(kind="camera_frame_time", frame_number=int(info.nFrameNum),
|
||||
device_tick=tick, sdk_host_stamp_ms=int(info.nHostTimeStamp),
|
||||
received_ns=received_ns, stamp_ns=stamp, exposure_us=exposure,
|
||||
timestamp_reference="exposure_midpoint" if exposure else "exposure_start"))
|
||||
return stamp
|
||||
|
||||
def close(self):
|
||||
if self.journal is not None:
|
||||
self.journal.close()
|
||||
self.journal = None
|
||||
@@ -0,0 +1,9 @@
|
||||
setup: /home/huangwen/calibration/linkerhand_retarget_ros2/install/setup.bash
|
||||
frame_rate: 30.0
|
||||
exposure_time_us: 5000.0
|
||||
tag_size_m: 0.016
|
||||
fixture_plate:
|
||||
camera: top
|
||||
tag_ids: [20, 21, 28, 29]
|
||||
x_from: 29
|
||||
x_to: 21
|
||||
@@ -0,0 +1,32 @@
|
||||
<?xml version="1.0" encoding="UTF-8" ?>
|
||||
<dds>
|
||||
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
|
||||
<transport_descriptors>
|
||||
<transport_descriptor>
|
||||
<transport_id>g20_udp_transport</transport_id>
|
||||
<type>UDPv4</type>
|
||||
<sendBufferSize>10485760</sendBufferSize>
|
||||
<receiveBufferSize>10485760</receiveBufferSize>
|
||||
</transport_descriptor>
|
||||
<transport_descriptor>
|
||||
<transport_id>g20_shm_transport</transport_id>
|
||||
<type>SHM</type>
|
||||
<segment_size>67108864</segment_size>
|
||||
<port_queue_capacity>512</port_queue_capacity>
|
||||
<healthy_check_timeout_ms>1000</healthy_check_timeout_ms>
|
||||
</transport_descriptor>
|
||||
</transport_descriptors>
|
||||
|
||||
<participant
|
||||
profile_name="g20_large_image_participant"
|
||||
is_default_profile="true">
|
||||
<rtps>
|
||||
<userTransports>
|
||||
<transport_id>g20_udp_transport</transport_id>
|
||||
<transport_id>g20_shm_transport</transport_id>
|
||||
</userTransports>
|
||||
<useBuiltinTransports>false</useBuiltinTransports>
|
||||
</rtps>
|
||||
</participant>
|
||||
</profiles>
|
||||
</dds>
|
||||
@@ -0,0 +1,150 @@
|
||||
"""Three-view compatibility policy over generic camera extrinsics."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from .extrinsics_geometry import (
|
||||
CameraCalibrationIdentity,
|
||||
CameraExtrinsics,
|
||||
camera_info_fingerprint,
|
||||
load_camera_extrinsics,
|
||||
matrix_payload,
|
||||
transform_matrix,
|
||||
validate_camera_extrinsics_payload,
|
||||
)
|
||||
|
||||
|
||||
VIEWS: tuple[str, ...] = ("front", "side", "top")
|
||||
_QUALITY_LIMITS = {
|
||||
"reprojection_rms_px": 1.2,
|
||||
"maximum_rotation_repeatability_deg": 0.3,
|
||||
"maximum_translation_repeatability_m": 0.0015,
|
||||
}
|
||||
_MINIMUM_CAPTURE_COUNTS = {
|
||||
"front_side_captures": 15,
|
||||
"front_top_captures": 15,
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ThreeCameraExtrinsics:
|
||||
"""Transforms points from each camera optical frame into front optical."""
|
||||
|
||||
cameras: Mapping[str, CameraCalibrationIdentity]
|
||||
front_from_view: Mapping[str, np.ndarray]
|
||||
quality: Mapping[str, float]
|
||||
|
||||
def transform(self, view: str) -> np.ndarray:
|
||||
if view not in self.front_from_view:
|
||||
raise KeyError(f"extrinsics do not contain view {view}")
|
||||
return np.asarray(self.front_from_view[view], dtype=float).copy()
|
||||
|
||||
def camera_matches(
|
||||
self,
|
||||
view: str,
|
||||
*,
|
||||
serial_number: str,
|
||||
width: int,
|
||||
height: int,
|
||||
intrinsics_sha256: str,
|
||||
) -> bool:
|
||||
expected = self.cameras.get(view)
|
||||
return bool(
|
||||
expected is not None
|
||||
and expected.serial_number == str(serial_number)
|
||||
and expected.width == int(width)
|
||||
and expected.height == int(height)
|
||||
and expected.intrinsics_sha256 == str(intrinsics_sha256)
|
||||
)
|
||||
|
||||
|
||||
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
|
||||
validate_camera_extrinsics_payload(
|
||||
payload,
|
||||
required_views=VIEWS,
|
||||
reference_view="front",
|
||||
quality_limits=_QUALITY_LIMITS,
|
||||
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
|
||||
)
|
||||
|
||||
|
||||
def load_three_camera_extrinsics(
|
||||
path: str | Path,
|
||||
*,
|
||||
quality_limits: Mapping[str, float] | None = None,
|
||||
minimum_capture_counts: Mapping[str, int] | None = None,
|
||||
) -> ThreeCameraExtrinsics:
|
||||
generic = load_camera_extrinsics(
|
||||
path,
|
||||
required_views=VIEWS,
|
||||
reference_view="front",
|
||||
quality_limits=(
|
||||
_QUALITY_LIMITS if quality_limits is None else quality_limits
|
||||
),
|
||||
minimum_capture_counts=(
|
||||
_MINIMUM_CAPTURE_COUNTS
|
||||
if minimum_capture_counts is None
|
||||
else minimum_capture_counts
|
||||
),
|
||||
)
|
||||
return ThreeCameraExtrinsics(
|
||||
cameras=generic.cameras,
|
||||
front_from_view=generic.reference_from_view,
|
||||
quality=generic.quality,
|
||||
)
|
||||
|
||||
|
||||
def dump_three_camera_extrinsics(
|
||||
path: str | Path,
|
||||
*,
|
||||
cameras: Mapping[str, Mapping[str, Any]],
|
||||
front_from_view: Mapping[str, Sequence[Sequence[float]]],
|
||||
quality: Mapping[str, Any],
|
||||
quality_limits: Mapping[str, float] | None = None,
|
||||
) -> None:
|
||||
payload = {
|
||||
"schema_version": 1,
|
||||
"reference_view": "front",
|
||||
"cameras": {view: dict(cameras[view]) for view in VIEWS},
|
||||
"front_from_view": {
|
||||
view: matrix_payload(front_from_view[view]) for view in VIEWS
|
||||
},
|
||||
"quality": dict(quality),
|
||||
}
|
||||
validate_camera_extrinsics_payload(
|
||||
payload,
|
||||
required_views=VIEWS,
|
||||
reference_view="front",
|
||||
quality_limits=(
|
||||
_QUALITY_LIMITS if quality_limits is None else quality_limits
|
||||
),
|
||||
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
|
||||
)
|
||||
destination = Path(path).expanduser().resolve()
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary = destination.with_suffix(destination.suffix + ".tmp")
|
||||
with temporary.open("w", encoding="utf-8") as stream:
|
||||
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
|
||||
temporary.replace(destination)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CameraCalibrationIdentity",
|
||||
"CameraExtrinsics",
|
||||
"ThreeCameraExtrinsics",
|
||||
"VIEWS",
|
||||
"camera_info_fingerprint",
|
||||
"dump_three_camera_extrinsics",
|
||||
"load_camera_extrinsics",
|
||||
"load_three_camera_extrinsics",
|
||||
"matrix_payload",
|
||||
"transform_matrix",
|
||||
"validate_camera_extrinsics_payload",
|
||||
"validate_extrinsics_payload",
|
||||
]
|
||||
@@ -0,0 +1,212 @@
|
||||
"""Camera-extrinsic data model shared by calibration and runtime nodes."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def camera_info_fingerprint(
|
||||
*,
|
||||
width: int,
|
||||
height: int,
|
||||
camera_matrix: Sequence[Sequence[float]] | Sequence[float],
|
||||
distortion: Sequence[float] = (),
|
||||
rectification: Sequence[float] = (),
|
||||
projection: Sequence[float] = (),
|
||||
) -> str:
|
||||
"""Return a stable fingerprint for rectified image geometry."""
|
||||
matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3)
|
||||
payload = {
|
||||
"width": int(width),
|
||||
"height": int(height),
|
||||
"camera_matrix": [round(float(value), 12) for value in matrix.flat],
|
||||
"distortion": [round(float(value), 12) for value in distortion],
|
||||
"rectification": [round(float(value), 12) for value in rectification],
|
||||
"projection": [round(float(value), 12) for value in projection],
|
||||
}
|
||||
encoded = json.dumps(
|
||||
payload, sort_keys=True, separators=(",", ":")
|
||||
).encode("utf-8")
|
||||
return hashlib.sha256(encoded).hexdigest()
|
||||
|
||||
|
||||
def transform_matrix(
|
||||
translation_xyz_m: Sequence[float],
|
||||
quaternion_xyzw: Sequence[float],
|
||||
) -> np.ndarray:
|
||||
translation = np.asarray(translation_xyz_m, dtype=float)
|
||||
quaternion = np.asarray(quaternion_xyzw, dtype=float)
|
||||
if translation.shape != (3,) or not np.all(np.isfinite(translation)):
|
||||
raise ValueError("translation_xyz_m must contain three finite values")
|
||||
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
|
||||
raise ValueError("quaternion_xyzw must contain four finite values")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if norm < 1.0e-12:
|
||||
raise ValueError("quaternion_xyzw has zero norm")
|
||||
result = np.eye(4, dtype=float)
|
||||
result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix()
|
||||
result[:3, 3] = translation
|
||||
return result
|
||||
|
||||
|
||||
def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]:
|
||||
value = np.asarray(matrix, dtype=float)
|
||||
if value.shape != (4, 4) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("transform must be a finite 4x4 matrix")
|
||||
return {
|
||||
"translation_xyz_m": [float(item) for item in value[:3, 3]],
|
||||
"quaternion_xyzw": [
|
||||
float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat()
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraCalibrationIdentity:
|
||||
serial_number: str
|
||||
width: int
|
||||
height: int
|
||||
intrinsics_sha256: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraExtrinsics:
|
||||
"""Transforms every declared camera into one Profile-selected reference."""
|
||||
|
||||
cameras: Mapping[str, CameraCalibrationIdentity]
|
||||
reference_view: str
|
||||
reference_from_view: Mapping[str, np.ndarray]
|
||||
quality: Mapping[str, float]
|
||||
|
||||
def transform(self, view: str) -> np.ndarray:
|
||||
if view not in self.reference_from_view:
|
||||
raise KeyError(f"extrinsics do not contain view {view}")
|
||||
return np.asarray(self.reference_from_view[view], dtype=float).copy()
|
||||
|
||||
|
||||
def validate_camera_extrinsics_payload(
|
||||
payload: Mapping[str, Any],
|
||||
*,
|
||||
required_views: Sequence[str],
|
||||
reference_view: str,
|
||||
quality_limits: Mapping[str, float] | None = None,
|
||||
minimum_capture_counts: Mapping[str, int] | None = None,
|
||||
) -> None:
|
||||
if int(payload.get("schema_version", -1)) != 1:
|
||||
raise ValueError("camera extrinsics schema_version must be 1")
|
||||
reference = str(reference_view)
|
||||
if payload.get("reference_view") != reference:
|
||||
raise ValueError(
|
||||
"camera extrinsics reference_view differs from the Profile"
|
||||
)
|
||||
cameras = payload.get("cameras")
|
||||
transforms = payload.get(f"{reference}_from_view")
|
||||
quality = payload.get("quality")
|
||||
views = tuple(str(view) for view in required_views)
|
||||
if not views or len(set(views)) != len(views):
|
||||
raise ValueError("required extrinsic views must be non-empty and unique")
|
||||
if reference not in views:
|
||||
raise ValueError("extrinsic reference view is not required")
|
||||
if not isinstance(cameras, Mapping) or set(cameras) != set(views):
|
||||
raise ValueError("camera extrinsics differ from the Profile views")
|
||||
if not isinstance(transforms, Mapping) or set(transforms) != set(views):
|
||||
raise ValueError("camera transforms differ from the Profile views")
|
||||
if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
|
||||
raise ValueError("camera extrinsics quality is not passed")
|
||||
for key, limit in dict(quality_limits or {}).items():
|
||||
value = float(quality.get(key, float("inf")))
|
||||
if not np.isfinite(value) or value > limit:
|
||||
raise ValueError(
|
||||
f"camera extrinsics {key}={value} exceeds {limit}"
|
||||
)
|
||||
for key, minimum in dict(minimum_capture_counts or {}).items():
|
||||
if int(quality.get(key, 0)) < int(minimum):
|
||||
raise ValueError(
|
||||
f"camera extrinsics {key} must be at least {minimum}"
|
||||
)
|
||||
for view in views:
|
||||
identity = cameras[view]
|
||||
if not isinstance(identity, Mapping):
|
||||
raise ValueError(f"{view} camera identity must be an object")
|
||||
if not str(identity.get("serial_number", "")):
|
||||
raise ValueError(f"{view} camera serial_number is missing")
|
||||
if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0:
|
||||
raise ValueError(f"{view} camera image dimensions are invalid")
|
||||
fingerprint = str(identity.get("intrinsics_sha256", ""))
|
||||
if len(fingerprint) != 64:
|
||||
raise ValueError(f"{view} camera intrinsics fingerprint is invalid")
|
||||
transform = transforms[view]
|
||||
if not isinstance(transform, Mapping):
|
||||
raise ValueError(f"{view} transform must be an object")
|
||||
matrix = transform_matrix(
|
||||
transform.get("translation_xyz_m", ()),
|
||||
transform.get("quaternion_xyzw", ()),
|
||||
)
|
||||
if view == reference and not np.allclose(
|
||||
matrix, np.eye(4), atol=1.0e-9
|
||||
):
|
||||
raise ValueError("reference-view transform must be identity")
|
||||
serials = [str(cameras[view]["serial_number"]) for view in views]
|
||||
if len(set(serials)) != len(views):
|
||||
raise ValueError("camera extrinsics serial numbers must be unique")
|
||||
|
||||
|
||||
def load_camera_extrinsics(
|
||||
path: str | Path,
|
||||
*,
|
||||
required_views: Sequence[str],
|
||||
reference_view: str,
|
||||
quality_limits: Mapping[str, float] | None = None,
|
||||
minimum_capture_counts: Mapping[str, int] | None = None,
|
||||
) -> CameraExtrinsics:
|
||||
source = Path(path).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
raise ValueError(f"camera extrinsics file does not exist: {source}")
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
payload = yaml.safe_load(stream)
|
||||
if not isinstance(payload, Mapping):
|
||||
raise ValueError("camera extrinsics file must contain an object")
|
||||
validate_camera_extrinsics_payload(
|
||||
payload,
|
||||
required_views=required_views,
|
||||
reference_view=reference_view,
|
||||
quality_limits=quality_limits,
|
||||
minimum_capture_counts=minimum_capture_counts,
|
||||
)
|
||||
views = tuple(str(view) for view in required_views)
|
||||
transform_key = f"{reference_view}_from_view"
|
||||
cameras = {
|
||||
view: CameraCalibrationIdentity(
|
||||
serial_number=str(payload["cameras"][view]["serial_number"]),
|
||||
width=int(payload["cameras"][view]["width"]),
|
||||
height=int(payload["cameras"][view]["height"]),
|
||||
intrinsics_sha256=str(
|
||||
payload["cameras"][view]["intrinsics_sha256"]
|
||||
),
|
||||
)
|
||||
for view in views
|
||||
}
|
||||
transforms = {
|
||||
view: transform_matrix(
|
||||
payload[transform_key][view]["translation_xyz_m"],
|
||||
payload[transform_key][view]["quaternion_xyzw"],
|
||||
)
|
||||
for view in views
|
||||
}
|
||||
return CameraExtrinsics(
|
||||
cameras=cameras,
|
||||
reference_view=str(reference_view),
|
||||
reference_from_view=transforms,
|
||||
quality={
|
||||
str(key): float(value) if isinstance(value, (int, float)) else value
|
||||
for key, value in payload["quality"].items()
|
||||
},
|
||||
)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,734 @@
|
||||
"""ROS 2 image publisher for Hikrobot MVS USB3 Vision cameras."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from ctypes import byref, cast, memset, POINTER, sizeof, string_at
|
||||
from dataclasses import dataclass
|
||||
import importlib
|
||||
import os
|
||||
from pathlib import Path
|
||||
import sys
|
||||
from threading import Event, Lock, Thread
|
||||
from typing import Any, Sequence
|
||||
from urllib.parse import unquote, urlparse
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from sensor_msgs.srv import SetCameraInfo
|
||||
import yaml
|
||||
|
||||
from .output_paths import intrinsic_output_path, latest_intrinsic_path, resolve_workspace
|
||||
|
||||
|
||||
DEFAULT_MVS_PYTHON_PATH = "/opt/MVS/Samples/64/Python/MvImport"
|
||||
DEFAULT_MVS_RUNTIME_PATH = "/opt/MVS/lib"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeviceDescriptor:
|
||||
"""Stable identity fields for one enumerated USB3 Vision camera."""
|
||||
|
||||
index: int
|
||||
model: str
|
||||
serial: str
|
||||
guid: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraCalibration:
|
||||
"""Validated subset of a standard ROS camera calibration YAML file."""
|
||||
|
||||
camera_name: str
|
||||
width: int
|
||||
height: int
|
||||
distortion_model: str
|
||||
d: tuple[float, ...]
|
||||
k: tuple[float, ...]
|
||||
r: tuple[float, ...]
|
||||
p: tuple[float, ...]
|
||||
|
||||
|
||||
def decode_c_string(value: Any) -> str:
|
||||
"""Decode a null-terminated MVS SDK character array."""
|
||||
raw = bytes(value).split(b"\0", 1)[0]
|
||||
for encoding in ("utf-8", "gbk", "latin-1"):
|
||||
try:
|
||||
return raw.decode(encoding)
|
||||
except UnicodeDecodeError:
|
||||
continue
|
||||
return raw.decode("latin-1", errors="replace")
|
||||
|
||||
|
||||
def select_device(
|
||||
devices: Sequence[DeviceDescriptor],
|
||||
selector: str,
|
||||
expected_model: str,
|
||||
) -> DeviceDescriptor:
|
||||
"""Select exactly one camera by serial or GUID and check its model."""
|
||||
if not devices:
|
||||
raise RuntimeError("MVS did not enumerate any USB3 Vision cameras")
|
||||
selector = selector.strip()
|
||||
if selector:
|
||||
matches = [
|
||||
device
|
||||
for device in devices
|
||||
if selector in {device.serial, device.guid}
|
||||
]
|
||||
if not matches:
|
||||
available = ", ".join(
|
||||
f"{device.model}:{device.serial}:{device.guid}"
|
||||
for device in devices
|
||||
)
|
||||
raise RuntimeError(
|
||||
f"camera selector {selector!r} was not found; "
|
||||
f"available cameras: {available}"
|
||||
)
|
||||
elif len(devices) == 1:
|
||||
matches = [devices[0]]
|
||||
else:
|
||||
available = ", ".join(device.serial for device in devices)
|
||||
raise RuntimeError(
|
||||
"camera selector is required when multiple cameras are present; "
|
||||
f"available serials: {available}"
|
||||
)
|
||||
if len(matches) != 1:
|
||||
raise RuntimeError(f"camera selector {selector!r} is not unique")
|
||||
selected = matches[0]
|
||||
if expected_model and expected_model not in selected.model:
|
||||
raise RuntimeError(
|
||||
f"camera {selected.serial} is {selected.model!r}, expected a model "
|
||||
f"containing {expected_model!r}"
|
||||
)
|
||||
return selected
|
||||
|
||||
|
||||
def resolve_camera_info_path(url_or_path: str) -> Path | None:
|
||||
"""Resolve a plain path or file:// URL used for CameraInfo persistence."""
|
||||
value = url_or_path.strip()
|
||||
if not value:
|
||||
return None
|
||||
parsed = urlparse(value)
|
||||
if parsed.scheme not in {"", "file"}:
|
||||
raise ValueError(
|
||||
"camera_info_url must be a filesystem path or file:// URL"
|
||||
)
|
||||
if parsed.scheme == "file":
|
||||
if parsed.netloc not in {"", "localhost"}:
|
||||
raise ValueError("remote file:// camera_info_url is not supported")
|
||||
value = unquote(parsed.path)
|
||||
return Path(value).expanduser().resolve()
|
||||
|
||||
|
||||
def _matrix_data(mapping: dict[str, Any], name: str, length: int) -> tuple[float, ...]:
|
||||
section = mapping.get(name)
|
||||
if not isinstance(section, dict):
|
||||
raise ValueError(f"camera calibration is missing {name}")
|
||||
data = section.get("data")
|
||||
if not isinstance(data, list) or len(data) != length:
|
||||
raise ValueError(f"{name}.data must contain exactly {length} values")
|
||||
result = tuple(float(value) for value in data)
|
||||
if not all(value == value and abs(value) != float("inf") for value in result):
|
||||
raise ValueError(f"{name}.data contains a non-finite value")
|
||||
return result
|
||||
|
||||
|
||||
def load_camera_calibration(path: Path) -> CameraCalibration:
|
||||
"""Load and validate a standard ROS camera calibration YAML file."""
|
||||
mapping = yaml.safe_load(path.read_text(encoding="utf-8"))
|
||||
if not isinstance(mapping, dict):
|
||||
raise ValueError("camera calibration YAML root must be a mapping")
|
||||
width = int(mapping.get("image_width", 0))
|
||||
height = int(mapping.get("image_height", 0))
|
||||
if width <= 0 or height <= 0:
|
||||
raise ValueError("camera calibration image dimensions must be positive")
|
||||
distortion = mapping.get("distortion_coefficients")
|
||||
if not isinstance(distortion, dict) or not isinstance(
|
||||
distortion.get("data"), list
|
||||
):
|
||||
raise ValueError(
|
||||
"camera calibration is missing distortion_coefficients.data"
|
||||
)
|
||||
d = tuple(float(value) for value in distortion["data"])
|
||||
calibration = CameraCalibration(
|
||||
camera_name=str(mapping.get("camera_name", "hikrobot_camera")),
|
||||
width=width,
|
||||
height=height,
|
||||
distortion_model=str(mapping.get("distortion_model", "plumb_bob")),
|
||||
d=d,
|
||||
k=_matrix_data(mapping, "camera_matrix", 9),
|
||||
r=_matrix_data(mapping, "rectification_matrix", 9),
|
||||
p=_matrix_data(mapping, "projection_matrix", 12),
|
||||
)
|
||||
if calibration.k[0] <= 0.0 or calibration.k[4] <= 0.0:
|
||||
raise ValueError("camera calibration focal lengths must be positive")
|
||||
if calibration.p[0] <= 0.0 or calibration.p[5] <= 0.0:
|
||||
raise ValueError("camera calibration projection focal lengths must be positive")
|
||||
return calibration
|
||||
|
||||
|
||||
def calibration_to_mapping(
|
||||
camera_name: str, camera_info: CameraInfo
|
||||
) -> dict[str, Any]:
|
||||
"""Convert CameraInfo into the standard ROS YAML representation."""
|
||||
return {
|
||||
"image_width": int(camera_info.width),
|
||||
"image_height": int(camera_info.height),
|
||||
"camera_name": camera_name,
|
||||
"camera_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"data": [float(value) for value in camera_info.k],
|
||||
},
|
||||
"distortion_model": camera_info.distortion_model,
|
||||
"distortion_coefficients": {
|
||||
"rows": 1,
|
||||
"cols": len(camera_info.d),
|
||||
"data": [float(value) for value in camera_info.d],
|
||||
},
|
||||
"rectification_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"data": [float(value) for value in camera_info.r],
|
||||
},
|
||||
"projection_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 4,
|
||||
"data": [float(value) for value in camera_info.p],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def configure_fastdds_large_image_transport() -> Path:
|
||||
"""Select the package's large-image SHM profile before creating ROS nodes."""
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
profile = (
|
||||
Path(get_package_share_directory("camera_parameters"))
|
||||
/ "config"
|
||||
/ "fastdds_large_images.xml"
|
||||
)
|
||||
if not profile.is_file():
|
||||
raise RuntimeError(f"Fast DDS large-image profile not found: {profile}")
|
||||
os.environ.setdefault("FASTRTPS_DEFAULT_PROFILES_FILE", str(profile))
|
||||
return profile
|
||||
|
||||
|
||||
class HikrobotCameraNode(Node):
|
||||
"""Publish synchronized Mono8 Image and CameraInfo messages from MVS."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("hikrobot_camera")
|
||||
self._declare_parameters()
|
||||
self._stop_event = Event()
|
||||
self._camera_info_lock = Lock()
|
||||
self._grab_thread: Thread | None = None
|
||||
self._camera: Any | None = None
|
||||
self._mvs: Any | None = None
|
||||
self._sdk_initialized = False
|
||||
self._handle_created = False
|
||||
self._camera_opened = False
|
||||
self._grabbing = False
|
||||
self._closed = False
|
||||
self._frame_count = 0
|
||||
self._timeout_count = 0
|
||||
self._camera_timing = None
|
||||
|
||||
self.selector = str(self.get_parameter("serial_number").value)
|
||||
self.workspace = resolve_workspace(str(self.get_parameter("workspace").value))
|
||||
self.opened_serial = ""
|
||||
self.expected_model = str(self.get_parameter("expected_model").value)
|
||||
self.frame_id = str(self.get_parameter("frame_id").value)
|
||||
self.camera_name = str(self.get_parameter("camera_name").value)
|
||||
self.image_width = int(self.get_parameter("image_width").value)
|
||||
self.image_height = int(self.get_parameter("image_height").value)
|
||||
if self.image_width <= 0 or self.image_height <= 0:
|
||||
raise ValueError("image_width and image_height must be positive")
|
||||
if float(self.get_parameter("frame_rate").value) <= 0.0:
|
||||
raise ValueError("frame_rate must be positive")
|
||||
if int(self.get_parameter("grab_timeout_ms").value) <= 0:
|
||||
raise ValueError("grab_timeout_ms must be positive")
|
||||
self.camera_info_path = resolve_camera_info_path(
|
||||
str(self.get_parameter("camera_info_url").value)
|
||||
)
|
||||
self._calibration = (
|
||||
self._load_calibration() if self.camera_info_path is not None else None
|
||||
)
|
||||
|
||||
self.image_publisher = self.create_publisher(
|
||||
Image, "image_raw", qos_profile_sensor_data
|
||||
)
|
||||
self.camera_info_publisher = self.create_publisher(
|
||||
CameraInfo, "camera_info", qos_profile_sensor_data
|
||||
)
|
||||
self.create_service(
|
||||
SetCameraInfo, "set_camera_info", self._set_camera_info_callback
|
||||
)
|
||||
|
||||
try:
|
||||
self._open_camera()
|
||||
self._grab_thread = Thread(
|
||||
target=self._grab_loop,
|
||||
name="hikrobot_mvs_grab",
|
||||
daemon=True,
|
||||
)
|
||||
self._grab_thread.start()
|
||||
except Exception:
|
||||
self.close()
|
||||
raise
|
||||
|
||||
def _declare_parameters(self) -> None:
|
||||
self.declare_parameter("serial_number", "")
|
||||
self.declare_parameter("workspace", "")
|
||||
self.declare_parameter("expected_model", "MV-CS020-10UM")
|
||||
self.declare_parameter("camera_name", "hikrobot_front")
|
||||
self.declare_parameter("frame_id", "camera_color_optical_frame")
|
||||
self.declare_parameter("image_width", 1624)
|
||||
self.declare_parameter("image_height", 1240)
|
||||
self.declare_parameter("frame_rate", 30.0)
|
||||
self.declare_parameter("exposure_time_us", 5000.0)
|
||||
self.declare_parameter("gain_db", 0.0)
|
||||
self.declare_parameter("auto_exposure", False)
|
||||
self.declare_parameter("camera_info_url", "")
|
||||
self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH)
|
||||
self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH)
|
||||
self.declare_parameter("grab_timeout_ms", 1000)
|
||||
self.declare_parameter("timestamp_journal_path", "")
|
||||
|
||||
def _load_calibration(self) -> CameraCalibration | None:
|
||||
if self.camera_info_path is None or not self.camera_info_path.exists():
|
||||
target = self.camera_info_path or "an unset camera_info_url"
|
||||
self.get_logger().warning(
|
||||
f"No camera calibration found at {target}; publishing "
|
||||
"uncalibrated CameraInfo. Trajectory calibration will remain "
|
||||
"locked until valid intrinsics are installed."
|
||||
)
|
||||
return None
|
||||
calibration = load_camera_calibration(self.camera_info_path)
|
||||
self._validate_calibration_dimensions(calibration)
|
||||
self.get_logger().info(
|
||||
f"Loaded camera intrinsics from {self.camera_info_path}"
|
||||
)
|
||||
return calibration
|
||||
|
||||
def _validate_calibration_dimensions(
|
||||
self, calibration: CameraCalibration
|
||||
) -> None:
|
||||
if (
|
||||
calibration.width != self.image_width
|
||||
or calibration.height != self.image_height
|
||||
):
|
||||
raise ValueError(
|
||||
"camera calibration dimensions "
|
||||
f"{calibration.width}x{calibration.height} do not match "
|
||||
f"configured stream {self.image_width}x{self.image_height}"
|
||||
)
|
||||
|
||||
def _import_mvs(self) -> Any:
|
||||
python_path = Path(
|
||||
str(self.get_parameter("mvs_python_path").value)
|
||||
).expanduser()
|
||||
runtime_path = Path(
|
||||
str(self.get_parameter("mvs_runtime_path").value)
|
||||
).expanduser()
|
||||
if not python_path.is_dir():
|
||||
raise RuntimeError(f"MVS Python bindings not found: {python_path}")
|
||||
if not runtime_path.is_dir():
|
||||
raise RuntimeError(f"MVS runtime not found: {runtime_path}")
|
||||
os.environ.setdefault("MVCAM_COMMON_RUNENV", str(runtime_path))
|
||||
if str(python_path) not in sys.path:
|
||||
sys.path.insert(0, str(python_path))
|
||||
return importlib.import_module("MvCameraControl_class")
|
||||
|
||||
def _enumerate_usb_cameras(
|
||||
self, mvs: Any
|
||||
) -> tuple[Any, list[DeviceDescriptor]]:
|
||||
device_list = mvs.MV_CC_DEVICE_INFO_LIST()
|
||||
result = mvs.MvCamera.MV_CC_EnumDevices(
|
||||
mvs.MV_USB_DEVICE, device_list
|
||||
)
|
||||
self._require_ok("enumerate USB3 Vision devices", result)
|
||||
descriptors: list[DeviceDescriptor] = []
|
||||
for index in range(device_list.nDeviceNum):
|
||||
device_info = cast(
|
||||
device_list.pDeviceInfo[index], POINTER(mvs.MV_CC_DEVICE_INFO)
|
||||
).contents
|
||||
usb_info = device_info.SpecialInfo.stUsb3VInfo
|
||||
descriptors.append(
|
||||
DeviceDescriptor(
|
||||
index=index,
|
||||
model=decode_c_string(usb_info.chModelName),
|
||||
serial=decode_c_string(usb_info.chSerialNumber),
|
||||
guid=decode_c_string(usb_info.chDeviceGUID),
|
||||
)
|
||||
)
|
||||
return device_list, descriptors
|
||||
|
||||
def _open_camera(self) -> None:
|
||||
mvs = self._import_mvs()
|
||||
self._mvs = mvs
|
||||
self._require_ok("initialize MVS SDK", mvs.MvCamera.MV_CC_Initialize())
|
||||
self._sdk_initialized = True
|
||||
device_list, descriptors = self._enumerate_usb_cameras(mvs)
|
||||
selected = select_device(descriptors, self.selector, self.expected_model)
|
||||
self.opened_serial = selected.serial
|
||||
if self.camera_info_path is None:
|
||||
latest = latest_intrinsic_path(self.workspace, selected.serial)
|
||||
if latest is not None:
|
||||
self.camera_info_path = latest
|
||||
self._calibration = self._load_calibration()
|
||||
self.get_logger().info(
|
||||
f"Loaded latest intrinsics for {selected.serial} from {latest}"
|
||||
)
|
||||
device_info = cast(
|
||||
device_list.pDeviceInfo[selected.index],
|
||||
POINTER(mvs.MV_CC_DEVICE_INFO),
|
||||
).contents
|
||||
|
||||
camera = mvs.MvCamera()
|
||||
self._require_ok("create camera handle", camera.MV_CC_CreateHandle(device_info))
|
||||
self._camera = camera
|
||||
self._handle_created = True
|
||||
self._require_ok(
|
||||
"open camera",
|
||||
camera.MV_CC_OpenDevice(mvs.MV_ACCESS_Exclusive, 0),
|
||||
)
|
||||
self._camera_opened = True
|
||||
|
||||
self._set_required_enum("AcquisitionMode", "Continuous")
|
||||
self._set_required_enum("TriggerMode", "Off")
|
||||
self._set_required_enum("PixelFormat", "Mono8")
|
||||
self._set_required_integer("OffsetX", 0)
|
||||
self._set_required_integer("OffsetY", 0)
|
||||
self._set_required_integer("Width", self.image_width)
|
||||
self._set_required_integer("Height", self.image_height)
|
||||
self._set_optional_bool("AcquisitionFrameRateEnable", True)
|
||||
self._set_optional_float(
|
||||
"AcquisitionFrameRate",
|
||||
float(self.get_parameter("frame_rate").value),
|
||||
)
|
||||
if bool(self.get_parameter("auto_exposure").value):
|
||||
self._set_required_enum("ExposureAuto", "Continuous")
|
||||
else:
|
||||
self._set_required_enum("ExposureAuto", "Off")
|
||||
self._set_optional_float(
|
||||
"ExposureTime",
|
||||
float(self.get_parameter("exposure_time_us").value),
|
||||
)
|
||||
self._set_required_enum("GainAuto", "Off")
|
||||
self._set_optional_float(
|
||||
"Gain", float(self.get_parameter("gain_db").value)
|
||||
)
|
||||
|
||||
self._require_ok(
|
||||
"set latest-frame grab strategy",
|
||||
camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly),
|
||||
)
|
||||
from .camera_timing import MvsCameraTiming
|
||||
self._camera_timing = MvsCameraTiming(camera, mvs,
|
||||
lambda: int(self.get_clock().now().nanoseconds),
|
||||
exposure_us=0. if bool(self.get_parameter("auto_exposure").value)
|
||||
else float(self.get_parameter("exposure_time_us").value),
|
||||
journal_path=str(self.get_parameter("timestamp_journal_path").value))
|
||||
self._camera_timing.start()
|
||||
self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing())
|
||||
self._grabbing = True
|
||||
self.get_logger().info(
|
||||
"Opened Hikrobot camera "
|
||||
f"model={selected.model} serial={selected.serial} "
|
||||
f"guid={selected.guid} stream={self.image_width}x{self.image_height} "
|
||||
"encoding=mono8"
|
||||
)
|
||||
|
||||
def _set_required_enum(self, name: str, value: str) -> None:
|
||||
assert self._camera is not None
|
||||
self._require_ok(
|
||||
f"set {name}={value}",
|
||||
self._camera.MV_CC_SetEnumValueByString(name, value),
|
||||
)
|
||||
|
||||
def _set_required_integer(self, name: str, value: int) -> None:
|
||||
assert self._camera is not None
|
||||
self._require_ok(
|
||||
f"set {name}={value}",
|
||||
self._camera.MV_CC_SetIntValueEx(name, value),
|
||||
)
|
||||
|
||||
def _set_optional_bool(self, name: str, value: bool) -> None:
|
||||
assert self._camera is not None
|
||||
result = self._camera.MV_CC_SetBoolValue(name, value)
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS could not set {name}={value}: 0x{result:08x}"
|
||||
)
|
||||
|
||||
def _set_optional_float(self, name: str, value: float) -> None:
|
||||
assert self._camera is not None
|
||||
result = self._camera.MV_CC_SetFloatValue(name, value)
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS could not set {name}={value}: 0x{result:08x}"
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _require_ok(operation: str, result: int) -> None:
|
||||
if result != 0:
|
||||
hint = ""
|
||||
if result == 0x80000203:
|
||||
hint = (
|
||||
"; access denied: stop acquisition and close the MVS "
|
||||
"viewer or any other process using this camera"
|
||||
)
|
||||
raise RuntimeError(
|
||||
f"MVS failed to {operation}: 0x{result:08x}{hint}"
|
||||
)
|
||||
|
||||
def _grab_loop(self) -> None:
|
||||
assert self._camera is not None
|
||||
assert self._mvs is not None
|
||||
timeout_ms = int(self.get_parameter("grab_timeout_ms").value)
|
||||
recovering_clock = False
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
self._camera_timing.refresh_if_due()
|
||||
except ValueError as error:
|
||||
if not recovering_clock:
|
||||
self.get_logger().error(
|
||||
f"Camera timestamp source failed: {error}; "
|
||||
"pausing images and retrying clock synchronization"
|
||||
)
|
||||
self._camera_timing.invalidate(error)
|
||||
recovering_clock = True
|
||||
self._stop_event.wait(0.2)
|
||||
continue
|
||||
if recovering_clock:
|
||||
self.get_logger().info(
|
||||
"Camera clock synchronized again; resuming image acquisition"
|
||||
)
|
||||
recovering_clock = False
|
||||
frame = self._mvs.MV_FRAME_OUT()
|
||||
memset(byref(frame), 0, sizeof(frame))
|
||||
result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms)
|
||||
if result != 0:
|
||||
self._timeout_count += 1
|
||||
if self._timeout_count == 1 or self._timeout_count % 10 == 0:
|
||||
self.get_logger().warning(
|
||||
"MVS image acquisition timed out or failed: "
|
||||
f"0x{result:08x}; consecutive={self._timeout_count}"
|
||||
)
|
||||
continue
|
||||
try:
|
||||
self._timeout_count = 0
|
||||
self._publish_frame(frame)
|
||||
except Exception as error:
|
||||
self.get_logger().error(f"Failed to publish MVS frame: {error}")
|
||||
finally:
|
||||
free_result = self._camera.MV_CC_FreeImageBuffer(frame)
|
||||
if free_result != 0:
|
||||
self.get_logger().error(
|
||||
f"MVS failed to release frame: 0x{free_result:08x}"
|
||||
)
|
||||
|
||||
def _publish_frame(self, frame: Any) -> None:
|
||||
assert self._mvs is not None
|
||||
info = frame.stFrameInfo
|
||||
width = int(info.nWidth)
|
||||
height = int(info.nHeight)
|
||||
if width != self.image_width or height != self.image_height:
|
||||
raise RuntimeError(
|
||||
f"received {width}x{height}, expected "
|
||||
f"{self.image_width}x{self.image_height}"
|
||||
)
|
||||
if int(info.enPixelType) != int(self._mvs.PixelType_Gvsp_Mono8):
|
||||
raise RuntimeError(
|
||||
f"received pixel type 0x{int(info.enPixelType):x}, expected Mono8"
|
||||
)
|
||||
expected_size = width * height
|
||||
if int(info.nFrameLen) < expected_size:
|
||||
raise RuntimeError(
|
||||
f"frame contains {info.nFrameLen} bytes, expected {expected_size}"
|
||||
)
|
||||
from rclpy.time import Time
|
||||
received_ns = int(self.get_clock().now().nanoseconds)
|
||||
stamp = Time(nanoseconds=self._camera_timing.timestamp(info, received_ns)).to_msg()
|
||||
image = Image()
|
||||
image.header.stamp = stamp
|
||||
image.header.frame_id = self.frame_id
|
||||
image.height = height
|
||||
image.width = width
|
||||
image.encoding = "mono8"
|
||||
image.is_bigendian = 0
|
||||
image.step = width
|
||||
image.data = string_at(frame.pBufAddr, expected_size)
|
||||
|
||||
camera_info = self._make_camera_info(width, height)
|
||||
camera_info.header.stamp = stamp
|
||||
camera_info.header.frame_id = self.frame_id
|
||||
self.image_publisher.publish(image)
|
||||
self.camera_info_publisher.publish(camera_info)
|
||||
self._frame_count += 1
|
||||
|
||||
def _make_camera_info(self, width: int, height: int) -> CameraInfo:
|
||||
message = CameraInfo()
|
||||
message.width = width
|
||||
message.height = height
|
||||
with self._camera_info_lock:
|
||||
calibration = self._calibration
|
||||
if calibration is None:
|
||||
message.distortion_model = "plumb_bob"
|
||||
message.d = []
|
||||
message.k = [0.0] * 9
|
||||
message.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||
message.p = [0.0] * 12
|
||||
return message
|
||||
message.distortion_model = calibration.distortion_model
|
||||
message.d = list(calibration.d)
|
||||
message.k = list(calibration.k)
|
||||
message.r = list(calibration.r)
|
||||
message.p = list(calibration.p)
|
||||
return message
|
||||
|
||||
def _set_camera_info_callback(
|
||||
self,
|
||||
request: SetCameraInfo.Request,
|
||||
response: SetCameraInfo.Response,
|
||||
) -> SetCameraInfo.Response:
|
||||
try:
|
||||
if not self.opened_serial:
|
||||
raise ValueError("camera serial is unknown; open the camera before saving")
|
||||
message = request.camera_info
|
||||
if (
|
||||
int(message.width) != self.image_width
|
||||
or int(message.height) != self.image_height
|
||||
):
|
||||
raise ValueError(
|
||||
f"calibration is {message.width}x{message.height}, expected "
|
||||
f"{self.image_width}x{self.image_height}"
|
||||
)
|
||||
mapping = calibration_to_mapping(
|
||||
f"hikrobot_{self.opened_serial}", message
|
||||
)
|
||||
self.camera_info_path = intrinsic_output_path(
|
||||
self.workspace, self.opened_serial
|
||||
)
|
||||
self.camera_info_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary_path = self.camera_info_path.with_suffix(
|
||||
self.camera_info_path.suffix + ".tmp"
|
||||
)
|
||||
temporary_path.write_text(
|
||||
yaml.safe_dump(mapping, sort_keys=False), encoding="utf-8"
|
||||
)
|
||||
temporary_path.replace(self.camera_info_path)
|
||||
calibration = load_camera_calibration(self.camera_info_path)
|
||||
self._validate_calibration_dimensions(calibration)
|
||||
with self._camera_info_lock:
|
||||
self._calibration = calibration
|
||||
response.success = True
|
||||
response.status_message = (
|
||||
f"saved camera calibration to {self.camera_info_path}"
|
||||
)
|
||||
self.get_logger().info(response.status_message)
|
||||
except Exception as error:
|
||||
response.success = False
|
||||
response.status_message = str(error)
|
||||
self.get_logger().error(
|
||||
f"Rejected camera calibration: {response.status_message}"
|
||||
)
|
||||
return response
|
||||
|
||||
def close(self) -> None:
|
||||
if self._closed:
|
||||
return
|
||||
self._closed = True
|
||||
self._stop_event.set()
|
||||
if self._grab_thread is not None:
|
||||
self._grab_thread.join(timeout=2.0)
|
||||
if self._camera_timing is not None:
|
||||
self._camera_timing.close()
|
||||
if self._camera is not None and self._camera_opened:
|
||||
if self._grabbing:
|
||||
result = self._camera.MV_CC_StopGrabbing()
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS failed to stop grabbing: 0x{result:08x}"
|
||||
)
|
||||
self._grabbing = False
|
||||
result = self._camera.MV_CC_CloseDevice()
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS failed to close camera: 0x{result:08x}"
|
||||
)
|
||||
self._camera.MV_CC_DestroyHandle()
|
||||
elif self._camera is not None and self._handle_created:
|
||||
self._camera.MV_CC_DestroyHandle()
|
||||
self._camera = None
|
||||
self._handle_created = False
|
||||
self._camera_opened = False
|
||||
if self._sdk_initialized and self._mvs is not None:
|
||||
self._mvs.MvCamera.MV_CC_Finalize()
|
||||
self._sdk_initialized = False
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self.close()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def enumerate_usb_serials(
|
||||
mvs_python_path: str = DEFAULT_MVS_PYTHON_PATH,
|
||||
mvs_runtime_path: str = DEFAULT_MVS_RUNTIME_PATH,
|
||||
) -> list[str]:
|
||||
"""列出当前接上的 USB3 相机序列号。只枚举,不打开设备。"""
|
||||
python_path = Path(mvs_python_path).expanduser()
|
||||
runtime_path = Path(mvs_runtime_path).expanduser()
|
||||
if not python_path.is_dir():
|
||||
raise RuntimeError(f"MVS Python bindings not found: {python_path}")
|
||||
if not runtime_path.is_dir():
|
||||
raise RuntimeError(f"MVS runtime not found: {runtime_path}")
|
||||
os.environ.setdefault("MVCAM_COMMON_RUNENV", str(runtime_path))
|
||||
if str(python_path) not in sys.path:
|
||||
sys.path.insert(0, str(python_path))
|
||||
mvs = importlib.import_module("MvCameraControl_class")
|
||||
initialized = False
|
||||
try:
|
||||
result = mvs.MvCamera.MV_CC_Initialize()
|
||||
if result != 0:
|
||||
raise RuntimeError(f"initialize MVS SDK failed: 0x{result:08x}")
|
||||
initialized = True
|
||||
device_list = mvs.MV_CC_DEVICE_INFO_LIST()
|
||||
result = mvs.MvCamera.MV_CC_EnumDevices(mvs.MV_USB_DEVICE, device_list)
|
||||
if result != 0:
|
||||
raise RuntimeError(f"enumerate USB3 Vision devices failed: 0x{result:08x}")
|
||||
serials = []
|
||||
for index in range(device_list.nDeviceNum):
|
||||
device_info = cast(
|
||||
device_list.pDeviceInfo[index], POINTER(mvs.MV_CC_DEVICE_INFO)
|
||||
).contents
|
||||
usb_info = device_info.SpecialInfo.stUsb3VInfo
|
||||
serial = decode_c_string(usb_info.chSerialNumber)
|
||||
if serial:
|
||||
serials.append(serial)
|
||||
return serials
|
||||
finally:
|
||||
if initialized:
|
||||
mvs.MvCamera.MV_CC_Finalize()
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
configure_fastdds_large_image_transport()
|
||||
rclpy.init(args=args)
|
||||
node: HikrobotCameraNode | None = None
|
||||
try:
|
||||
node = HikrobotCameraNode()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,236 @@
|
||||
"""打开所有相机,在界面里点哪一台就启动那一台的内参标定。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import subprocess
|
||||
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
import numpy as np
|
||||
from PIL import Image, ImageDraw, ImageFont
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image as ImageMessage
|
||||
|
||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
||||
from .output_paths import camera_namespace
|
||||
|
||||
|
||||
_FONT_CANDIDATES = (
|
||||
"/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc",
|
||||
"/usr/share/fonts/truetype/droid/DroidSansFallbackFull.ttf",
|
||||
)
|
||||
|
||||
|
||||
def calibrator_command(
|
||||
serial: str, columns: int, rows: int, square_size_m: float
|
||||
) -> list[str]:
|
||||
"""官方标定器订这一台相机的原始图像,SAVE 会写到该相机的 set_camera_info。"""
|
||||
parent = camera_namespace(serial).rsplit("/", 1)[0]
|
||||
return [
|
||||
"ros2", "run", "camera_calibration", "cameracalibrator",
|
||||
"--size", f"{columns}x{rows}",
|
||||
"--square", f"{square_size_m}",
|
||||
"--camera_name", f"hikrobot_{serial}",
|
||||
"--ros-args",
|
||||
"-r", f"__ns:={parent}",
|
||||
"-r", "image:=camera/image_raw",
|
||||
]
|
||||
|
||||
|
||||
def _font(size: int) -> ImageFont.ImageFont:
|
||||
for path in _FONT_CANDIDATES:
|
||||
if Path(path).is_file():
|
||||
return ImageFont.truetype(path, size)
|
||||
return ImageFont.load_default()
|
||||
|
||||
|
||||
def window_closed(name: str) -> bool:
|
||||
"""用户点了窗口关闭。最小化时窗口还在,不当成退出。"""
|
||||
try:
|
||||
return cv2.getWindowProperty(name, cv2.WND_PROP_VISIBLE) < 0
|
||||
except cv2.error:
|
||||
return True
|
||||
|
||||
|
||||
def _draw_text(
|
||||
canvas: np.ndarray,
|
||||
text: str,
|
||||
origin: tuple[int, int],
|
||||
size: int,
|
||||
color: tuple[int, int, int],
|
||||
) -> None:
|
||||
rgb = cv2.cvtColor(canvas, cv2.COLOR_BGR2RGB)
|
||||
image = Image.fromarray(rgb)
|
||||
red, green, blue = color
|
||||
ImageDraw.Draw(image).text(origin, text, font=_font(size), fill=(blue, green, red))
|
||||
canvas[:] = cv2.cvtColor(np.asarray(image), cv2.COLOR_RGB2BGR)
|
||||
|
||||
|
||||
class IntrinsicCalibrationNode(Node):
|
||||
GUI_WINDOW = "Camera Intrinsics"
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("camera_intrinsics")
|
||||
self.declare_parameter("camera_serials", "")
|
||||
self.declare_parameter("checkerboard_columns", 8)
|
||||
self.declare_parameter("checkerboard_rows", 5)
|
||||
self.declare_parameter("square_size_m", 0.027)
|
||||
self.declare_parameter("gui_refresh_hz", 2.0)
|
||||
self.camera_serials = [
|
||||
item.strip()
|
||||
for item in str(self.get_parameter("camera_serials").value).split(",")
|
||||
if item.strip()
|
||||
]
|
||||
self.columns = int(self.get_parameter("checkerboard_columns").value)
|
||||
self.rows = int(self.get_parameter("checkerboard_rows").value)
|
||||
self.square_size_m = float(self.get_parameter("square_size_m").value)
|
||||
refresh = float(self.get_parameter("gui_refresh_hz").value)
|
||||
if len(self.camera_serials) < 1:
|
||||
raise ValueError("camera_serials must list at least one camera")
|
||||
if len(set(self.camera_serials)) != len(self.camera_serials):
|
||||
raise ValueError("camera_serials contains a duplicate")
|
||||
if self.columns < 3 or self.rows < 3 or self.square_size_m <= 0.0:
|
||||
raise ValueError("checkerboard geometry is invalid")
|
||||
if refresh <= 0.0 or refresh > 10.0:
|
||||
raise ValueError("gui_refresh_hz must be in (0, 10]")
|
||||
self.bridge = CvBridge()
|
||||
self.latest: dict[str, ImageMessage] = {}
|
||||
self.buttons: dict[str, tuple[int, int, int, int]] = {}
|
||||
self.calibrator: subprocess.Popen | None = None
|
||||
self.calibrating_serial = ""
|
||||
self._closing = False
|
||||
self.feedback = "点一台相机开始标定"
|
||||
for serial in self.camera_serials:
|
||||
self.create_subscription(
|
||||
ImageMessage,
|
||||
f"{camera_namespace(serial)}/image_raw",
|
||||
lambda message, selected=serial: self.latest.__setitem__(selected, message),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
if not os.environ.get("DISPLAY"):
|
||||
raise RuntimeError("内参标定需要图形界面")
|
||||
cv2.namedWindow(self.GUI_WINDOW, cv2.WINDOW_NORMAL)
|
||||
cv2.resizeWindow(self.GUI_WINDOW, 1400, 760)
|
||||
cv2.setMouseCallback(self.GUI_WINDOW, self._mouse)
|
||||
self.create_timer(1.0 / refresh, self._draw)
|
||||
|
||||
def _mouse(self, event, x, y, flags, parameter) -> None:
|
||||
del flags, parameter
|
||||
if event != cv2.EVENT_LBUTTONUP:
|
||||
return
|
||||
for name, rectangle in self.buttons.items():
|
||||
x1, y1, x2, y2 = rectangle
|
||||
if x1 <= x <= x2 and y1 <= y <= y2 and name.startswith("calibrate:"):
|
||||
self._start(name.split(":", 1)[1])
|
||||
return
|
||||
|
||||
def _start(self, serial: str) -> None:
|
||||
if serial == self.calibrating_serial and self._running():
|
||||
return
|
||||
self._stop_calibrator()
|
||||
self.calibrator = subprocess.Popen(
|
||||
calibrator_command(serial, self.columns, self.rows, self.square_size_m),
|
||||
start_new_session=True,
|
||||
)
|
||||
self.calibrating_serial = serial
|
||||
self.feedback = f"正在标定 {serial}。棋盘摆出不同姿态后,在标定器窗口点 CALIBRATE,再点 SAVE"
|
||||
self.get_logger().info(self.feedback)
|
||||
|
||||
def _running(self) -> bool:
|
||||
return self.calibrator is not None and self.calibrator.poll() is None
|
||||
|
||||
def _stop_calibrator(self) -> None:
|
||||
process = self.calibrator
|
||||
self.calibrator = None
|
||||
self.calibrating_serial = ""
|
||||
if process is None or process.poll() is not None:
|
||||
return
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGINT)
|
||||
process.wait(timeout=2)
|
||||
except subprocess.TimeoutExpired:
|
||||
os.killpg(process.pid, signal.SIGKILL)
|
||||
process.wait(timeout=1)
|
||||
except ProcessLookupError:
|
||||
pass
|
||||
|
||||
def _preview(self, serial: str, width: int, height: int) -> np.ndarray:
|
||||
canvas = np.full((height, width, 3), 24, dtype=np.uint8)
|
||||
message = self.latest.get(serial)
|
||||
if message is None:
|
||||
_draw_text(canvas, f"{serial} 等待图像", (20, height // 2), 28, (0, 180, 255))
|
||||
return canvas
|
||||
image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
|
||||
scale = min(width / image.shape[1], height / image.shape[0])
|
||||
resized = cv2.resize(
|
||||
image,
|
||||
(max(1, int(round(image.shape[1] * scale))), max(1, int(round(image.shape[0] * scale)))),
|
||||
interpolation=cv2.INTER_AREA,
|
||||
)
|
||||
x = (width - resized.shape[1]) // 2
|
||||
y = (height - resized.shape[0]) // 2
|
||||
canvas[y : y + resized.shape[0], x : x + resized.shape[1]] = resized
|
||||
return canvas
|
||||
|
||||
def _request_shutdown(self) -> None:
|
||||
if self._closing:
|
||||
return
|
||||
self._closing = True
|
||||
self.get_logger().info("标定窗口已关闭")
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
def _draw(self) -> None:
|
||||
if self._closing:
|
||||
return
|
||||
if window_closed(self.GUI_WINDOW):
|
||||
self._request_shutdown()
|
||||
return
|
||||
if self.calibrating_serial and not self._running():
|
||||
self.feedback = f"{self.calibrating_serial} 的标定器已退出"
|
||||
self.calibrating_serial = ""
|
||||
self.calibrator = None
|
||||
serials = self.camera_serials
|
||||
canvas = np.full((760, 1400, 3), 18, dtype=np.uint8)
|
||||
_draw_text(canvas, self.feedback, (20, 16), 28, (230, 230, 230))
|
||||
count = len(serials)
|
||||
column = 1400 // count
|
||||
self.buttons = {}
|
||||
for index, serial in enumerate(serials):
|
||||
x = index * column
|
||||
preview = self._preview(serial, max(1, column - 16), 560)
|
||||
canvas[70 : 70 + preview.shape[0], x + 8 : x + 8 + preview.shape[1]] = preview
|
||||
_draw_text(canvas, serial, (x + 16, 640), 24, (220, 220, 220))
|
||||
rectangle = (x + 16, 680, x + column - 16, 740)
|
||||
self.buttons[f"calibrate:{serial}"] = rectangle
|
||||
selected = serial == self.calibrating_serial
|
||||
cv2.rectangle(canvas, (rectangle[0], rectangle[1]), (rectangle[2], rectangle[3]),
|
||||
(40, 140, 40) if selected else (70, 70, 70), -1)
|
||||
label = "标定中" if selected else "标定这台"
|
||||
_draw_text(canvas, label, (rectangle[0] + 24, rectangle[1] + 12), 28, (255, 255, 255))
|
||||
cv2.imshow(self.GUI_WINDOW, canvas)
|
||||
cv2.waitKey(1)
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self._stop_calibrator()
|
||||
cv2.destroyWindow(self.GUI_WINDOW)
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
configure_fastdds_large_image_transport()
|
||||
rclpy.init(args=args)
|
||||
node = IntrinsicCalibrationNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
@@ -0,0 +1,115 @@
|
||||
"""Launch every connected Hikrobot camera and pick one to calibrate."""
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler, SetEnvironmentVariable, Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
|
||||
from camera_parameters.hikrobot_camera import enumerate_usb_serials
|
||||
from camera_parameters.output_paths import camera_namespace
|
||||
|
||||
|
||||
def _workspace_root() -> Path:
|
||||
here = Path(__file__).resolve()
|
||||
for candidate in here.parents:
|
||||
if (candidate / "src" / "camera_parameters" / "package.xml").is_file():
|
||||
return candidate
|
||||
return Path.cwd().resolve()
|
||||
|
||||
|
||||
def _launch(context):
|
||||
workspace = _workspace_root()
|
||||
serials = enumerate_usb_serials()
|
||||
if not serials:
|
||||
raise RuntimeError("没有枚举到海康相机")
|
||||
cameras = []
|
||||
for serial in serials:
|
||||
cameras.append(
|
||||
Node(
|
||||
package="camera_parameters",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=camera_namespace(serial),
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": serial,
|
||||
"workspace": str(workspace),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": f"hikrobot_{serial}",
|
||||
"frame_id": f"calibration_{serial}_optical_frame",
|
||||
"image_width": 1624,
|
||||
"image_height": 1240,
|
||||
"frame_rate": ParameterValue(
|
||||
LaunchConfiguration("camera_frame_rate"),
|
||||
value_type=float,
|
||||
),
|
||||
"exposure_time_us": ParameterValue(
|
||||
LaunchConfiguration("exposure_time_us"),
|
||||
value_type=float,
|
||||
),
|
||||
"gain_db": ParameterValue(
|
||||
LaunchConfiguration("gain_db"), value_type=float
|
||||
),
|
||||
"auto_exposure": False,
|
||||
"camera_info_url": "",
|
||||
}
|
||||
],
|
||||
)
|
||||
)
|
||||
picker = Node(
|
||||
package="camera_parameters",
|
||||
executable="camera_intrinsics_node",
|
||||
name="camera_intrinsics",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"camera_serials": ",".join(serials),
|
||||
"checkerboard_columns": ParameterValue(
|
||||
LaunchConfiguration("checkerboard_columns"), value_type=int
|
||||
),
|
||||
"checkerboard_rows": ParameterValue(
|
||||
LaunchConfiguration("checkerboard_rows"), value_type=int
|
||||
),
|
||||
"square_size_m": ParameterValue(
|
||||
LaunchConfiguration("square_size_m"), value_type=float
|
||||
),
|
||||
}
|
||||
],
|
||||
)
|
||||
shutdown = RegisterEventHandler(OnProcessExit(
|
||||
target_action=picker, on_exit=[Shutdown(reason="标定窗口已关闭")],
|
||||
))
|
||||
return [*cameras, picker, shutdown]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(get_package_share_directory("camera_parameters"))
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(name="RMW_IMPLEMENTATION", value="rmw_fastrtps_cpp"),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTDDS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("checkerboard_columns", default_value="8"),
|
||||
DeclareLaunchArgument("checkerboard_rows", default_value="5"),
|
||||
DeclareLaunchArgument("square_size_m", default_value="0.027"),
|
||||
OpaqueFunction(function=_launch),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,188 @@
|
||||
"""Launch every connected Hikrobot camera, then assign views in the GUI."""
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler, SetEnvironmentVariable, Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import ComposableNodeContainer, Node
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
|
||||
from camera_parameters.hikrobot_camera import enumerate_usb_serials
|
||||
from camera_parameters.output_paths import camera_namespace
|
||||
|
||||
|
||||
def _workspace_root() -> Path:
|
||||
here = Path(__file__).resolve()
|
||||
for candidate in here.parents:
|
||||
if (candidate / "src" / "camera_parameters" / "package.xml").is_file():
|
||||
return candidate
|
||||
return Path.cwd().resolve()
|
||||
|
||||
|
||||
def _launch(context):
|
||||
workspace = _workspace_root()
|
||||
serials = enumerate_usb_serials()
|
||||
if len(serials) < 3:
|
||||
raise RuntimeError(
|
||||
"需要至少三台相机才能标定外参,"
|
||||
f"当前枚举到: {', '.join(serials) if serials else '无'}"
|
||||
)
|
||||
cameras = []
|
||||
rectifiers = []
|
||||
for serial in serials:
|
||||
namespace = camera_namespace(serial)
|
||||
cameras.append(
|
||||
Node(
|
||||
package="camera_parameters",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=namespace,
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": serial,
|
||||
"workspace": str(workspace),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": f"hikrobot_{serial}",
|
||||
"frame_id": f"calibration_{serial}_optical_frame",
|
||||
"image_width": 1624,
|
||||
"image_height": 1240,
|
||||
"frame_rate": ParameterValue(
|
||||
LaunchConfiguration("camera_frame_rate"),
|
||||
value_type=float,
|
||||
),
|
||||
"exposure_time_us": ParameterValue(
|
||||
LaunchConfiguration("exposure_time_us"),
|
||||
value_type=float,
|
||||
),
|
||||
"gain_db": ParameterValue(
|
||||
LaunchConfiguration("gain_db"), value_type=float
|
||||
),
|
||||
"auto_exposure": False,
|
||||
"camera_info_url": "",
|
||||
}
|
||||
],
|
||||
)
|
||||
)
|
||||
rectifiers.append(
|
||||
ComposableNode(
|
||||
package="image_proc",
|
||||
plugin="image_proc::RectifyNode",
|
||||
name=f"rectify_{serial}",
|
||||
namespace=namespace,
|
||||
remappings=[
|
||||
("image", f"{namespace}/image_raw"),
|
||||
("camera_info", f"{namespace}/camera_info"),
|
||||
("image_rect", f"{namespace}/image_rect"),
|
||||
],
|
||||
parameters=[{"queue_size": 1}],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
)
|
||||
)
|
||||
container = ComposableNodeContainer(
|
||||
name="extrinsics_vision",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
composable_node_descriptions=rectifiers,
|
||||
output="screen",
|
||||
)
|
||||
solver = Node(
|
||||
package="camera_parameters",
|
||||
executable="three_camera_extrinsics_node",
|
||||
name="three_camera_extrinsics",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"workspace": str(workspace),
|
||||
"camera_serials": ",".join(serials),
|
||||
"checkerboard_columns": ParameterValue(
|
||||
LaunchConfiguration("checkerboard_columns"), value_type=int
|
||||
),
|
||||
"checkerboard_rows": ParameterValue(
|
||||
LaunchConfiguration("checkerboard_rows"), value_type=int
|
||||
),
|
||||
"square_size_m": ParameterValue(
|
||||
LaunchConfiguration("square_size_m"), value_type=float
|
||||
),
|
||||
"enable_gui": ParameterValue(
|
||||
LaunchConfiguration("enable_gui"), value_type=bool
|
||||
),
|
||||
"gui_refresh_hz": ParameterValue(
|
||||
LaunchConfiguration("gui_refresh_hz"), value_type=float
|
||||
),
|
||||
"maximum_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration("maximum_reprojection_rms_px"),
|
||||
value_type=float,
|
||||
),
|
||||
"maximum_candidate_pair_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration(
|
||||
"maximum_candidate_pair_reprojection_rms_px"
|
||||
),
|
||||
value_type=float,
|
||||
),
|
||||
"maximum_single_camera_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration(
|
||||
"maximum_single_camera_reprojection_rms_px"
|
||||
),
|
||||
value_type=float,
|
||||
),
|
||||
"auto_capture_default": ParameterValue(
|
||||
LaunchConfiguration("auto_capture_default"),
|
||||
value_type=bool,
|
||||
),
|
||||
"auto_capture_stable_seconds": ParameterValue(
|
||||
LaunchConfiguration("auto_capture_stable_seconds"),
|
||||
value_type=float,
|
||||
),
|
||||
}
|
||||
],
|
||||
)
|
||||
shutdown = RegisterEventHandler(OnProcessExit(
|
||||
target_action=solver, on_exit=[Shutdown(reason="标定窗口已关闭")],
|
||||
))
|
||||
return [*cameras, container, solver, shutdown]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(get_package_share_directory("camera_parameters"))
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(name="RMW_IMPLEMENTATION", value="rmw_fastrtps_cpp"),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTDDS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("checkerboard_columns", default_value="8"),
|
||||
DeclareLaunchArgument("checkerboard_rows", default_value="5"),
|
||||
DeclareLaunchArgument("square_size_m", default_value="0.027"),
|
||||
DeclareLaunchArgument("enable_gui", default_value="true"),
|
||||
DeclareLaunchArgument("gui_refresh_hz", default_value="2.0"),
|
||||
DeclareLaunchArgument("maximum_reprojection_rms_px", default_value="1.2"),
|
||||
DeclareLaunchArgument(
|
||||
"maximum_candidate_pair_reprojection_rms_px",
|
||||
default_value="1.5",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"maximum_single_camera_reprojection_rms_px",
|
||||
default_value="1.5",
|
||||
),
|
||||
DeclareLaunchArgument("auto_capture_default", default_value="false"),
|
||||
DeclareLaunchArgument("auto_capture_stable_seconds", default_value="1.0"),
|
||||
OpaqueFunction(function=_launch),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,125 @@
|
||||
"""相机内外参结果的目录和文件名。每次保存都新建文件。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
_STAMP = re.compile(r"^\d{8}_\d{6}$")
|
||||
|
||||
|
||||
def resolve_workspace(explicit: str | Path | None = None) -> Path:
|
||||
"""仓库根目录。显式路径优先,否则从当前目录向上找本包。"""
|
||||
if explicit:
|
||||
return Path(explicit).expanduser().resolve()
|
||||
current = Path.cwd().resolve()
|
||||
for candidate in (current, *current.parents):
|
||||
marker = candidate / "src" / "camera_parameters" / "package.xml"
|
||||
if marker.is_file():
|
||||
return candidate
|
||||
return current
|
||||
|
||||
|
||||
def camera_namespace(serial: str) -> str:
|
||||
return f"/calibration/{serial}/camera"
|
||||
|
||||
|
||||
def _stamp(when: datetime | None) -> str:
|
||||
moment = when or datetime.now()
|
||||
return moment.strftime("%Y%m%d_%H%M%S")
|
||||
|
||||
|
||||
def _unique(path: Path) -> Path:
|
||||
if not path.exists():
|
||||
return path
|
||||
for index in range(2, 100):
|
||||
candidate = path.with_name(f"{path.stem}_{index}{path.suffix}")
|
||||
if not candidate.exists():
|
||||
return candidate
|
||||
raise FileExistsError(f"too many files named like {path.name}")
|
||||
|
||||
|
||||
def intrinsic_output_path(
|
||||
workspace: Path, serial: str, when: datetime | None = None
|
||||
) -> Path:
|
||||
directory = Path(workspace) / "output_camera" / "intrinsics"
|
||||
return _unique(directory / f"{serial}-{_stamp(when)}.yaml")
|
||||
|
||||
|
||||
def extrinsic_output_path(
|
||||
workspace: Path,
|
||||
front_serial: str,
|
||||
side_serial: str,
|
||||
top_serial: str,
|
||||
when: datetime | None = None,
|
||||
) -> Path:
|
||||
directory = Path(workspace) / "output_camera" / "extrinsics"
|
||||
name = f"{front_serial}-{side_serial}-{top_serial}-{_stamp(when)}.yaml"
|
||||
return _unique(directory / name)
|
||||
|
||||
|
||||
def intrinsic_paths(workspace: Path, serial: str) -> list[Path]:
|
||||
"""同一序列号的内参,按时间戳从旧到新。"""
|
||||
directory = Path(workspace) / "output_camera" / "intrinsics"
|
||||
if not directory.is_dir():
|
||||
return []
|
||||
prefix = f"{serial}-"
|
||||
matches = []
|
||||
for path in directory.glob(f"{serial}-*.yaml"):
|
||||
body = path.stem[len(prefix):]
|
||||
token = body[:15]
|
||||
if not _STAMP.match(token):
|
||||
continue
|
||||
matches.append((token, body, path))
|
||||
matches.sort()
|
||||
return [path for _, _, path in matches]
|
||||
|
||||
|
||||
def latest_intrinsic_path(workspace: Path, serial: str) -> Path | None:
|
||||
"""同一序列号取时间戳最新的一份内参。没有文件时返回空。"""
|
||||
paths = intrinsic_paths(workspace, serial)
|
||||
if not paths:
|
||||
return None
|
||||
return paths[-1]
|
||||
|
||||
|
||||
_EXTRINSIC_STAMP = re.compile(r"(\d{8}_\d{6})(?:_\d+)?$")
|
||||
|
||||
|
||||
def latest_extrinsic_path(workspace: Path) -> Path | None:
|
||||
"""output_camera/extrinsics 里时间戳最新的一份。没有文件时返回空。"""
|
||||
directory = Path(workspace) / "output_camera" / "extrinsics"
|
||||
if not directory.is_dir():
|
||||
return None
|
||||
matches = []
|
||||
for path in directory.glob("*.yaml"):
|
||||
found = _EXTRINSIC_STAMP.search(path.stem)
|
||||
if found is None:
|
||||
continue
|
||||
matches.append((found.group(1), path.stem, path))
|
||||
if not matches:
|
||||
return None
|
||||
matches.sort()
|
||||
return matches[-1][2]
|
||||
|
||||
|
||||
def assign_view(
|
||||
serial_of_view: dict[str, str], serial: str, view: str
|
||||
) -> dict[str, str]:
|
||||
"""把一台相机标成一个视角。同一台或同一个视角上的旧指定会被清掉。"""
|
||||
if view not in VIEWS:
|
||||
raise ValueError(f"unknown view {view}")
|
||||
updated = {name: serial_of_view.get(name, "") for name in VIEWS}
|
||||
for name in VIEWS:
|
||||
if updated[name] == serial:
|
||||
updated[name] = ""
|
||||
updated[view] = serial
|
||||
return updated
|
||||
|
||||
|
||||
def assignment_ready(serial_of_view: dict[str, str]) -> bool:
|
||||
chosen = [serial_of_view.get(view, "") for view in VIEWS]
|
||||
return all(chosen) and len(set(chosen)) == 3
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>camera_parameters</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Hikrobot camera intrinsics and three-camera checkerboard extrinsics.</description>
|
||||
<maintainer email="support@linker-robotics.com">lxp</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<exec_depend>ament_index_python</exec_depend>
|
||||
<exec_depend>camera_calibration</exec_depend>
|
||||
<exec_depend>cv_bridge</exec_depend>
|
||||
<exec_depend>image_proc</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>rclcpp_components</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>std_srvs</exec_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
<exec_depend>python3-scipy</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,179 @@
|
||||
"""Pure quaternion summaries and rotation-axis fitting."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Sequence
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
|
||||
quaternion = np.asarray(values, dtype=float)
|
||||
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
|
||||
raise ValueError("quaternion must contain four finite xyzw values")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if norm < 1e-12:
|
||||
raise ValueError("quaternion norm is zero")
|
||||
return quaternion / norm
|
||||
|
||||
|
||||
def relative_quaternion_xyzw(
|
||||
parent_camera_quaternion: Sequence[float],
|
||||
child_camera_quaternion: Sequence[float],
|
||||
) -> tuple[float, float, float, float]:
|
||||
"""Compute parent-to-child orientation from two camera-to-Tag rotations."""
|
||||
parent = Rotation.from_quat(
|
||||
normalize_quaternion_xyzw(parent_camera_quaternion)
|
||||
)
|
||||
child = Rotation.from_quat(
|
||||
normalize_quaternion_xyzw(child_camera_quaternion)
|
||||
)
|
||||
quaternion = (parent.inv() * child).as_quat()
|
||||
return tuple(float(value) for value in quaternion)
|
||||
|
||||
|
||||
def image_plane_tag_quaternion_xyzw(
|
||||
corners_xy: Sequence[Sequence[float]],
|
||||
) -> tuple[float, float, float, float]:
|
||||
"""Estimate Tag orientation about the optical axis from ordered corners."""
|
||||
corners = np.asarray(corners_xy, dtype=float)
|
||||
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
|
||||
raise ValueError("corners_xy must contain four finite xy points")
|
||||
x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
|
||||
if float(np.linalg.norm(x_axis)) < 1e-9:
|
||||
raise ValueError("tag x-axis is degenerate")
|
||||
angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
|
||||
quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
|
||||
return tuple(float(value) for value in quaternion)
|
||||
|
||||
|
||||
def robust_rotation_summary(
|
||||
quaternions_xyzw: Sequence[Sequence[float]],
|
||||
) -> tuple[tuple[float, float, float, float], float]:
|
||||
"""Return a robust orientation and maximum angular residual in radians."""
|
||||
if not quaternions_xyzw:
|
||||
raise ValueError("at least one quaternion is required")
|
||||
rotations = Rotation.from_quat(
|
||||
np.asarray(
|
||||
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
reference = rotations[0]
|
||||
delta_vectors = (reference.inv() * rotations).as_rotvec()
|
||||
median_delta = np.median(delta_vectors, axis=0)
|
||||
robust = reference * Rotation.from_rotvec(median_delta)
|
||||
residuals = (robust.inv() * rotations).magnitude()
|
||||
maximum = float(np.max(residuals)) if residuals.size else 0.0
|
||||
return tuple(float(value) for value in robust.as_quat()), maximum
|
||||
|
||||
|
||||
def rotation_spread_rad(
|
||||
quaternions_xyzw: Sequence[Sequence[float]],
|
||||
) -> float:
|
||||
"""Return the maximum geodesic residual around a robust orientation."""
|
||||
_, spread = robust_rotation_summary(quaternions_xyzw)
|
||||
return spread
|
||||
|
||||
|
||||
def rotation_rms_rad(
|
||||
quaternions_xyzw: Sequence[Sequence[float]],
|
||||
*,
|
||||
outlier_threshold_rad: float | None = None,
|
||||
) -> float:
|
||||
"""Return RMS geodesic noise around a robust orientation."""
|
||||
robust, _ = robust_rotation_summary(quaternions_xyzw)
|
||||
reference = Rotation.from_quat(robust)
|
||||
rotations = Rotation.from_quat(
|
||||
np.asarray(
|
||||
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
residuals = (reference.inv() * rotations).magnitude()
|
||||
if outlier_threshold_rad is not None:
|
||||
threshold = float(outlier_threshold_rad)
|
||||
if threshold <= 0.0:
|
||||
raise ValueError("outlier_threshold_rad must be positive")
|
||||
residuals = residuals[residuals <= threshold]
|
||||
if residuals.size == 0:
|
||||
return float("inf")
|
||||
return float(np.sqrt(np.mean(np.square(residuals))))
|
||||
|
||||
|
||||
def rotation_inlier_fraction(
|
||||
quaternions_xyzw: Sequence[Sequence[float]],
|
||||
*,
|
||||
outlier_threshold_rad: float,
|
||||
) -> float:
|
||||
"""Return the fraction close to the robust orientation."""
|
||||
threshold = float(outlier_threshold_rad)
|
||||
if threshold <= 0.0:
|
||||
raise ValueError("outlier_threshold_rad must be positive")
|
||||
robust, _ = robust_rotation_summary(quaternions_xyzw)
|
||||
reference = Rotation.from_quat(robust)
|
||||
rotations = Rotation.from_quat(
|
||||
np.asarray(
|
||||
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
residuals = (reference.inv() * rotations).magnitude()
|
||||
return float(np.mean(residuals <= threshold))
|
||||
|
||||
|
||||
def delta_rotation_vector(
|
||||
reference_xyzw: Sequence[float],
|
||||
observed_xyzw: Sequence[float],
|
||||
) -> np.ndarray:
|
||||
reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
|
||||
observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
|
||||
return (reference.inv() * observed).as_rotvec()
|
||||
|
||||
|
||||
def fit_rotation_axis(
|
||||
vectors: Sequence[Sequence[float]],
|
||||
commands: Sequence[int],
|
||||
) -> np.ndarray:
|
||||
"""Fit and orient the single rotational axis used by one command sweep."""
|
||||
matrix = np.asarray(vectors, dtype=float)
|
||||
command_values = np.asarray(commands, dtype=int)
|
||||
if matrix.ndim != 2 or matrix.shape[1] != 3:
|
||||
raise ValueError("vectors must have shape (N, 3)")
|
||||
if command_values.shape != (matrix.shape[0],):
|
||||
raise ValueError("commands must match vectors")
|
||||
useful = np.linalg.norm(matrix, axis=1) > 1e-6
|
||||
if int(np.count_nonzero(useful)) < 3:
|
||||
raise ValueError("insufficient non-zero rotations to fit an axis")
|
||||
_, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
|
||||
axis = vh[0]
|
||||
projections = matrix @ axis
|
||||
low = projections[command_values <= 16]
|
||||
high = projections[command_values >= 239]
|
||||
# Physical-angle adapters normalize their measured feedback into the
|
||||
# legacy 0..255 fitting coordinate. Tracking lag and mechanical travel
|
||||
# mean those records need not reach the historical <=16 endpoint even
|
||||
# when the observed physical stroke is complete. SVD axis sign is
|
||||
# arbitrary, so fall back to the ends of the *observed* command domain
|
||||
# instead of allowing that arbitrary sign to reverse an otherwise valid
|
||||
# trajectory. Byte-feedback products that observe the canonical end
|
||||
# bands retain their existing behaviour.
|
||||
if not low.size or not high.size:
|
||||
observed = command_values[useful]
|
||||
lower = int(np.min(observed))
|
||||
upper = int(np.max(observed))
|
||||
span = upper - lower
|
||||
if span <= 0:
|
||||
raise ValueError("commands do not span a rotation trajectory")
|
||||
band = max(1, int(math.ceil(0.1 * span)))
|
||||
low = projections[
|
||||
useful & (command_values <= lower + band)
|
||||
]
|
||||
high = projections[
|
||||
useful & (command_values >= upper - band)
|
||||
]
|
||||
if low.size and high.size and float(np.median(low)) < float(np.median(high)):
|
||||
axis = -axis
|
||||
return axis / np.linalg.norm(axis)
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/camera_parameters
|
||||
[install]
|
||||
install_scripts=$base/lib/camera_parameters
|
||||
@@ -0,0 +1,36 @@
|
||||
from glob import glob
|
||||
|
||||
from setuptools import setup
|
||||
|
||||
|
||||
package_name = "camera_parameters"
|
||||
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
package_dir={package_name: "."},
|
||||
data_files=[
|
||||
(
|
||||
"share/ament_index/resource_index/packages",
|
||||
["resource/" + package_name],
|
||||
),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
("share/" + package_name + "/launch", glob("launch/*.launch.py")),
|
||||
("share/" + package_name + "/config", glob("config/*")),
|
||||
],
|
||||
install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
|
||||
zip_safe=True,
|
||||
maintainer="lxp",
|
||||
maintainer_email="support@linker-robotics.com",
|
||||
description="Hikrobot camera intrinsics and three-camera checkerboard extrinsics",
|
||||
license="MIT",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"hikrobot_camera_node = camera_parameters.hikrobot_camera:main",
|
||||
"camera_intrinsics_node = camera_parameters.intrinsics_node:main",
|
||||
"three_camera_extrinsics_node = camera_parameters.extrinsics_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
"""Visual command-range calibration, independent of vendor SDK source code."""
|
||||
@@ -0,0 +1 @@
|
||||
"""Hardware interfaces and a deterministic simulator."""
|
||||
@@ -0,0 +1,20 @@
|
||||
from dataclasses import dataclass
|
||||
from typing import Protocol
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Feedback:
|
||||
stamp_ns: int
|
||||
received: float
|
||||
positions: tuple[float, ...]
|
||||
|
||||
|
||||
class HandAdapter(Protocol):
|
||||
uid: str
|
||||
feedback: Feedback | None
|
||||
|
||||
def control_error(self, now: float) -> str | None: ...
|
||||
def diagnostic_warning(self) -> str | None: ...
|
||||
def send_positions(self, positions): ...
|
||||
def set_motion(self, speed, torque): ...
|
||||
def set_offset(self, offset): ...
|
||||
@@ -0,0 +1,103 @@
|
||||
"""No ROS imports or hardware access: virtual hand and projected tag observations."""
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .base import Feedback
|
||||
|
||||
|
||||
def synthetic_theta(command, travel_min, travel_max, lower, upper):
|
||||
"""指令落在行程死区时角度贴在对应 URDF 限位,中间按指令线性变化。"""
|
||||
command = float(command)
|
||||
lower, upper = float(lower), float(upper)
|
||||
if command <= travel_min:
|
||||
return lower
|
||||
if command >= travel_max:
|
||||
return upper
|
||||
span = float(travel_max) - float(travel_min)
|
||||
if span <= 0:
|
||||
return lower
|
||||
weight = (command - float(travel_min)) / span
|
||||
return lower + weight * (upper - lower)
|
||||
|
||||
|
||||
def _square(phi, center):
|
||||
direction = np.array([math.cos(phi), math.sin(phi)])
|
||||
ortho = np.array([-direction[1], direction[0]])
|
||||
origin = np.asarray(center, dtype=float)
|
||||
return np.vstack([
|
||||
origin,
|
||||
origin + 40 * direction,
|
||||
origin + 40 * direction + 40 * ortho,
|
||||
origin + 40 * ortho,
|
||||
])
|
||||
|
||||
|
||||
class FakeAdapter:
|
||||
def __init__(self, profile, ranges=None):
|
||||
self.profile, self.uid = profile, 'SIMULATED_HAND'
|
||||
self.target = tuple(j.minimum for j in profile.joints)
|
||||
self.feedback = Feedback(0, 0, self.target)
|
||||
self.ranges = ranges or {j.name: (j.minimum, j.maximum) for j in profile.joints}
|
||||
self.sent, self.settings_sent, self.offsets_sent = [], [], []
|
||||
self.error = None
|
||||
self.warning = None
|
||||
self.external_publishers = 0
|
||||
|
||||
def control_error(self, now):
|
||||
if self.external_publishers:
|
||||
return '存在其他位置命令发布者'
|
||||
return self.error
|
||||
|
||||
def diagnostic_warning(self):
|
||||
return self.warning
|
||||
|
||||
def send_positions(self, positions):
|
||||
self.target = self.profile.vector(positions)
|
||||
self.sent.append(self.target)
|
||||
|
||||
def set_motion(self, speed, torque):
|
||||
self.settings_sent.append((speed, torque))
|
||||
|
||||
def set_offset(self, offset):
|
||||
if len(offset) != len(self.profile.joints):
|
||||
raise ValueError(f'偏移数组长度必须为 {len(self.profile.joints)}')
|
||||
self.offsets_sent.append([list(pair) for pair in offset])
|
||||
|
||||
def advance(self, now, stamp_ns):
|
||||
# Feedback intentionally echoes targets. Only image motion identifies limits.
|
||||
self.feedback = Feedback(stamp_ns, now, self.target)
|
||||
|
||||
def frames(self, noise=0, rng=None, angle_model=None, active=None):
|
||||
frames = {view: {} for view in self.profile.views}
|
||||
if angle_model is None:
|
||||
displacements = {}
|
||||
for joint in self.profile.joints:
|
||||
lo, hi = self.ranges[joint.name]
|
||||
key = joint.view, joint.tag_id
|
||||
displacements[key] = displacements.get(key, 0) + .8 * float(
|
||||
np.clip(self.target[joint.index], lo, hi))
|
||||
for (view, tag_id), displacement in displacements.items():
|
||||
points = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float)
|
||||
points += [tag_id * 65 + displacement, 50]
|
||||
if noise:
|
||||
points += rng.normal(0, noise, points.shape)
|
||||
frames[view][tag_id] = points.tolist()
|
||||
return frames
|
||||
owners = {}
|
||||
for joint in self.profile.joints:
|
||||
key = (joint.view, joint.tag_id)
|
||||
if active and joint.name == active:
|
||||
owners[key] = joint
|
||||
elif key not in owners:
|
||||
owners[key] = joint
|
||||
for (view, tag_id), joint in owners.items():
|
||||
travel_min, travel_max = self.ranges[joint.name]
|
||||
lower, upper = angle_model.kinematics.limits(joint.name)
|
||||
theta = synthetic_theta(self.target[joint.index], travel_min, travel_max, lower, upper)
|
||||
phi = angle_model.phi_for_theta(joint.name, view, theta, {})
|
||||
points = _square(phi, np.array([100 + tag_id * 65, 150], dtype=float))
|
||||
if noise:
|
||||
points = points + rng.normal(0, noise, points.shape)
|
||||
frames[view][tag_id] = points.tolist()
|
||||
return frames
|
||||
@@ -0,0 +1,4 @@
|
||||
"""按适配器名字到 hands 各型号里查找。采集层不登记具体手型。"""
|
||||
from hands import adapter_type
|
||||
|
||||
__all__ = ["adapter_type"]
|
||||
@@ -0,0 +1,154 @@
|
||||
"""用工装坐标系里的同一根 URDF 轴,把贴纸边的图像转角还原成关节角。"""
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from .fixture import front_from_views, load_hand_mount
|
||||
from .kinematic import KinematicModel
|
||||
|
||||
OPTICAL_AXIS = np.array([0.0, 0.0, 1.0])
|
||||
MAX_AXIS_ANGLE_RAD = math.radians(20.0)
|
||||
MIN_AXIS_COSINE = math.cos(MAX_AXIS_ANGLE_RAD)
|
||||
MAX_FRAME_JUMP_RAD = math.radians(35.0)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AngleReading:
|
||||
theta_rad: float | None
|
||||
phi_rad: float | None
|
||||
cosine: float | None
|
||||
reason: str = ''
|
||||
|
||||
@property
|
||||
def ok(self) -> bool:
|
||||
return self.reason == ''
|
||||
|
||||
|
||||
def edge_angle(corners) -> float:
|
||||
points = np.asarray(corners, dtype=float)
|
||||
if points.shape != (4, 2) or not np.all(np.isfinite(points)):
|
||||
raise ValueError('tag_not_visible')
|
||||
edge = points[1] - points[0]
|
||||
return float(math.atan2(edge[1], edge[0]))
|
||||
|
||||
|
||||
def unwrap_edge_angle(previous_rad: float, current_rad: float) -> float | None:
|
||||
delta = (current_rad - previous_rad + math.pi) % (2.0 * math.pi) - math.pi
|
||||
if abs(delta) > MAX_FRAME_JUMP_RAD:
|
||||
return None
|
||||
return previous_rad + delta
|
||||
|
||||
|
||||
def _wrap(delta: float) -> float:
|
||||
return (delta + math.pi) % (2.0 * math.pi) - math.pi
|
||||
|
||||
|
||||
def direction_in_camera(direction_palm, *, camera: str, fixture_from_hand, front_from_fixture,
|
||||
front_from_view: dict) -> np.ndarray:
|
||||
if camera not in front_from_view:
|
||||
raise ValueError(f'missing_camera_extrinsic:{camera}')
|
||||
direction = np.asarray(direction_palm, dtype=float)
|
||||
direction = fixture_from_hand[:3, :3] @ direction
|
||||
direction = front_from_fixture[:3, :3] @ direction
|
||||
direction = np.linalg.inv(front_from_view[camera][:3, :3]) @ direction
|
||||
norm = float(np.linalg.norm(direction))
|
||||
if norm <= 1e-12 or not math.isfinite(norm):
|
||||
raise ValueError('camera_axis_degenerate')
|
||||
return direction / norm
|
||||
|
||||
|
||||
class AngleModel:
|
||||
def __init__(self, kinematics: KinematicModel, fixture_from_hand, front_from_fixture,
|
||||
front_from_view: dict, edge_in_link: dict[str, np.ndarray]):
|
||||
self.kinematics = kinematics
|
||||
self.fixture_from_hand = np.asarray(fixture_from_hand, dtype=float)
|
||||
self.front_from_fixture = np.asarray(front_from_fixture, dtype=float)
|
||||
self.front_from_view = front_from_view
|
||||
self.edge_in_link = edge_in_link
|
||||
self._zero_reference: dict[tuple[str, str], tuple[float, float]] = {}
|
||||
|
||||
def edge(self, joint: str) -> np.ndarray:
|
||||
vector = np.asarray(self.edge_in_link.get(joint, (1.0, 0.0, 0.0)), dtype=float)
|
||||
norm = float(np.linalg.norm(vector))
|
||||
if vector.shape != (3,) or norm <= 1e-12 or not np.all(np.isfinite(vector)):
|
||||
raise ValueError(f'{joint} 的贴纸边方向无效')
|
||||
return vector / norm
|
||||
|
||||
def zero_reference(self, joint: str, camera: str) -> tuple[float, float]:
|
||||
"""URDF 全零时的图像边角和轴点积。与具体哪只手无关。"""
|
||||
key = (joint, camera)
|
||||
if key not in self._zero_reference:
|
||||
axis = direction_in_camera(
|
||||
self.kinematics.axis_in_palm(joint, {}),
|
||||
camera=camera, fixture_from_hand=self.fixture_from_hand,
|
||||
front_from_fixture=self.front_from_fixture, front_from_view=self.front_from_view)
|
||||
cosine = float(np.dot(axis, OPTICAL_AXIS))
|
||||
if abs(cosine) < MIN_AXIS_COSINE:
|
||||
raise ValueError('axis_camera_angle_too_large')
|
||||
edge = self.kinematics.link_rotation(joint, {}) @ self.edge(joint)
|
||||
imaged = direction_in_camera(
|
||||
edge, camera=camera, fixture_from_hand=self.fixture_from_hand,
|
||||
front_from_fixture=self.front_from_fixture, front_from_view=self.front_from_view)
|
||||
self._zero_reference[key] = (float(math.atan2(imaged[1], imaged[0])), cosine)
|
||||
return self._zero_reference[key]
|
||||
|
||||
def cosine(self, joint: str, camera: str, ancestor_angles: dict[str, float]) -> float:
|
||||
axis = direction_in_camera(
|
||||
self.kinematics.axis_in_palm(joint, ancestor_angles),
|
||||
camera=camera, fixture_from_hand=self.fixture_from_hand,
|
||||
front_from_fixture=self.front_from_fixture, front_from_view=self.front_from_view)
|
||||
cosine = float(np.dot(axis, OPTICAL_AXIS))
|
||||
if abs(cosine) < MIN_AXIS_COSINE:
|
||||
raise ValueError('axis_camera_angle_too_large')
|
||||
return cosine
|
||||
|
||||
def measure(self, corners, *, joint: str, camera: str, ancestor_angles: dict[str, float],
|
||||
previous_phi: float | None) -> AngleReading:
|
||||
if corners is None:
|
||||
return AngleReading(None, None, None, 'tag_not_visible')
|
||||
try:
|
||||
phi = edge_angle(corners)
|
||||
except ValueError:
|
||||
return AngleReading(None, None, None, 'tag_not_visible')
|
||||
if previous_phi is not None and unwrap_edge_angle(previous_phi, phi) is None:
|
||||
return AngleReading(None, None, None, 'corner_jump')
|
||||
try:
|
||||
phi_ref, _zero_cosine = self.zero_reference(joint, camera)
|
||||
cosine = self.cosine(joint, camera, ancestor_angles)
|
||||
except ValueError as error:
|
||||
return AngleReading(None, phi, None, str(error))
|
||||
theta = _wrap(phi - phi_ref) / cosine
|
||||
return AngleReading(theta, phi, cosine, '')
|
||||
|
||||
def phi_for_theta(self, joint: str, camera: str, theta_rad: float, ancestor_angles: dict[str, float]) -> float:
|
||||
phi_ref, _zero_cosine = self.zero_reference(joint, camera)
|
||||
cosine = self.cosine(joint, camera, ancestor_angles)
|
||||
return phi_ref + cosine * float(theta_rad)
|
||||
|
||||
|
||||
def identity_angle_model(profile) -> AngleModel:
|
||||
"""模拟和演示:转动轴改到光轴上,工装和外参用单位阵。实机不要走这里。"""
|
||||
if profile.kinematics is None:
|
||||
raise ValueError('型号配置缺少 URDF')
|
||||
eye = np.eye(4)
|
||||
views = {view: eye.copy() for view in profile.views}
|
||||
return AngleModel(profile.kinematics.optical_copy(), eye, eye, views, profile.edge_in_link or {})
|
||||
|
||||
|
||||
def load_angle_model(profile, station) -> AngleModel:
|
||||
"""实机:用这份型号自己的 URDF,加上 config.yaml 里的工装和工位外参。"""
|
||||
if profile.kinematics is None or not profile.source_dir:
|
||||
raise ValueError('型号配置缺少 URDF')
|
||||
mount, front = load_hand_mount(Path(profile.source_dir) / 'config.yaml')
|
||||
path = Path(station['extrinsics'])
|
||||
document = yaml.safe_load(path.read_text(encoding='utf-8'))
|
||||
views = front_from_views(document)
|
||||
missing = [view for view in profile.views if view not in views]
|
||||
if missing:
|
||||
raise ValueError('外参缺少视角: ' + '、'.join(missing))
|
||||
return AngleModel(profile.kinematics, mount, front, views, profile.edge_in_link or {})
|
||||
@@ -0,0 +1,57 @@
|
||||
"""Entry point: static validation and replay intentionally precede ROS imports."""
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
import sys
|
||||
|
||||
from .profiles import load_profile, load_station, Settings
|
||||
|
||||
|
||||
def main(argv=None):
|
||||
parser = argparse.ArgumentParser(description='LinkerHand 视觉行程标定')
|
||||
parser.add_argument('--profile', default='o30_right')
|
||||
parser.add_argument('--station')
|
||||
mode = parser.add_mutually_exclusive_group()
|
||||
mode.add_argument('--validate-only', action='store_true', help='仅检查配置,不启动ROS或硬件')
|
||||
mode.add_argument('--simulate', action='store_true', help='虚拟时间模拟全手扫描,不连接硬件')
|
||||
mode.add_argument('--demo', action='store_true', help='使用假SDK和合成预览打开界面')
|
||||
mode.add_argument('--replay', help='从 samples.jsonl 离线复算')
|
||||
parser.add_argument('--output', help='模拟输出目录或离线复算JSON路径')
|
||||
args, ros_args = parser.parse_known_args(argv)
|
||||
try:
|
||||
if args.replay:
|
||||
from .replay import replay
|
||||
output = args.output or str(Path(args.replay).with_name('recomputed_ranges.json'))
|
||||
replay(args.replay, output)
|
||||
print(f'离线复算完成:{output}')
|
||||
return 0
|
||||
profile, station = load_profile(args.profile), load_station(args.station)
|
||||
if not set(profile.views) <= set(station['cameras']):
|
||||
raise ValueError('工位配置缺少所需相机')
|
||||
settings = Settings.from_dict(station.get('scan', {}))
|
||||
if args.validate_only:
|
||||
from .angle_model import load_angle_model
|
||||
load_angle_model(profile, station)
|
||||
print(f'配置通过:{profile.model} {profile.side},{len(profile.joints)} 关节,{len(profile.tasks)} 任务。')
|
||||
for view in profile.views:
|
||||
path = Path(station['cameras'][view]['intrinsics']).expanduser()
|
||||
print(f'{view}: {station["cameras"][view]["serial"]};内参文件:{"存在" if path.is_file() else "尚不存在"}')
|
||||
print('未启动 ROS、SDK 或相机。实机身份、示教和内参有效性将在运行时检查。')
|
||||
return 0
|
||||
if args.simulate:
|
||||
from .simulation import simulate
|
||||
path = simulate(profile, settings, args.output or station['output_root'])
|
||||
print(f'模拟全手标定完成:{path}')
|
||||
return 0
|
||||
except (ValueError, OSError, KeyError) as error:
|
||||
parser.error(str(error))
|
||||
from gui.__main__ import main as gui_main
|
||||
forwarded = list(ros_args)
|
||||
if args.demo:
|
||||
forwarded.append('--demo')
|
||||
if args.station:
|
||||
forwarded += ['--station', args.station]
|
||||
return gui_main(forwarded)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1 @@
|
||||
"""Deterministic calibration logic without ROS or Qt imports."""
|
||||
@@ -0,0 +1,78 @@
|
||||
"""Full-sweep analysis retained for replay of older coarse/fine journals."""
|
||||
import numpy as np
|
||||
|
||||
from ..vision.observations import rms_delta
|
||||
from .errors import Unmeasurable
|
||||
|
||||
|
||||
class RefineWindow(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
def grid(lo, hi, step):
|
||||
values = [float(x) for x in np.arange(lo, hi + step * 1e-6, step)]
|
||||
if not values or abs(values[-1] - hi) > 1e-6:
|
||||
values.append(float(hi))
|
||||
return values
|
||||
|
||||
|
||||
def _departure(distances, threshold, count):
|
||||
"""Confirm displacement from an endpoint, not motion between adjacent samples."""
|
||||
for index in range(1, len(distances) - count + 1):
|
||||
if all(d > threshold for d in distances[index:index+count]):
|
||||
return index
|
||||
raise Unmeasurable('未检测到足够观测点确认离开端点姿态')
|
||||
|
||||
|
||||
def direction_range(samples, joint, threshold, settings, coarse=False):
|
||||
"""Find the outer command boundaries regardless of motion within the range."""
|
||||
samples = sorted(samples, key=lambda s: s['command'])
|
||||
if len(samples) < settings.confirmation_points+1 or len({s['command'] for s in samples}) != len(samples):
|
||||
raise Unmeasurable('采样点不足或重复')
|
||||
commands = [s['command'] for s in samples]
|
||||
points = [s['observations'][joint.name]['corners'] for s in samples]
|
||||
if abs(commands[0]-joint.minimum) > 1e-6 or abs(commands[-1]-joint.maximum) > 1e-6:
|
||||
raise Unmeasurable('缺少指令端点观测')
|
||||
lower = _departure([rms_delta(p, points[0]) for p in points], threshold,
|
||||
settings.confirmation_points)
|
||||
reverse = _departure([rms_delta(p, points[-1]) for p in reversed(points)], threshold,
|
||||
settings.confirmation_points)
|
||||
upper = len(points) - 1 - reverse
|
||||
lo, hi = commands[lower-1], commands[upper+1]
|
||||
if lo >= hi:
|
||||
raise Unmeasurable('两端平台重叠或运动范围无法区分')
|
||||
brackets = ((commands[lower-1], commands[lower]), (commands[upper], commands[upper+1]))
|
||||
if not coarse and any(b-a > joint.resolution + 1e-6 for a, b in brackets):
|
||||
raise RefineWindow('细扫窗口未完整覆盖边界,需扩大窗口')
|
||||
return {'min': lo, 'max': hi, 'brackets': brackets}
|
||||
|
||||
|
||||
def fine_commands(joint, estimates, radius):
|
||||
values = {joint.minimum, joint.maximum}
|
||||
for estimate in estimates:
|
||||
for a, b in estimate['brackets']:
|
||||
first = max(0, int(np.floor((a-radius-joint.minimum)/joint.resolution)))
|
||||
last = min(round((joint.maximum-joint.minimum)/joint.resolution),
|
||||
int(np.ceil((b+radius-joint.minimum)/joint.resolution)))
|
||||
values.update(joint.minimum + i*joint.resolution for i in range(first, last+1))
|
||||
return sorted(values)
|
||||
|
||||
|
||||
def aggregate(joint, sweeps, threshold, settings):
|
||||
expected = {(r, d) for r in range(settings.fine_repetitions) for d in ('up', 'down')}
|
||||
if set(sweeps) != expected:
|
||||
raise Unmeasurable('正反向复测数据不完整')
|
||||
estimates = {key: direction_range(samples, joint, threshold, settings)
|
||||
for key, samples in sweeps.items()}
|
||||
for direction in ('up', 'down'):
|
||||
selected = [v for (r, d), v in estimates.items() if d == direction]
|
||||
for bound in ('min', 'max'):
|
||||
values = [v[bound] for v in selected]
|
||||
if max(values)-min(values) > settings.repeat_tolerance:
|
||||
raise Unmeasurable(f'{direction} 方向 {bound} 复测差异过大')
|
||||
lo = max(v['min'] for v in estimates.values())
|
||||
hi = min(v['max'] for v in estimates.values())
|
||||
if lo >= hi:
|
||||
raise Unmeasurable('正反向复测区间无有效交集')
|
||||
return {'min': joint.validate_value(lo), 'max': joint.validate_value(hi), 'status': '成功',
|
||||
'directions': {f'{r}_{d}': v for (r, d), v in estimates.items()}}
|
||||
@@ -0,0 +1,271 @@
|
||||
"""按工装坐标系里的关节角,记录 10 个驱动指令。"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
ANGLE_COUNT = 10
|
||||
SCAN_METHOD = 'angle_curve_v1'
|
||||
|
||||
|
||||
def command_knots(command_maximum=255):
|
||||
last = ANGLE_COUNT - 1
|
||||
return tuple(int(round(i * float(command_maximum) / last)) for i in range(ANGLE_COUNT))
|
||||
|
||||
|
||||
def angle_targets(lower, upper):
|
||||
lower, upper = float(lower), float(upper)
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or upper < lower:
|
||||
raise ValueError('关节角限位无效')
|
||||
if math.isclose(upper, lower, rel_tol=0, abs_tol=1e-15):
|
||||
return tuple(lower for _ in range(ANGLE_COUNT))
|
||||
values = [lower + (upper - lower) * i / (ANGLE_COUNT - 1) for i in range(ANGLE_COUNT)]
|
||||
values[0] = lower
|
||||
values[-1] = upper
|
||||
return tuple(values)
|
||||
|
||||
|
||||
def assemble_commands(hits, travel_min, travel_max):
|
||||
"""两端够不到 URDF 限位角时,用行程边界填上。中间空格保留。"""
|
||||
commands = list(hits)
|
||||
if len(commands) != ANGLE_COUNT:
|
||||
raise ValueError('指令曲线必须有 10 个点')
|
||||
if commands[0] is None:
|
||||
commands[0] = int(travel_min)
|
||||
if commands[-1] is None:
|
||||
commands[-1] = int(travel_max)
|
||||
return commands
|
||||
|
||||
|
||||
def angle_from_curve(angles, commands, command):
|
||||
pairs = sorted((float(command_i), float(angle))
|
||||
for angle, command_i in zip(angles, commands) if command_i is not None)
|
||||
if not pairs:
|
||||
return None
|
||||
if command <= pairs[0][0]:
|
||||
return pairs[0][1]
|
||||
if command >= pairs[-1][0]:
|
||||
return pairs[-1][1]
|
||||
for (left_command, left_angle), (right_command, right_angle) in zip(pairs, pairs[1:]):
|
||||
if left_command <= command <= right_command and right_command != left_command:
|
||||
weight = (command - left_command) / (right_command - left_command)
|
||||
return left_angle + weight * (right_angle - left_angle)
|
||||
return pairs[-1][1]
|
||||
|
||||
|
||||
def ancestor_angles(kinematics, joint_name, pose, profile, curves):
|
||||
"""瞄准子关节的轴。已标完的祖先用曲线反查角度,其余按指令在 URDF 限位里线性估计。"""
|
||||
names = {joint.index: joint for joint in profile.joints}
|
||||
command_of = {names[index].name: value for index, value in enumerate(pose)}
|
||||
angles = {}
|
||||
for item in kinematics.chain(joint_name)[:-1]:
|
||||
if item.kind == 'fixed' or item.name not in profile.by_name:
|
||||
continue
|
||||
spec = profile.by_name[item.name]
|
||||
command = float(command_of[item.name])
|
||||
curve = curves.get(item.name)
|
||||
if curve is not None:
|
||||
estimated = angle_from_curve(curve['angles_rad'], curve['commands'], command)
|
||||
if estimated is not None:
|
||||
angles[item.name] = estimated
|
||||
continue
|
||||
lower, upper = kinematics.limits(item.name)
|
||||
span = spec.maximum - spec.minimum
|
||||
fraction = 0.0 if span == 0 else (command - spec.minimum) / span
|
||||
angles[item.name] = lower + (upper - lower) * fraction
|
||||
return angles
|
||||
|
||||
|
||||
def command_pose(prepare, profile, task, joint_name, command):
|
||||
"""四指里中指低于分段值时,其余三指保持在指令下限,避免挤在一起。"""
|
||||
goal = list(prepare)
|
||||
deferred = task.deferred_lower
|
||||
if deferred is not None and joint_name == deferred.joint:
|
||||
for name in task.joints:
|
||||
if name == joint_name:
|
||||
continue
|
||||
spec = profile.by_name[name]
|
||||
goal[spec.index] = spec.minimum if command <= deferred.split else spec.maximum
|
||||
goal[profile.by_name[joint_name].index] = command
|
||||
return profile.vector(goal)
|
||||
|
||||
|
||||
class JointCurveScan:
|
||||
"""一个关节:先找角度开始和停止变化的指令,再对 10 个目标角二分。"""
|
||||
|
||||
def __init__(self, joint, settings, targets):
|
||||
self.joint = joint
|
||||
self.settings = settings
|
||||
self.targets = tuple(targets)
|
||||
self.command = joint.minimum
|
||||
self.phase = 'low_anchor'
|
||||
self.theta_low = None
|
||||
self.theta_high = None
|
||||
self.travel_min = None
|
||||
self.travel_max = None
|
||||
self.hits = [None] * ANGLE_COUNT
|
||||
self.pending = []
|
||||
self.target_cursor = 0
|
||||
self.left = self.right = self.probe = None
|
||||
self.sign = 1
|
||||
self.target = None
|
||||
self.hit_index = None
|
||||
self.increasing = True
|
||||
self.error = None
|
||||
self.commands = None
|
||||
|
||||
def start(self):
|
||||
self.command = self.joint.minimum
|
||||
self.phase = 'low_anchor'
|
||||
return 'move'
|
||||
|
||||
def _step(self, command, sign):
|
||||
return self.joint.validate_value(command + sign * self.joint.resolution)
|
||||
|
||||
def _reached(self, theta):
|
||||
if self.increasing:
|
||||
return theta >= self.target
|
||||
return theta <= self.target
|
||||
|
||||
def accept(self, theta):
|
||||
if self.commands is not None or self.error:
|
||||
raise RuntimeError('这个关节的曲线已经结束')
|
||||
theta = float(theta)
|
||||
if self.phase == 'low_anchor':
|
||||
self.theta_low = theta
|
||||
self.command = self._step(self.joint.minimum, 1)
|
||||
self.phase = 'low_search'
|
||||
return 'move'
|
||||
if self.phase == 'low_search':
|
||||
if abs(theta - self.theta_low) > self.settings.angle_motion_rad:
|
||||
self.travel_min = self.joint.validate_value(self.command - self.joint.resolution)
|
||||
self.command = self.joint.maximum
|
||||
self.phase = 'high_anchor'
|
||||
return 'move'
|
||||
if math.isclose(self.command, self.joint.maximum, rel_tol=0, abs_tol=1e-9):
|
||||
self.error = '低端已扫描至对端,未检测到超过阈值的角度变化'
|
||||
return 'fail'
|
||||
self.command = self._step(self.command, 1)
|
||||
return 'move'
|
||||
if self.phase == 'high_anchor':
|
||||
self.theta_high = theta
|
||||
self.command = self._step(self.joint.maximum, -1)
|
||||
self.phase = 'high_search'
|
||||
return 'move'
|
||||
if self.phase == 'high_search':
|
||||
if abs(theta - self.theta_high) > self.settings.angle_motion_rad:
|
||||
self.travel_max = self.joint.validate_value(self.command + self.joint.resolution)
|
||||
if self.travel_min >= self.travel_max:
|
||||
self.error = '两端边界重叠或复测区间无有效交集'
|
||||
return 'fail'
|
||||
return self._begin_targets()
|
||||
if math.isclose(self.command, self.joint.minimum, rel_tol=0, abs_tol=1e-9):
|
||||
self.error = '高端已扫描至对端,未检测到超过阈值的角度变化'
|
||||
return 'fail'
|
||||
self.command = self._step(self.command, -1)
|
||||
return 'move'
|
||||
if self.phase == 'probe':
|
||||
if self._reached(theta):
|
||||
self.right = self.probe
|
||||
else:
|
||||
self.left = self.probe
|
||||
return self._arm_probe()
|
||||
raise RuntimeError(f'未知扫描阶段 {self.phase}')
|
||||
|
||||
def _begin_targets(self):
|
||||
self.increasing = self.theta_high >= self.theta_low
|
||||
low = min(self.theta_low, self.theta_high)
|
||||
high = max(self.theta_low, self.theta_high)
|
||||
self.pending = [index for index, target in enumerate(self.targets)
|
||||
if low - 1e-9 <= target <= high + 1e-9]
|
||||
self.target_cursor = 0
|
||||
return self._arm_target()
|
||||
|
||||
def _arm_target(self):
|
||||
if self.target_cursor >= len(self.pending):
|
||||
self.commands = assemble_commands(self.hits, self.travel_min, self.travel_max)
|
||||
self.phase = 'done'
|
||||
return 'done'
|
||||
self.hit_index = self.pending[self.target_cursor]
|
||||
self.target = self.targets[self.hit_index]
|
||||
if self.increasing:
|
||||
near, far = self.travel_min, self.travel_max
|
||||
near_theta = self.theta_low
|
||||
self.sign = 1
|
||||
else:
|
||||
near, far = self.travel_max, self.travel_min
|
||||
near_theta = self.theta_high
|
||||
self.sign = -1
|
||||
if self._reached(near_theta):
|
||||
self.hits[self.hit_index] = int(near)
|
||||
self.target_cursor += 1
|
||||
return self._arm_target()
|
||||
self.left, self.right = near, far
|
||||
return self._arm_probe()
|
||||
|
||||
def _arm_probe(self):
|
||||
steps = int(round(abs(self.right - self.left) / self.joint.resolution))
|
||||
if steps <= 1:
|
||||
self.hits[self.hit_index] = int(self.right)
|
||||
self.target_cursor += 1
|
||||
return self._arm_target()
|
||||
half = max(1, steps // 2)
|
||||
self.probe = self.joint.validate_value(self.left + self.sign * half * self.joint.resolution)
|
||||
self.command = self.probe
|
||||
self.phase = 'probe'
|
||||
return 'move'
|
||||
|
||||
|
||||
class TaskAngleScan:
|
||||
def __init__(self, task, profile, settings, kinematics, prepare):
|
||||
self.task = task
|
||||
self.profile = profile
|
||||
self.settings = settings
|
||||
self.kinematics = kinematics
|
||||
self.prepare = list(prepare)
|
||||
self.names = list(task.joints)
|
||||
self.cursor = 0
|
||||
self.outputs = {}
|
||||
self.errors = {}
|
||||
self.joint_scan = None
|
||||
self._open()
|
||||
|
||||
def _open(self):
|
||||
name = self.names[self.cursor]
|
||||
joint = self.profile.by_name[name]
|
||||
lower, upper = self.kinematics.limits(name)
|
||||
self.joint_scan = JointCurveScan(joint, self.settings, angle_targets(lower, upper))
|
||||
self.joint_scan.start()
|
||||
|
||||
@property
|
||||
def joint_name(self):
|
||||
return self.joint_scan.joint.name
|
||||
|
||||
def goal(self):
|
||||
return command_pose(self.prepare, self.profile, self.task, self.joint_name, self.joint_scan.command)
|
||||
|
||||
def accept(self, theta):
|
||||
status = self.joint_scan.accept(theta)
|
||||
if status == 'fail':
|
||||
self.errors[self.joint_name] = self.joint_scan.error
|
||||
self.outputs[self.joint_name] = None
|
||||
return self._advance()
|
||||
if status == 'done':
|
||||
self.outputs[self.joint_name] = {
|
||||
'angles_rad': [float(value) for value in self.joint_scan.targets],
|
||||
'commands': list(self.joint_scan.commands),
|
||||
'travel_min': int(self.joint_scan.travel_min),
|
||||
'travel_max': int(self.joint_scan.travel_max),
|
||||
'status': '成功',
|
||||
}
|
||||
return self._advance()
|
||||
return 'move'
|
||||
|
||||
def _advance(self):
|
||||
self.cursor += 1
|
||||
if self.cursor >= len(self.names):
|
||||
return 'task_done'
|
||||
self._open()
|
||||
return 'move'
|
||||
|
||||
def progress(self):
|
||||
return self.cursor, len(self.names)
|
||||
@@ -0,0 +1,70 @@
|
||||
"""Readable, structured sampling diagnostics shared by the engine and ROS host."""
|
||||
import numpy as np
|
||||
|
||||
|
||||
def view_label(view):
|
||||
return {'front': '正面', 'side': '侧面', 'top': '顶部'}.get(view, view)
|
||||
|
||||
|
||||
def tag_label(view, tag_id, joint):
|
||||
return f'{view_label(view)} Tag ID{tag_id}({joint})'
|
||||
|
||||
|
||||
def feedback_issues(profile, samples, count, tolerance, indices=None):
|
||||
"""The same check supplies both the readiness decision and its explanation."""
|
||||
if len(samples) < count:
|
||||
return [{'source': 'feedback', 'reason': f'新反馈不足:{len(samples)}/{count} 帧'}]
|
||||
spread = np.ptp(np.asarray([sample.positions for sample in samples]), axis=0)
|
||||
return [{'source': 'feedback', 'joint': joint.name,
|
||||
'reason': f'反馈未稳定:变化 {spread[joint.index]:g} > 允许 {tolerance:g}',
|
||||
'spread': float(spread[joint.index]), 'limit': tolerance}
|
||||
for joint in profile.joints
|
||||
if (indices is None or joint.index in indices) and spread[joint.index] > tolerance]
|
||||
|
||||
|
||||
def describe_issue(issue):
|
||||
if issue['source'] == 'tag':
|
||||
label = tag_label(issue['view'], issue['tag_id'], issue['joint'])
|
||||
else:
|
||||
label = f'关节 {issue["joint"]}' if 'joint' in issue else 'SDK 反馈'
|
||||
return f'{label}:{issue["reason"]}'
|
||||
|
||||
|
||||
def shared_translation(issues):
|
||||
"""Describe aligned Tag motion for diagnosis only; never correct observations.
|
||||
|
||||
Similar magnitudes alone are not evidence of shared motion. Require at least
|
||||
three distinct IDs in one view and compare their corners at common times.
|
||||
The result cannot identify which part of the camera/hand setup is moving.
|
||||
"""
|
||||
views = {}
|
||||
for issue in issues:
|
||||
if issue['source'] == 'tag' and issue.get('window_samples'):
|
||||
views.setdefault(issue['view'], {})[issue['tag_id']] = {
|
||||
frame['stamp_ns']: frame['corners'] for frame in issue['window_samples']}
|
||||
reports = []
|
||||
for view, tags in views.items():
|
||||
if len(tags) < 3:
|
||||
continue
|
||||
stamps = sorted(set.intersection(*(set(frames) for frames in tags.values())))
|
||||
if len(stamps) < 4:
|
||||
continue
|
||||
ids = sorted(tags)
|
||||
points = np.asarray([[tags[tag][stamp] for stamp in stamps] for tag in ids], dtype=float)
|
||||
offsets = points - np.median(points, axis=1, keepdims=True)
|
||||
translation = np.median(np.mean(offsets, axis=2), axis=0)
|
||||
energy = float(np.sum(offsets ** 2))
|
||||
if energy <= 1e-12:
|
||||
continue
|
||||
residual = offsets - translation[None, :, None, :]
|
||||
fraction = 1 - float(np.sum(residual ** 2)) / energy
|
||||
if fraction < .95:
|
||||
continue
|
||||
span = np.ptp(translation, axis=0)
|
||||
reason = (f'{view_label(view)}:多个 Tag 出现共同平移(位移占比 {fraction:.1%},'
|
||||
f'水平往返 {span[0]:.2f}、垂直往返 {span[1]:.2f} 像素);'
|
||||
'请检查相机和手掌固定处是否振动。')
|
||||
reports.append({'view': view, 'tag_ids': ids, 'frames': len(stamps),
|
||||
'shared_fraction': fraction, 'horizontal_span_px': float(span[0]),
|
||||
'vertical_span_px': float(span[1]), 'reason': reason})
|
||||
return reports
|
||||
@@ -0,0 +1,154 @@
|
||||
"""Measure each endpoint at the first witnessed visual displacement."""
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .errors import Unmeasurable
|
||||
from ..vision.observations import rms_delta
|
||||
from ..vision.motion import transient_evidence, witnessed_motion
|
||||
from ..vision.reference import jitter_radius
|
||||
|
||||
|
||||
SCAN_METHOD = 'endpoint_search_v10'
|
||||
ENDPOINT_METHODS = (SCAN_METHOD, *(f'endpoint_search_v{version}' for version in range(9, 0, -1)))
|
||||
|
||||
|
||||
class EndpointSearch:
|
||||
"""Compare stable corners with the fixed reference at the command limit.
|
||||
|
||||
The engine verifies a quiet reference and supplies stable positions plus
|
||||
all post-command frames. Settled position and transient motion have
|
||||
different noise envelopes; neither uses image-space direction.
|
||||
"""
|
||||
|
||||
def __init__(self, joint, direction, settings, scan_method=SCAN_METHOD):
|
||||
if direction not in ('up', 'down'):
|
||||
raise ValueError('端点搜索方向无效')
|
||||
self.joint, self.direction, self.settings = joint, direction, settings
|
||||
self.scan_method = scan_method
|
||||
self.sign = 1 if direction == 'up' else -1
|
||||
self.origin = joint.minimum if direction == 'up' else joint.maximum
|
||||
self.terminal = joint.maximum if direction == 'up' else joint.minimum
|
||||
self.steps = 0
|
||||
self.reference, self.threshold = None, None
|
||||
self.reference_radius = 0.0
|
||||
self.evidence = None
|
||||
self.boundary, self.error = None, None
|
||||
|
||||
@property
|
||||
def done(self):
|
||||
return self.boundary is not None or self.error is not None
|
||||
|
||||
@property
|
||||
def next_command(self):
|
||||
return self.origin + self.sign * self.steps * self.joint.resolution
|
||||
|
||||
def add(self, command, observation):
|
||||
if self.done:
|
||||
return
|
||||
command = self.joint.validate_value(command)
|
||||
if not math.isclose(command, self.next_command, rel_tol=0, abs_tol=1e-6):
|
||||
raise Unmeasurable('端点搜索采样必须从指令端点按分辨率连续推进,不能缺点或重复')
|
||||
points = np.asarray(observation['corners'], dtype=float)
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
raise Unmeasurable('端点角点观测无效')
|
||||
if self.reference is None:
|
||||
noise = float(observation['noise'])
|
||||
if not math.isfinite(noise) or noise < 0:
|
||||
raise Unmeasurable('端点噪声估计无效')
|
||||
self.reference = points.copy()
|
||||
self.threshold = self.settings.motion_floor_px
|
||||
if self.scan_method != SCAN_METHOD:
|
||||
self.threshold = max(self.threshold, self.settings.noise_multiplier * noise)
|
||||
if observation.get('window_samples'):
|
||||
self.reference_radius = jitter_radius([f['corners'] for f in observation['window_samples']])
|
||||
else:
|
||||
displacement = rms_delta(points, self.reference)
|
||||
if self.scan_method == SCAN_METHOD:
|
||||
transient = transient_evidence(observation.get('motion_trace'), self.reference,
|
||||
self.reference_radius, observation.get('window_samples'),
|
||||
self.settings)
|
||||
else:
|
||||
transient = witnessed_motion(observation.get('motion'), self.reference, self.threshold,
|
||||
self.settings.motion_frames, self.settings.freshness)
|
||||
if displacement > self.threshold or transient:
|
||||
self.evidence = {'source': 'settled' if displacement > self.threshold else 'transient',
|
||||
'settled_displacement_px': displacement, 'transient': transient}
|
||||
self.boundary = {
|
||||
'bound': self.joint.validate_value(command - self.sign * self.joint.resolution),
|
||||
'trigger': command, 'confirmed_at': command, 'threshold': self.threshold,
|
||||
}
|
||||
self.steps += 1
|
||||
if self.boundary is None and math.isclose(command, self.terminal, rel_tol=0, abs_tol=1e-6):
|
||||
end = '低端' if self.direction == 'up' else '高端'
|
||||
self.error = end + '已扫描至对端,未检测到超过阈值的运动'
|
||||
|
||||
def result(self):
|
||||
if self.boundary is None:
|
||||
raise Unmeasurable(self.error or '端点搜索数据不足,尚未检测到起动')
|
||||
return dict(self.boundary)
|
||||
|
||||
|
||||
def aggregate_endpoints(joint, endpoints, settings):
|
||||
expected = {(r, d) for r in range(settings.endpoint_repetitions) for d in ('up', 'down')}
|
||||
if set(endpoints) != expected:
|
||||
raise Unmeasurable('两端边界数据不完整')
|
||||
limits = {}
|
||||
for direction, bound in (('up', 'min'), ('down', 'max')):
|
||||
values = [value['bound'] for (repeat, side), value in endpoints.items() if side == direction]
|
||||
if max(values) - min(values) > settings.repeat_tolerance:
|
||||
raise Unmeasurable(f'{bound} 端点复测差异过大')
|
||||
limits[bound] = joint.validate_value(max(values) if bound == 'min' else min(values))
|
||||
if limits['min'] >= limits['max']:
|
||||
raise Unmeasurable('两端边界重叠或复测区间无有效交集')
|
||||
return {**limits, 'status': '成功',
|
||||
'endpoints': {f'{r}_{d}': value for (r, d), value in endpoints.items()}}
|
||||
|
||||
|
||||
def _replay_search(joint, direction, settings, scan_method):
|
||||
if scan_method in (SCAN_METHOD, 'endpoint_search_v9', 'endpoint_search_v8'):
|
||||
return EndpointSearch(joint, direction, settings, scan_method)
|
||||
if scan_method not in ENDPOINT_METHODS:
|
||||
raise ValueError(f'不支持的日志扫描方法: {scan_method}')
|
||||
# Historical confirmation rules are isolated from the live scan path.
|
||||
from .legacy_endpoints import EndpointSearch as LegacyEndpointSearch
|
||||
return LegacyEndpointSearch(joint, direction, settings, scan_method)
|
||||
|
||||
|
||||
def analyze_endpoint_samples(joint, sweeps, settings, scan_method=SCAN_METHOD):
|
||||
endpoints = {}
|
||||
for repeat in range(settings.endpoint_repetitions):
|
||||
for direction in ('up', 'down'):
|
||||
search = _replay_search(joint, direction, settings, scan_method)
|
||||
for sample in sweeps.get((repeat, direction), []):
|
||||
if 'search_commands' in sample:
|
||||
if joint.name not in sample['search_commands']:
|
||||
continue
|
||||
command = sample['search_commands'][joint.name]
|
||||
else: # Historical logs used the same command for all channels.
|
||||
command = sample['command']
|
||||
observed = sample['observations'][joint.name]
|
||||
if scan_method == SCAN_METHOD and 'window_samples' in observed:
|
||||
observed = _recompute_window(observed, settings, search.reference is None)
|
||||
search.add(command, observed)
|
||||
endpoints[(repeat, direction)] = search.result()
|
||||
return aggregate_endpoints(joint, endpoints, settings)
|
||||
|
||||
|
||||
def _recompute_window(observed, settings, reference):
|
||||
"""Recheck the recorded window instead of trusting a saved stable flag."""
|
||||
from ..vision.observations import Observation, StableWindow
|
||||
from ..vision.reference import ReferenceWindow
|
||||
window = (ReferenceWindow(settings) if reference
|
||||
else StableWindow(settings.stable_frames, settings.stable_seconds))
|
||||
for frame in observed['window_samples']:
|
||||
points = np.asarray(frame['corners'], dtype=float)
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
raise Unmeasurable('日志包含无效的稳定窗口角点')
|
||||
if not window.add(Observation(frame['stamp_ns'], 0, frame['corners'])):
|
||||
raise Unmeasurable('日志稳定窗口时间戳重复或倒退')
|
||||
measured = window.summary(settings.stability_noise_px, settings.stability_drift_px,
|
||||
settings.stability_drift_sigma)
|
||||
if measured is None:
|
||||
raise Unmeasurable('日志的端点参考或采样窗口不满足稳定条件')
|
||||
return {**observed, **measured}
|
||||
@@ -0,0 +1,500 @@
|
||||
"""Event-driven calibration engine. Only this object owns motion commands."""
|
||||
from collections import deque
|
||||
|
||||
from .angle_scan import SCAN_METHOD, TaskAngleScan, ancestor_angles, angle_targets
|
||||
from .diagnostics import describe_issue, feedback_issues, shared_translation, tag_label
|
||||
from .trajectory import Trajectory
|
||||
from ..angle_model import edge_angle, unwrap_edge_angle
|
||||
from ..vision.observations import Observation, StableWindow
|
||||
|
||||
|
||||
class Engine:
|
||||
AUTOMATIC = {'PREPARING', 'SCANNING', 'RESTORING'}
|
||||
ACTIVE = AUTOMATIC | {'TEACHING'}
|
||||
|
||||
def __init__(self, profile, settings, adapter, angle_model=None, emit=lambda event: None):
|
||||
self.profile, self.settings, self.adapter = profile, settings, adapter
|
||||
self.angle_model, self.emit = angle_model, emit
|
||||
self.state, self.reason = 'IDLE', ''
|
||||
self.command = None
|
||||
self.task = None
|
||||
self.tasks = []
|
||||
self.task_index = 0
|
||||
self.results = _blank_results(profile, angle_model)
|
||||
self.finished_curves = {}
|
||||
self.angle_task = None
|
||||
self.latest, self.view_stamps = {}, {}
|
||||
self.rejected_observations = {}
|
||||
self.feedbacks = deque(maxlen=64)
|
||||
self.windows = {}
|
||||
self.motion_captures = {}
|
||||
self.trajectory = None
|
||||
self.restoration_steps, self.restoring_joint = deque(), ''
|
||||
self.manual_target, self.manual_uid = None, None
|
||||
self.manual_pending, self.manual_gate_ns = False, 0
|
||||
self.manual_finished = float('-inf')
|
||||
self.phase = ''
|
||||
self.stage, self.direction, self.repeat = '', '', 0
|
||||
self.segment = None
|
||||
self.progress = (0, 0)
|
||||
self.last_diagnostic_warning = None
|
||||
|
||||
@property
|
||||
def active(self):
|
||||
return self.state in self.ACTIVE
|
||||
|
||||
@property
|
||||
def manual_allowed(self):
|
||||
return self.state in ('IDLE', 'TEACHING', 'CANCELLED', 'COMPLETED')
|
||||
|
||||
def _require_control(self, now):
|
||||
reason = self.adapter.control_error(now)
|
||||
if reason:
|
||||
raise ValueError(reason)
|
||||
|
||||
def _record(self, kind, **data):
|
||||
self.emit({'kind': kind, **data})
|
||||
|
||||
def start(self, teaching, now, stamp_ns, task_names=None):
|
||||
if self.active or self.state == 'PAUSED':
|
||||
raise ValueError('请先结束当前任务')
|
||||
self._require_control(now)
|
||||
self.tasks = [t for t in self.profile.tasks if task_names is None or t.name in task_names]
|
||||
if not self.tasks:
|
||||
raise ValueError('没有选中的任务')
|
||||
if task_names and set(task_names) != {t.name for t in self.tasks}:
|
||||
raise ValueError('包含未知任务')
|
||||
if teaching.missing(self.tasks):
|
||||
raise ValueError('缺少示教: ' + ', '.join(teaching.missing(self.tasks)))
|
||||
self.teaching, self.session_uid, self.task_index = teaching, self.adapter.uid, 0
|
||||
if self.angle_model is None:
|
||||
raise ValueError('没有工装坐标系下的关节轴,不能测量角度')
|
||||
self.results = _blank_results(self.profile, self.angle_model)
|
||||
self.finished_curves = {}
|
||||
self.command = self.profile.vector(self.adapter.feedback.positions)
|
||||
self.last_diagnostic_warning = None
|
||||
self._begin_task(now, stamp_ns)
|
||||
|
||||
def _begin_task(self, now, stamp_ns):
|
||||
self.restoration_steps.clear()
|
||||
self.restoring_joint = ''
|
||||
self.task = self.tasks[self.task_index]
|
||||
self.prepare_pose = self.teaching.prepare(self.task)
|
||||
self.specs = [self.profile.by_name[name] for name in self.task.joints]
|
||||
self._set_observation_channels(self.task.joints)
|
||||
self.segment, self.segment_index = None, -1
|
||||
self.angle_task = None
|
||||
self.failures = {}
|
||||
self.reason = ''
|
||||
self.stage, self.direction, self.repeat = 'prepare', '', 0
|
||||
self.progress = (0, len(self.task.joints))
|
||||
self.state = 'PREPARING'
|
||||
self._record('task_started', task=self.task.name, joints=list(self.task.joints), scan_method=SCAN_METHOD)
|
||||
self._move(self.prepare_pose, now)
|
||||
|
||||
def _move(self, target, now):
|
||||
self.motion_captures = {}
|
||||
self.trajectory = Trajectory(self.profile, self.command, target, self.settings.rate, now,
|
||||
coordinated=self.state == 'SCANNING')
|
||||
self.phase = 'moving'
|
||||
self.blocked_since = None
|
||||
self.unstable_reported = set()
|
||||
self.windows = {name: StableWindow(self.settings.stable_frames, self.settings.stable_seconds)
|
||||
for name in self.keys}
|
||||
|
||||
def _set_observation_channels(self, names):
|
||||
joints = [self.profile.by_name[name] for name in names]
|
||||
self.active_indices = {joint.index for joint in joints}
|
||||
self.keys = {joint.name: (joint.view, joint.tag_id) for joint in joints}
|
||||
self.windows = {name: StableWindow(self.settings.stable_frames, self.settings.stable_seconds)
|
||||
for name in names}
|
||||
|
||||
def observe(self, view, stamp_ns, accepted, now, rejected=None):
|
||||
"""One whole detector frame; missing IDs explicitly invalidate observations."""
|
||||
if stamp_ns <= self.view_stamps.get(view, -1):
|
||||
return
|
||||
self.view_stamps[view] = stamp_ns
|
||||
for key in [k for k in self.latest if k[0] == view]:
|
||||
self.latest.pop(key)
|
||||
for key in [k for k in self.rejected_observations if k[0] == view]:
|
||||
self.rejected_observations.pop(key)
|
||||
for tag_id, reason in (rejected or {}).items():
|
||||
self.rejected_observations[(view, tag_id)] = (now, reason)
|
||||
for tag_id, corners in accepted.items():
|
||||
self.latest[(view, tag_id)] = Observation(stamp_ns, now, tuple(tuple(p) for p in corners))
|
||||
if self.state == 'SCANNING':
|
||||
for name, capture in self.motion_captures.items():
|
||||
key = self.keys[name]
|
||||
if key[0] == view:
|
||||
capture.observe(stamp_ns, self.latest.get(key))
|
||||
if self.state not in ('PREPARING', 'SCANNING') or self.phase != 'waiting':
|
||||
return
|
||||
required = [(name, key) for name, key in self.keys.items() if key[0] == view]
|
||||
if any(key not in self.latest for _, key in required):
|
||||
for window in self.windows.values():
|
||||
window.clear()
|
||||
return
|
||||
for name, key in required:
|
||||
obs = self.latest[key]
|
||||
if obs.stamp_ns >= self.gate_ns:
|
||||
self.windows[name].add(obs)
|
||||
|
||||
def feedback_stable(self, after_ns=0, indices=None):
|
||||
"""Check selected channels; preparation and teaching use the whole hand."""
|
||||
return not self._feedback_issues(after_ns, indices)
|
||||
|
||||
def _feedback_issues(self, after_ns=0, indices=None):
|
||||
count = self.settings.stable_frames
|
||||
selected = [f for f in self.feedbacks if f.stamp_ns >= after_ns][-count:]
|
||||
return feedback_issues(self.profile, selected, count, self.settings.feedback_tolerance, indices)
|
||||
|
||||
def can_capture_teaching(self, now):
|
||||
return (not self.active and self.state != 'PAUSED' and self.feedback_stable(self.manual_gate_ns)
|
||||
and now-self.manual_finished >= self.settings.settle_seconds
|
||||
and not self.adapter.control_error(now))
|
||||
|
||||
def manual_move(self, target, now):
|
||||
"""Queue the operator's latest target; scan trajectories do not shape teaching."""
|
||||
if not self.manual_allowed:
|
||||
raise ValueError('标定或暂停期间不能手动调节')
|
||||
target = self.profile.vector(target)
|
||||
self._require_control(now)
|
||||
if self.state == 'TEACHING' and self.adapter.uid != self.manual_uid:
|
||||
raise ValueError('设备 UID 改变,请取消后重新示教')
|
||||
self.manual_target, self.manual_pending = target, True
|
||||
self.manual_uid = self.adapter.uid
|
||||
self.trajectory = None
|
||||
self.tasks, self.task, self.task_index = [], None, 0
|
||||
self.stage, self.direction, self.repeat, self.progress = '', '', 0, (0, 0)
|
||||
self.state, self.reason = 'TEACHING', ''
|
||||
|
||||
def manual_adjust(self, targets, now):
|
||||
"""Change selected joints while retaining every other current target."""
|
||||
if self.state == 'TEACHING':
|
||||
target = list(self.manual_target)
|
||||
else:
|
||||
target = list(self.command if self.command is not None else self.adapter.feedback.positions)
|
||||
for name, value in targets.items():
|
||||
joint = self.profile.by_name.get(name)
|
||||
if joint is None:
|
||||
raise ValueError(f'未知关节: {name}')
|
||||
target[joint.index] = joint.validate_value(value)
|
||||
self.manual_move(target, now)
|
||||
|
||||
def pause(self, reason):
|
||||
self.state, self.reason = 'PAUSED', str(reason)
|
||||
self.trajectory = None
|
||||
self.manual_target, self.manual_pending = None, False
|
||||
for window in self.windows.values():
|
||||
window.clear()
|
||||
self._record('paused', reason=self.reason)
|
||||
|
||||
def resume(self, now, stamp_ns):
|
||||
if self.state != 'PAUSED' or not self.tasks:
|
||||
raise ValueError('没有可恢复的扫描任务,请取消后重新操作')
|
||||
self._require_control(now)
|
||||
if self.adapter.uid != self.session_uid:
|
||||
raise ValueError('设备 UID 已改变,不能继续该会话')
|
||||
# Always reacquire the interrupted task, never combine incompatible passes.
|
||||
self.command = self.profile.vector(self.adapter.feedback.positions)
|
||||
self._begin_task(now, stamp_ns)
|
||||
|
||||
def cancel(self):
|
||||
self.state, self.reason, self.trajectory = 'CANCELLED', '已取消,保持最后目标', None
|
||||
self.manual_target, self.manual_pending = None, False
|
||||
self._record('cancelled')
|
||||
|
||||
def _visible(self, now):
|
||||
return all(key in self.latest and now-self.latest[key].received <= self.settings.freshness
|
||||
for key in self.keys.values())
|
||||
|
||||
def _visibility_issues(self, now):
|
||||
issues = []
|
||||
for name, (view, tag_id) in self.keys.items():
|
||||
key = (view, tag_id)
|
||||
observation = self.latest.get(key)
|
||||
rejected = self.rejected_observations.get(key)
|
||||
if rejected and now - rejected[0] <= self.settings.freshness:
|
||||
reason = rejected[1]
|
||||
elif observation is None:
|
||||
reason = '未检出有效 Tag'
|
||||
elif now - observation.received > self.settings.freshness:
|
||||
reason = f'有效观测已过期:{now - observation.received:.2f} 秒未更新'
|
||||
elif self.phase == 'waiting' and observation.stamp_ns < self.gate_ns:
|
||||
reason = '尚未收到目标到达并等待后的新图像'
|
||||
else:
|
||||
continue
|
||||
issues.append({'source': 'tag', 'joint': name, 'view': view, 'tag_id': tag_id,
|
||||
'reason': reason})
|
||||
return issues
|
||||
|
||||
@property
|
||||
def collecting_reference(self):
|
||||
return False
|
||||
|
||||
@property
|
||||
def waiting_timeout(self):
|
||||
return (self.settings.reference_timeout if self.collecting_reference
|
||||
else self.settings.point_timeout)
|
||||
|
||||
def _pause_sampling_timeout(self, now, moving=False):
|
||||
restoring = self.state == 'RESTORING'
|
||||
limit = self.settings.point_timeout if moving else self.waiting_timeout
|
||||
elapsed = now - (self.blocked_since if moving else self.arrived)
|
||||
issues = [] if restoring else self._visibility_issues(now)
|
||||
if not moving:
|
||||
indices = self.active_indices if self.state == 'SCANNING' else None
|
||||
issues.extend(self._feedback_issues(self.gate_ns, indices))
|
||||
# Missing observations or unstable feedback reset all image windows.
|
||||
# Report those primary blockers instead of blaming every cleared Tag.
|
||||
if not issues and not moving and not restoring and self.stage != 'segment_finish':
|
||||
for name, window in self.windows.items():
|
||||
problem = window.problem(self.settings.stability_noise_px, self.settings.stability_drift_px,
|
||||
self.settings.stability_drift_sigma)
|
||||
if problem:
|
||||
view, tag_id = self.keys[name]
|
||||
issues.append({'source': 'tag', 'joint': name, 'view': view,
|
||||
'tag_id': tag_id, 'window_samples': window.snapshot(), **problem})
|
||||
if moving:
|
||||
prefix = '目标 Tag 持续不可见或观测过期'
|
||||
elif restoring:
|
||||
prefix = '测后回位稳定超时'
|
||||
elif self.collecting_reference:
|
||||
prefix = f'端点参考未稳定:等待超时(已保持目标 {elapsed:.1f} 秒,上限 {limit:g} 秒)'
|
||||
else:
|
||||
prefix = '单点稳定采样超时'
|
||||
details = '\n'.join(describe_issue(issue) for issue in issues)
|
||||
shared_motion = shared_translation(issues)
|
||||
if shared_motion:
|
||||
details += '\n' + '\n'.join(report['reason'] for report in shared_motion)
|
||||
if not details:
|
||||
# Conditions can recover on the tick that crosses the deadline.
|
||||
context = self.restoring_joint if restoring else '、'.join(
|
||||
tag_label(view, tag_id, name) for name, (view, tag_id) in self.keys.items())
|
||||
details = f'{context}:条件刚恢复,但已超过 {limit:g} 秒等待时限'
|
||||
self._record('sampling_timeout', time=now, task=self.task.name, stage=self.stage,
|
||||
direction=self.direction, repeat=self.repeat, target=list(self.command),
|
||||
elapsed_seconds=elapsed, timeout_seconds=limit,
|
||||
issues=issues, shared_motion=shared_motion, reason=details)
|
||||
self.pause(prefix + '\n' + details)
|
||||
|
||||
def tick(self, now, stamp_ns):
|
||||
feedback = self.adapter.feedback
|
||||
if feedback and (not self.feedbacks or feedback.stamp_ns > self.feedbacks[-1].stamp_ns):
|
||||
self.feedbacks.append(feedback)
|
||||
if not self.active:
|
||||
return
|
||||
if self.state in self.AUTOMATIC:
|
||||
warning = self.adapter.diagnostic_warning()
|
||||
if warning != self.last_diagnostic_warning:
|
||||
self._record('diagnostic_warning', reason=warning, time=now, stamp_ns=stamp_ns,
|
||||
task=self.task.name, stage=self.stage, direction=self.direction,
|
||||
repeat=self.repeat, target=list(self.command),
|
||||
feedback=list(feedback.positions) if feedback else None)
|
||||
self.last_diagnostic_warning = warning
|
||||
error = self.adapter.control_error(now)
|
||||
expected_uid = self.manual_uid if self.state == 'TEACHING' else self.session_uid
|
||||
if not error and self.adapter.uid != expected_uid:
|
||||
error = '设备 UID 改变'
|
||||
if error:
|
||||
self.pause(error)
|
||||
return
|
||||
if self.state == 'TEACHING':
|
||||
self._tick_manual(now, stamp_ns)
|
||||
return
|
||||
if self.phase == 'moving':
|
||||
hold = self.state == 'SCANNING' and not self._visible(now)
|
||||
if hold:
|
||||
if self.blocked_since is None:
|
||||
self.blocked_since = now
|
||||
if now-self.blocked_since > self.settings.point_timeout:
|
||||
self._pause_sampling_timeout(now, moving=True)
|
||||
return
|
||||
else:
|
||||
self.blocked_since = None
|
||||
values, done = self.trajectory.advance(now, hold=hold)
|
||||
if not hold:
|
||||
self._send(values, now, stamp_ns)
|
||||
if done and not hold:
|
||||
self.phase = 'waiting'
|
||||
self.arrived = now
|
||||
self.gate_ns = stamp_ns + int(self.settings.settle_seconds*1e9)
|
||||
for window in self.windows.values():
|
||||
window.clear()
|
||||
return
|
||||
if now-self.arrived > self.waiting_timeout:
|
||||
self._pause_sampling_timeout(now)
|
||||
return
|
||||
if self.state == 'RESTORING':
|
||||
if self.feedback_stable(self.gate_ns):
|
||||
self._record('restored', task=self.task.name, joints=[self.restoring_joint],
|
||||
target=list(self.command),
|
||||
feedback=list(feedback.positions), stamp_ns=stamp_ns)
|
||||
self._next_restoration(now)
|
||||
return
|
||||
feedback_indices = self.active_indices if self.state == 'SCANNING' else None
|
||||
if not self._visible(now) or not self.feedback_stable(self.gate_ns, feedback_indices):
|
||||
for window in self.windows.values():
|
||||
window.clear()
|
||||
return
|
||||
summaries = {name: window.summary(self.settings.stability_noise_px, self.settings.stability_drift_px,
|
||||
self.settings.stability_drift_sigma)
|
||||
for name, window in self.windows.items()}
|
||||
if any(value is None for value in summaries.values()):
|
||||
for name, summary in summaries.items():
|
||||
quality = self.windows[name].last_quality
|
||||
if summary is None and quality and name not in self.unstable_reported:
|
||||
view, tag_id = self.keys[name]
|
||||
self._record('sampling_unstable', task=self.task.name, joint=name,
|
||||
view=view, tag_id=tag_id, target=list(self.command),
|
||||
window_samples=self.windows[name].snapshot(), **quality)
|
||||
self.unstable_reported.add(name)
|
||||
return
|
||||
if self.state == 'PREPARING':
|
||||
self._record('prepared', task=self.task.name, target=list(self.command),
|
||||
feedback=list(feedback.positions))
|
||||
self.state = 'SCANNING'
|
||||
self._begin_angle_scan(now)
|
||||
else:
|
||||
self._accept_angle_sample(summaries, now)
|
||||
|
||||
def _tick_manual(self, now, stamp_ns):
|
||||
if self.manual_pending:
|
||||
self._send(self.manual_target, now, stamp_ns)
|
||||
self.manual_pending = False
|
||||
self.manual_finished = now
|
||||
self.manual_gate_ns = stamp_ns + int(self.settings.settle_seconds * 1e9)
|
||||
elif (now - self.manual_finished >= self.settings.settle_seconds and
|
||||
self.feedback_stable(self.manual_gate_ns)):
|
||||
# A published target does not mean that the real hand has settled.
|
||||
self.state = 'IDLE'
|
||||
|
||||
def _send(self, values, now, stamp_ns):
|
||||
self.adapter.send_positions(values)
|
||||
self.command = tuple(values)
|
||||
self._record('command', target=list(values), stamp_ns=stamp_ns, time=now,
|
||||
state=self.state, task=self.task.name if self.task else None)
|
||||
def _begin_angle_scan(self, now):
|
||||
self.angle_task = TaskAngleScan(
|
||||
self.task, self.profile, self.settings, self.angle_model.kinematics, self.prepare_pose)
|
||||
self._follow_angle_goal(now)
|
||||
|
||||
def _follow_angle_goal(self, now):
|
||||
scan = self.angle_task.joint_scan
|
||||
self.specs = [scan.joint]
|
||||
self._set_observation_channels((scan.joint.name,))
|
||||
self.stage = scan.phase
|
||||
self.direction = 'up' if scan.phase in ('low_anchor', 'low_search') else 'down'
|
||||
self.segment = None
|
||||
done, total = self.angle_task.progress()
|
||||
self.progress = (done, total)
|
||||
self._move(self.angle_task.goal(), now)
|
||||
|
||||
def _accept_angle_sample(self, summaries, now):
|
||||
name = self.angle_task.joint_name
|
||||
summary = summaries[name]
|
||||
pose = list(self.angle_task.goal())
|
||||
curves = {**self.finished_curves, **{key: value for key, value in self.angle_task.outputs.items() if value}}
|
||||
samples = self.windows[name].snapshot()
|
||||
jump = _window_phi_jump(samples)
|
||||
reading = self.angle_model.measure(
|
||||
summary['corners'], joint=name, camera=self.keys[name][0],
|
||||
ancestor_angles=ancestor_angles(
|
||||
self.angle_model.kinematics, name, pose, self.profile, curves),
|
||||
previous_phi=None)
|
||||
reason = 'corner_jump' if jump else reading.reason
|
||||
self._record('angle_sample', task=self.task.name, joint=name,
|
||||
command=self.angle_task.joint_scan.command, phase=self.angle_task.joint_scan.phase,
|
||||
theta_rad=None if jump else reading.theta_rad,
|
||||
phi_rad=reading.phi_rad, reason=reason,
|
||||
corners=summary['corners'])
|
||||
if reason == 'corner_jump':
|
||||
for window in self.windows.values():
|
||||
window.clear()
|
||||
return
|
||||
if reason:
|
||||
self.pause(f'{name} 角度无效:{reason}')
|
||||
return
|
||||
status = self.angle_task.accept(reading.theta_rad)
|
||||
if status == 'task_done':
|
||||
self._finish_angle_task(now)
|
||||
return
|
||||
self._follow_angle_goal(now)
|
||||
|
||||
def _finish_angle_task(self, now):
|
||||
blank = _blank_results(self.profile, self.angle_model)
|
||||
for name in self.task.joints:
|
||||
outcome = self.angle_task.outputs.get(name)
|
||||
if outcome is None:
|
||||
failed = dict(blank[name])
|
||||
failed['status'] = '失败'
|
||||
failed['reason'] = self.angle_task.errors.get(name, '数据不足')
|
||||
self.results[name] = failed
|
||||
continue
|
||||
self.results[name] = {
|
||||
'angles_rad': outcome['angles_rad'],
|
||||
'commands': outcome['commands'],
|
||||
'status': '成功',
|
||||
}
|
||||
self.finished_curves[name] = self.results[name]
|
||||
self._record('task_result', task=self.task.name,
|
||||
results={name: self.results[name] for name in self.task.joints},
|
||||
travel={name: {'min': value['travel_min'], 'max': value['travel_max']}
|
||||
for name, value in self.angle_task.outputs.items() if value})
|
||||
self.restoration_steps = deque(self.teaching.restoration_steps(self.task, self.command))
|
||||
self._next_restoration(now)
|
||||
|
||||
def _next_restoration(self, now):
|
||||
while self.restoration_steps:
|
||||
name, target = self.restoration_steps.popleft()
|
||||
if target == self.command:
|
||||
continue
|
||||
self.restoring_joint = name
|
||||
self.state, self.stage, self.direction = 'RESTORING', 'restore', ''
|
||||
self._record('restoration_started', task=self.task.name, joints=[name],
|
||||
target=list(target))
|
||||
self._move(target, now)
|
||||
return
|
||||
self.restoring_joint = ''
|
||||
self._advance_task(now)
|
||||
|
||||
def _advance_task(self, now):
|
||||
self.task_index += 1
|
||||
if self.task_index == len(self.tasks):
|
||||
self.state, self.reason = 'COMPLETED', '标定结束,请查看每个关节的结果'
|
||||
self._record('completed')
|
||||
else:
|
||||
self._begin_task(now, 0)
|
||||
|
||||
|
||||
def _window_phi_jump(samples):
|
||||
"""同一保持点里,相邻帧的贴纸边转角跳过 35° 就不算到达。"""
|
||||
previous = None
|
||||
for item in samples:
|
||||
try:
|
||||
phi = edge_angle(item['corners'])
|
||||
except ValueError:
|
||||
return True
|
||||
if previous is None:
|
||||
previous = phi
|
||||
continue
|
||||
unwrapped = unwrap_edge_angle(previous, phi)
|
||||
if unwrapped is None:
|
||||
return True
|
||||
previous = unwrapped
|
||||
return False
|
||||
|
||||
|
||||
def _blank_results(profile, angle_model):
|
||||
if angle_model is None:
|
||||
return {joint.name: {'angles_rad': None, 'commands': [None] * 10, 'status': '未完成'}
|
||||
for joint in profile.joints}
|
||||
results = {}
|
||||
for joint in profile.joints:
|
||||
lower, upper = angle_model.kinematics.limits(joint.name)
|
||||
results[joint.name] = {
|
||||
'angles_rad': [float(value) for value in angle_targets(lower, upper)],
|
||||
'commands': [None] * 10,
|
||||
'status': '未完成',
|
||||
}
|
||||
return results
|
||||
@@ -0,0 +1,5 @@
|
||||
"""Shared measurement errors for live scans and offline analysis."""
|
||||
|
||||
|
||||
class Unmeasurable(ValueError):
|
||||
pass
|
||||
@@ -0,0 +1,218 @@
|
||||
"""Frozen v1-v7 algorithms used only to reproduce historical journals."""
|
||||
from collections import deque
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .errors import Unmeasurable
|
||||
from ..vision.observations import rms_delta
|
||||
|
||||
|
||||
SCAN_METHOD = 'endpoint_search_v7'
|
||||
ENDPOINT_METHODS = (SCAN_METHOD, 'endpoint_search_v6', 'endpoint_search_v5', 'endpoint_search_v4',
|
||||
'endpoint_search_v3', 'endpoint_search_v2', 'endpoint_search_v1')
|
||||
|
||||
|
||||
class _MotionContinuation:
|
||||
"""Confirm continued travel independently of the first departure direction.
|
||||
|
||||
The first displacement seeds a direction. A later significant turn replaces
|
||||
that direction, then needs further travel to confirm it. This neither moves
|
||||
the onset command nor accepts alternating offsets as continued motion.
|
||||
"""
|
||||
|
||||
def __init__(self, reference, departure, threshold):
|
||||
self.anchor = departure.copy()
|
||||
self.axis = departure - reference
|
||||
self.threshold = threshold
|
||||
|
||||
def observe(self, points):
|
||||
displacement = points - self.anchor
|
||||
distance = float(np.sqrt(np.mean(np.sum(self.axis ** 2, axis=1))))
|
||||
projection = float(np.mean(np.sum(displacement * self.axis, axis=1)) / distance)
|
||||
if projection > self.threshold:
|
||||
return True
|
||||
if rms_delta(points, self.anchor) > self.threshold:
|
||||
self.axis = displacement
|
||||
self.anchor = points.copy()
|
||||
return False
|
||||
|
||||
|
||||
class EndpointSearch:
|
||||
"""Confirm departure with continued travel from a stable endpoint platform.
|
||||
|
||||
Observations are stable summaries at consecutive command values. Confirmation
|
||||
retains the first trigger; later points never move an already confirmed bound.
|
||||
"""
|
||||
|
||||
def __init__(self, joint, direction, settings, scan_method=SCAN_METHOD):
|
||||
if direction not in ('up', 'down'):
|
||||
raise ValueError('端点搜索方向无效')
|
||||
if scan_method not in ENDPOINT_METHODS:
|
||||
raise ValueError('端点搜索方法无效')
|
||||
self.joint, self.direction, self.settings = joint, direction, settings
|
||||
self.scan_method = scan_method
|
||||
self.require_progress = scan_method not in ('endpoint_search_v1', 'endpoint_search_v2')
|
||||
self.recent_plateau = scan_method not in ('endpoint_search_v1', 'endpoint_search_v2', 'endpoint_search_v3')
|
||||
self.recover_subthreshold_offset = scan_method in ('endpoint_search_v5', 'endpoint_search_v6', 'endpoint_search_v7')
|
||||
self.reject_opposed_candidate = scan_method == 'endpoint_search_v6'
|
||||
self.follow_continuation = scan_method == 'endpoint_search_v7'
|
||||
self.sign = 1 if direction == 'up' else -1
|
||||
self.origin = joint.minimum if direction == 'up' else joint.maximum
|
||||
self.terminal = joint.maximum if direction == 'up' else joint.minimum
|
||||
self.steps = 0
|
||||
self.reference, self.threshold = None, None
|
||||
self.reference_noise = None
|
||||
self.candidate, self.support = None, 0
|
||||
self.candidate_points = None
|
||||
self.continuation = None
|
||||
self.history = deque(maxlen=max(3, settings.confirmation_points))
|
||||
self.rejections = []
|
||||
self.boundary, self.error = None, None
|
||||
|
||||
@property
|
||||
def done(self):
|
||||
return self.boundary is not None or self.error is not None
|
||||
|
||||
@property
|
||||
def next_command(self):
|
||||
return self.origin + self.sign * self.steps * self.joint.resolution
|
||||
|
||||
def add(self, command, observation):
|
||||
if self.done:
|
||||
return
|
||||
command = self.joint.validate_value(command)
|
||||
expected = self.next_command
|
||||
if not math.isclose(command, expected, rel_tol=0, abs_tol=1e-6):
|
||||
raise Unmeasurable('端点搜索采样必须从指令端点按分辨率连续推进,不能缺点或重复')
|
||||
points = np.asarray(observation['corners'], dtype=float)
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
raise Unmeasurable('端点角点观测无效')
|
||||
if self.reference is None:
|
||||
noise = float(observation['noise'])
|
||||
if not math.isfinite(noise) or noise < 0:
|
||||
raise Unmeasurable('端点噪声估计无效')
|
||||
self.reference = points.copy()
|
||||
self.reference_noise = noise
|
||||
self.threshold = max(self.settings.motion_floor_px, self.settings.noise_multiplier * noise)
|
||||
else:
|
||||
self._update_evidence(command, points)
|
||||
self.history.append((command, points.copy()))
|
||||
self.steps += 1
|
||||
if self.boundary is None and math.isclose(command, self.terminal, rel_tol=0, abs_tol=1e-6):
|
||||
end = '低端' if self.direction == 'up' else '高端'
|
||||
self.error = end + '已扫描至对端,未获得足够观测点确认起动边界'
|
||||
|
||||
def _progress_from_candidate(self, points):
|
||||
"""Signed additional travel along the candidate's 4-corner displacement.
|
||||
|
||||
Unlike center displacement, this also measures a tag rotating in place.
|
||||
Units are pixels, normalized in the same way as rms_delta.
|
||||
"""
|
||||
return self._projected_travel(points, self.candidate_points)
|
||||
|
||||
def _projected_travel(self, points, start, minimum_offset=None):
|
||||
axis = start - self.reference
|
||||
distance = rms_delta(start, self.reference)
|
||||
if distance <= (self.threshold if minimum_offset is None else max(1e-9, minimum_offset)):
|
||||
return 0.0
|
||||
return float(np.mean(np.sum((points - start) * axis, axis=1)) / distance)
|
||||
|
||||
def _discard_reversed_offset(self, command, points):
|
||||
"""A one-off offset followed by a plateau cannot establish scan direction.
|
||||
|
||||
If motion then leaves that stable plateau in the opposite direction,
|
||||
restart from the plateau. Never move a confirmed boundary or demand
|
||||
continuous motion through the finger's interior range.
|
||||
"""
|
||||
count = self.history.maxlen
|
||||
if not self.require_progress or len(self.history) < count:
|
||||
return
|
||||
if self.candidate is None:
|
||||
if not self.recover_subthreshold_offset:
|
||||
return
|
||||
elif self.support < count:
|
||||
return
|
||||
plateau = np.median([value for _, value in self.history], axis=0)
|
||||
if any(rms_delta(value, plateau) > self.threshold for _, value in self.history):
|
||||
return
|
||||
# The initial offset can creep before settling. A later departure must
|
||||
# be measured from that settled platform, not from the first offset.
|
||||
start = plateau if self.recent_plateau else self.candidate_points
|
||||
# A settled initial offset can stay below the departure threshold yet
|
||||
# cancel the first real movement in the opposite direction. V5 allows
|
||||
# that axis once it exceeds reference noise, even with no candidate.
|
||||
# Actual departure and subsequent confirmation still require the full
|
||||
# motion threshold; do not move the reference along slow forward travel.
|
||||
axis_floor = self.reference_noise if self.recover_subthreshold_offset else None
|
||||
if self._projected_travel(points, start, axis_floor) >= -self.threshold:
|
||||
return
|
||||
self.rejections.append({
|
||||
'trigger': self.candidate, 'rejected_at': command,
|
||||
'reason': '初始偏移形成稳定平台,随后反向离开;改用最近平台继续确认',
|
||||
'initial_offset_px': rms_delta(plateau, self.reference),
|
||||
'threshold': self.threshold,
|
||||
'reference_commands': [value for value, _ in self.history],
|
||||
'reference_corners': plateau.tolist(),
|
||||
})
|
||||
self.reference = plateau
|
||||
self._clear_candidate()
|
||||
|
||||
def _clear_candidate(self):
|
||||
self.candidate, self.candidate_points, self.support = None, None, 0
|
||||
self.continuation = None
|
||||
|
||||
def _discard_opposed_candidate(self, command, points):
|
||||
"""Crossing the reference invalidates the unconfirmed departure direction.
|
||||
|
||||
A discrete command step can move from one side of the reference to the
|
||||
other without a sample inside the threshold. Reset its evidence just as
|
||||
when returning to the reference; do not wait for another static plateau.
|
||||
Keep the original reference and noise threshold, and require fresh
|
||||
confirmation points plus additional travel in the new direction.
|
||||
"""
|
||||
if not self.reject_opposed_candidate or self.candidate is None:
|
||||
return
|
||||
axis = self.candidate_points - self.reference
|
||||
distance = rms_delta(self.candidate_points, self.reference)
|
||||
departure = float(np.mean(np.sum((points - self.reference) * axis, axis=1)) / distance)
|
||||
if departure >= -self.threshold:
|
||||
return
|
||||
self.rejections.append({
|
||||
'trigger': self.candidate, 'rejected_at': command,
|
||||
'reason': '观测跨过参考姿态,离开方向与未确认候选相反;重新确认起动',
|
||||
'projected_departure_px': departure, 'threshold': self.threshold,
|
||||
})
|
||||
self._clear_candidate()
|
||||
|
||||
def _update_evidence(self, command, points):
|
||||
self._discard_reversed_offset(command, points)
|
||||
if rms_delta(points, self.reference) <= self.threshold:
|
||||
self._clear_candidate()
|
||||
return
|
||||
self._discard_opposed_candidate(command, points)
|
||||
if self.candidate is None:
|
||||
self.candidate, self.candidate_points = command, points.copy()
|
||||
if self.follow_continuation:
|
||||
self.continuation = _MotionContinuation(self.reference, points, self.threshold)
|
||||
self.support += 1
|
||||
# Several still frames at an offset position are one displacement, not
|
||||
# repeated evidence of travel. Keep the first trigger while awaiting
|
||||
# additional motion, even if that motion takes a different direction.
|
||||
# Older journals retain their original fixed-direction confirmation.
|
||||
if self.continuation is not None:
|
||||
progressed = self.continuation.observe(points)
|
||||
else:
|
||||
progressed = not self.require_progress or self._progress_from_candidate(points) > self.threshold
|
||||
if self.support >= self.settings.confirmation_points and progressed:
|
||||
bound = self.candidate - self.sign * self.joint.resolution
|
||||
self.boundary = {
|
||||
'bound': self.joint.validate_value(bound), 'trigger': self.candidate,
|
||||
'confirmed_at': command, 'threshold': self.threshold,
|
||||
}
|
||||
|
||||
def result(self):
|
||||
if self.boundary is None:
|
||||
raise Unmeasurable(self.error or '端点搜索数据不足,起动边界尚未确认')
|
||||
return dict(self.boundary)
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
"""Model-independent endpoint stages, including a deferred lower-bound search."""
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ScanSegment:
|
||||
name: str
|
||||
label: str
|
||||
direction: str
|
||||
start: dict[str, float]
|
||||
end: dict[str, float]
|
||||
measure: tuple[str, ...]
|
||||
# A split search preserves its original endpoint reference.
|
||||
continues: bool = False
|
||||
has_continuation: bool = False
|
||||
finish_at_end: bool = False
|
||||
|
||||
@property
|
||||
def moving(self):
|
||||
return tuple(name for name in self.start if self.start[name] != self.end[name])
|
||||
|
||||
def steps(self, by_name):
|
||||
return max(round(abs(self.end[name] - value) / by_name[name].resolution)
|
||||
for name, value in self.start.items())
|
||||
|
||||
def point(self, index, by_name):
|
||||
"""Shared progress, quantized per channel; no channel skips a command step."""
|
||||
steps = self.steps(by_name)
|
||||
values = {}
|
||||
for name, start in self.start.items():
|
||||
joint = by_name[name]
|
||||
distance = round((self.end[name] - start) / joint.resolution)
|
||||
value = start + round(distance * index / steps) * joint.resolution
|
||||
values[name] = joint.validate_value(value)
|
||||
return values
|
||||
|
||||
|
||||
def endpoint_plan(task, by_name):
|
||||
low = {name: by_name[name].minimum for name in task.joints}
|
||||
high = {name: by_name[name].maximum for name in task.joints}
|
||||
if task.deferred_lower is None:
|
||||
return (
|
||||
ScanSegment('lower', '测下限', 'up', low, high, task.joints),
|
||||
ScanSegment('upper', '测上限', 'down', high, low, task.joints),
|
||||
)
|
||||
deferred = task.deferred_lower
|
||||
split = {**low, deferred.joint: by_name[deferred.joint].validate_value(deferred.split)}
|
||||
others = tuple(name for name in task.joints if name != deferred.joint)
|
||||
return (
|
||||
ScanSegment('group_lower', '同步测其余关节下限', 'up', split, high, others),
|
||||
ScanSegment('group_upper', '同步测全部关节上限', 'down', high, split,
|
||||
task.joints, finish_at_end=True),
|
||||
ScanSegment('deferred_lower', f'{deferred.joint} 单独测下限', 'up', low, split,
|
||||
(deferred.joint,), has_continuation=True),
|
||||
ScanSegment('deferred_extension', f'{deferred.joint} 联动继续测下限', 'up', split, high,
|
||||
(deferred.joint,), continues=True),
|
||||
)
|
||||
@@ -0,0 +1,26 @@
|
||||
"""Rate-limited vector motion; a held tick never accumulates catch-up time."""
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Trajectory:
|
||||
def __init__(self, profile, start, goal, rate, now, coordinated=False):
|
||||
self.profile = profile
|
||||
self.value = np.asarray(profile.vector(start), dtype=float)
|
||||
self.goal = np.asarray(profile.vector(goal), dtype=float)
|
||||
self.rate, self.previous = rate, now
|
||||
self.coordinated = coordinated
|
||||
|
||||
def advance(self, now, hold=False):
|
||||
dt = max(0.0, min(now-self.previous, 0.1))
|
||||
self.previous = now
|
||||
if not hold:
|
||||
delta = self.goal-self.value
|
||||
if self.coordinated:
|
||||
distance = np.max(np.abs(delta))
|
||||
scale = min(1.0, self.rate * dt / distance) if distance else 0.0
|
||||
self.value += delta * scale
|
||||
else:
|
||||
self.value += np.clip(delta, -self.rate*dt, self.rate*dt)
|
||||
values = [j.minimum + round((self.value[j.index]-j.minimum)/j.resolution)*j.resolution
|
||||
for j in self.profile.joints]
|
||||
return self.profile.vector(values), bool(np.allclose(self.value, self.goal, atol=1e-8))
|
||||
@@ -0,0 +1,71 @@
|
||||
"""工装和相机外参。缺字段就失败,不偷偷用单位阵。"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
import yaml
|
||||
|
||||
|
||||
def transform_from_xyz_rpy(translation_xyz_m, rpy_rad) -> np.ndarray:
|
||||
rotation = np.asarray(rpy_rad, dtype=float)
|
||||
translation = np.asarray(translation_xyz_m, dtype=float)
|
||||
if rotation.shape != (3,) or translation.shape != (3,):
|
||||
raise ValueError('fixture_from_hand_base_link 必须是 xyz 和 rpy')
|
||||
if not np.all(np.isfinite(rotation)) or not np.all(np.isfinite(translation)):
|
||||
raise ValueError('fixture_from_hand_base_link 含有非有限数值')
|
||||
pose = np.eye(4)
|
||||
pose[:3, :3] = Rotation.from_euler('xyz', rotation).as_matrix()
|
||||
pose[:3, 3] = translation
|
||||
return pose
|
||||
|
||||
|
||||
def transform_from_xyz_quat(translation_xyz_m, quaternion_xyzw) -> np.ndarray:
|
||||
quaternion = np.asarray(quaternion_xyzw, dtype=float)
|
||||
translation = np.asarray(translation_xyz_m, dtype=float)
|
||||
if quaternion.shape != (4,) or translation.shape != (3,):
|
||||
raise ValueError('外参平移或四元数形状不对')
|
||||
if not np.all(np.isfinite(quaternion)) or not np.all(np.isfinite(translation)):
|
||||
raise ValueError('外参含有非有限数值')
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if not math.isfinite(norm) or abs(norm - 1.0) > 1e-6:
|
||||
raise ValueError('外参四元数不是单位四元数')
|
||||
pose = np.eye(4)
|
||||
pose[:3, :3] = Rotation.from_quat(quaternion).as_matrix()
|
||||
pose[:3, 3] = translation
|
||||
return pose
|
||||
|
||||
|
||||
def matrix4(value) -> np.ndarray:
|
||||
pose = np.asarray(value, dtype=float)
|
||||
if pose.shape != (4, 4) or not np.all(np.isfinite(pose)):
|
||||
raise ValueError('front_from_fixture 必须是 4×4 数值矩阵')
|
||||
return pose
|
||||
|
||||
|
||||
def load_hand_mount(config_path: Path) -> tuple[np.ndarray, np.ndarray]:
|
||||
path = Path(config_path)
|
||||
document = yaml.safe_load(path.read_text(encoding='utf-8')) if path.is_file() else None
|
||||
if not isinstance(document, dict):
|
||||
raise ValueError(f'找不到手的工装配置: {path}')
|
||||
mount = document.get('fixture_from_hand_base_link')
|
||||
front = document.get('front_from_fixture')
|
||||
if not isinstance(mount, dict) or 'xyz' not in mount or 'rpy' not in mount:
|
||||
raise ValueError('config.yaml 缺少 fixture_from_hand_base_link')
|
||||
if front is None:
|
||||
raise ValueError('config.yaml 缺少 front_from_fixture')
|
||||
return transform_from_xyz_rpy(mount['xyz'], mount['rpy']), matrix4(front)
|
||||
|
||||
|
||||
def front_from_views(extrinsics_document: dict) -> dict[str, np.ndarray]:
|
||||
transforms = extrinsics_document.get('front_from_view') if isinstance(extrinsics_document, dict) else None
|
||||
if not isinstance(transforms, dict):
|
||||
raise ValueError('外参文件缺少 front_from_view')
|
||||
poses = {}
|
||||
for view, block in transforms.items():
|
||||
if not isinstance(block, dict):
|
||||
raise ValueError(f'外参缺少 {view}')
|
||||
poses[view] = transform_from_xyz_quat(block['translation_xyz_m'], block['quaternion_xyzw'])
|
||||
return poses
|
||||
@@ -0,0 +1,194 @@
|
||||
"""URDF 关节轴和限位。不引用 URDF 标定工程。"""
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def _triplet(value: str) -> np.ndarray:
|
||||
result = np.asarray([float(v) for v in str(value).split()], dtype=float)
|
||||
if result.shape != (3,) or not np.all(np.isfinite(result)):
|
||||
raise ValueError(f'URDF 向量无效: {value}')
|
||||
return result
|
||||
|
||||
|
||||
def _axis_rotation(axis: np.ndarray, angle: float) -> np.ndarray:
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = Rotation.from_rotvec(np.asarray(axis, dtype=float) * float(angle)).as_matrix()
|
||||
return transform
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UrdfJoint:
|
||||
name: str
|
||||
parent: str
|
||||
child: str
|
||||
origin: np.ndarray
|
||||
axis: np.ndarray
|
||||
mimic_joint: str | None
|
||||
mimic_multiplier: float
|
||||
mimic_offset: float
|
||||
kind: str
|
||||
lower: float | None
|
||||
upper: float | None
|
||||
|
||||
|
||||
class KinematicModel:
|
||||
"""手掌坐标系里的关节轴。轴不从某一次图像里估计。"""
|
||||
|
||||
def __init__(self, source: str | Path):
|
||||
self.source = Path(source).expanduser().resolve()
|
||||
root = ET.parse(self.source).getroot()
|
||||
if root.tag != 'robot':
|
||||
raise ValueError('URDF 根节点必须是 robot')
|
||||
self.joints: dict[str, UrdfJoint] = {}
|
||||
self.parent_joint_by_child: dict[str, str] = {}
|
||||
links = set()
|
||||
for element in root.findall('link'):
|
||||
name = element.get('name', '')
|
||||
if not name or name in links:
|
||||
raise ValueError('URDF 连杆名必须非空且唯一')
|
||||
links.add(name)
|
||||
for element in root.findall('joint'):
|
||||
name = element.get('name', '')
|
||||
kind = element.get('type', '')
|
||||
if not name or name in self.joints:
|
||||
raise ValueError('URDF 关节名必须非空且唯一')
|
||||
if kind not in {'fixed', 'revolute', 'continuous', 'prismatic'}:
|
||||
raise ValueError(f'不支持的关节类型: {name}:{kind}')
|
||||
parent = element.find('parent')
|
||||
child = element.find('child')
|
||||
if parent is None or child is None:
|
||||
raise ValueError(f'URDF 关节缺少 parent/child: {name}')
|
||||
parent_name, child_name = parent.get('link', ''), child.get('link', '')
|
||||
if parent_name not in links or child_name not in links:
|
||||
raise ValueError(f'URDF 关节引用了不存在的连杆: {name}')
|
||||
if child_name in self.parent_joint_by_child:
|
||||
raise ValueError(f'URDF 连杆有多个父关节: {child_name}')
|
||||
origin_node, axis_node = element.find('origin'), element.find('axis')
|
||||
xyz = _triplet('0 0 0' if origin_node is None else origin_node.get('xyz', '0 0 0'))
|
||||
rpy = _triplet('0 0 0' if origin_node is None else origin_node.get('rpy', '0 0 0'))
|
||||
axis = _triplet('1 0 0' if axis_node is None else axis_node.get('xyz', '1 0 0'))
|
||||
norm = float(np.linalg.norm(axis))
|
||||
if norm <= 1e-12:
|
||||
raise ValueError(f'URDF 关节轴长度为 0: {name}')
|
||||
axis = axis / norm
|
||||
origin = np.eye(4)
|
||||
origin[:3, :3] = Rotation.from_euler('xyz', rpy).as_matrix()
|
||||
origin[:3, 3] = xyz
|
||||
mimic, limit = element.find('mimic'), element.find('limit')
|
||||
multiplier = 1.0 if mimic is None else float(mimic.get('multiplier', '1'))
|
||||
offset = 0.0 if mimic is None else float(mimic.get('offset', '0'))
|
||||
lower = None if limit is None or kind in {'fixed', 'continuous'} else float(limit.get('lower', 'nan'))
|
||||
upper = None if lower is None else float(limit.get('upper', 'nan'))
|
||||
if lower is not None and (not math.isfinite(lower) or not math.isfinite(upper) or lower > upper):
|
||||
raise ValueError(f'URDF 关节限位无效: {name}')
|
||||
self.joints[name] = UrdfJoint(
|
||||
name, parent_name, child_name, origin, axis,
|
||||
None if mimic is None else mimic.get('joint', ''),
|
||||
multiplier, offset, kind, lower, upper)
|
||||
self.parent_joint_by_child[child_name] = name
|
||||
roots = links - set(self.parent_joint_by_child)
|
||||
if len(roots) != 1:
|
||||
raise ValueError('URDF 必须只有一个根连杆')
|
||||
self.root_link = next(iter(roots))
|
||||
for name in self.joints:
|
||||
self.chain(name)
|
||||
self.resolve_angles({})
|
||||
|
||||
def chain(self, target: str) -> list[UrdfJoint]:
|
||||
if target not in self.joints:
|
||||
raise ValueError(f'URDF 没有关节 {target}')
|
||||
ordered = []
|
||||
seen = set()
|
||||
current = self.joints[target]
|
||||
while True:
|
||||
if current.name in seen:
|
||||
raise ValueError(f'URDF 运动链成环: {current.name}')
|
||||
seen.add(current.name)
|
||||
ordered.append(current)
|
||||
parent = self.parent_joint_by_child.get(current.parent)
|
||||
if parent is None:
|
||||
if current.parent != self.root_link:
|
||||
raise ValueError('URDF 关节链断开')
|
||||
return list(reversed(ordered))
|
||||
current = self.joints[parent]
|
||||
|
||||
def resolve_angles(self, joint_angles: dict[str, float]) -> dict[str, float]:
|
||||
supplied = {str(key): float(value) for key, value in joint_angles.items()}
|
||||
if not all(math.isfinite(value) for value in supplied.values()):
|
||||
raise ValueError('关节角必须是有限数值')
|
||||
resolved: dict[str, float] = {}
|
||||
visiting: set[str] = set()
|
||||
|
||||
def resolve(name: str) -> float:
|
||||
if name in resolved:
|
||||
return resolved[name]
|
||||
if name in visiting or name not in self.joints:
|
||||
raise ValueError(f'URDF mimic 依赖无效: {name}')
|
||||
visiting.add(name)
|
||||
joint = self.joints[name]
|
||||
if joint.kind == 'fixed':
|
||||
angle = 0.0
|
||||
elif joint.mimic_joint:
|
||||
angle = joint.mimic_multiplier * resolve(joint.mimic_joint) + joint.mimic_offset
|
||||
else:
|
||||
angle = supplied.get(name, 0.0)
|
||||
visiting.remove(name)
|
||||
resolved[name] = angle
|
||||
return angle
|
||||
|
||||
for name in self.joints:
|
||||
resolve(name)
|
||||
return resolved
|
||||
|
||||
def limits(self, name: str) -> tuple[float, float]:
|
||||
joint = self.joints[name]
|
||||
if joint.lower is None or joint.upper is None:
|
||||
raise ValueError(f'{name} 没有转动限位')
|
||||
return float(joint.lower), float(joint.upper)
|
||||
|
||||
def axis_in_palm(self, name: str, joint_angles: dict[str, float]) -> np.ndarray:
|
||||
"""关节轴在手掌中的方向。本关节转角不改变自己的轴,调用方不要把它写进 joint_angles。"""
|
||||
resolved = self.resolve_angles(joint_angles)
|
||||
transform = np.eye(4)
|
||||
for joint in self.chain(name):
|
||||
frame = transform @ joint.origin
|
||||
if joint.name == name:
|
||||
return frame[:3, :3] @ joint.axis
|
||||
angle = 0.0 if joint.kind == 'fixed' else resolved[joint.name]
|
||||
transform = frame @ _axis_rotation(joint.axis, angle)
|
||||
raise ValueError(f'无法计算关节轴: {name}')
|
||||
|
||||
def link_rotation(self, name: str, joint_angles: dict[str, float]) -> np.ndarray:
|
||||
"""子连杆在手掌中的旋转。joint_angles 含本关节时,贴纸边会跟着转。"""
|
||||
resolved = self.resolve_angles(joint_angles)
|
||||
transform = np.eye(4)
|
||||
for joint in self.chain(name):
|
||||
frame = transform @ joint.origin
|
||||
angle = 0.0 if joint.kind == 'fixed' else resolved[joint.name]
|
||||
transform = frame @ _axis_rotation(joint.axis, angle)
|
||||
if joint.name == name:
|
||||
return transform[:3, :3].copy()
|
||||
raise ValueError(f'无法计算连杆姿态: {name}')
|
||||
|
||||
def optical_copy(self) -> 'KinematicModel':
|
||||
"""测试和模拟用:每根转动轴改成光轴方向,原点旋转清掉。限位保持不变。"""
|
||||
cloned = KinematicModel.__new__(KinematicModel)
|
||||
cloned.source = self.source
|
||||
cloned.root_link = self.root_link
|
||||
cloned.parent_joint_by_child = dict(self.parent_joint_by_child)
|
||||
cloned.joints = {}
|
||||
for name, joint in self.joints.items():
|
||||
origin = np.eye(4)
|
||||
axis = np.array([0.0, 0.0, 1.0]) if joint.kind == 'revolute' else joint.axis.copy()
|
||||
cloned.joints[name] = UrdfJoint(
|
||||
joint.name, joint.parent, joint.child, origin, axis,
|
||||
joint.mimic_joint, joint.mimic_multiplier, joint.mimic_offset,
|
||||
joint.kind, joint.lower, joint.upper)
|
||||
return cloned
|
||||
@@ -0,0 +1,67 @@
|
||||
"""Convert range-calibration result JSON into SDK set_offset commands."""
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def load_ranges_document(path):
|
||||
path = Path(path).expanduser()
|
||||
if not path.is_file():
|
||||
raise ValueError(f'结果文件不存在: {path}')
|
||||
try:
|
||||
document = json.loads(path.read_text(encoding='utf-8'))
|
||||
except (OSError, json.JSONDecodeError) as error:
|
||||
raise ValueError(f'无法读取结果 JSON: {error}') from error
|
||||
if not isinstance(document, dict):
|
||||
raise ValueError('结果 JSON 必须是对象')
|
||||
return document
|
||||
|
||||
|
||||
def ranges_to_set_offset(document, profile):
|
||||
"""Build the SDK setting payload from a ranges result document.
|
||||
|
||||
Offset order follows profile joint indices, which match the O30 SDK order:
|
||||
thumb_roll … little_tip.
|
||||
"""
|
||||
if document.get('schema_version') == 2:
|
||||
raise ValueError('角度曲线结果不能转换成 set_offset。嵌入式只烧录 commands 数组')
|
||||
if document.get('schema_version') != 1:
|
||||
raise ValueError('仅支持 schema_version=1 的行程结果')
|
||||
if document.get('model') != profile.model:
|
||||
raise ValueError(f'结果型号 {document.get("model")!r} 与当前配置 {profile.model!r} 不一致')
|
||||
if document.get('side') != profile.side:
|
||||
raise ValueError(f'结果左右手 {document.get("side")!r} 与当前配置 {profile.side!r} 不一致')
|
||||
joints = document.get('joints')
|
||||
if not isinstance(joints, dict):
|
||||
raise ValueError('结果缺少 joints 对象')
|
||||
offset = []
|
||||
incomplete = []
|
||||
for joint in profile.joints:
|
||||
entry = joints.get(joint.name)
|
||||
if not isinstance(entry, dict) or entry.get('min') is None or entry.get('max') is None:
|
||||
incomplete.append(joint.name)
|
||||
continue
|
||||
try:
|
||||
low = int(entry['min'])
|
||||
high = int(entry['max'])
|
||||
except (TypeError, ValueError) as error:
|
||||
raise ValueError(f'{joint.name} 的 min/max 必须是整数') from error
|
||||
if entry['min'] != low or entry['max'] != high:
|
||||
raise ValueError(f'{joint.name} 的 min/max 必须是整数')
|
||||
if not (0 <= low <= high <= 255):
|
||||
raise ValueError(f'{joint.name} 范围非法: [{low}, {high}](须满足 0≤min≤max≤255)')
|
||||
low = joint.validate_value(low)
|
||||
high = joint.validate_value(high)
|
||||
offset.append([int(low), int(high)])
|
||||
if incomplete:
|
||||
raise ValueError('结果不完整,以下关节缺少有效 min/max:' + '、'.join(incomplete))
|
||||
if len(offset) != len(profile.joints):
|
||||
raise ValueError(f'偏移数组长度应为 {len(profile.joints)}')
|
||||
return {
|
||||
'setting_cmd': 'set_offset',
|
||||
'params': {'hand_type': profile.side, 'offset': offset},
|
||||
}
|
||||
|
||||
|
||||
def cleared_offset(profile):
|
||||
"""Full command span for every joint — SDK '还原偏移量' ([0, 255] on O30)."""
|
||||
return [[int(joint.minimum), int(joint.maximum)] for joint in profile.joints]
|
||||
@@ -0,0 +1,381 @@
|
||||
"""Validated model/station contracts shared by launch, GUI and offline replay."""
|
||||
from dataclasses import asdict, dataclass, fields, replace
|
||||
import math
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Joint:
|
||||
name: str
|
||||
sdk_name: str
|
||||
index: int
|
||||
minimum: float
|
||||
maximum: float
|
||||
resolution: float
|
||||
view: str
|
||||
tag_id: int
|
||||
|
||||
def validate_value(self, value):
|
||||
value = float(value)
|
||||
if not math.isfinite(value) or not self.minimum <= value <= self.maximum:
|
||||
raise ValueError(f'{self.name}: 指令越界或非有限数值')
|
||||
steps = (value - self.minimum) / self.resolution
|
||||
if abs(steps - round(steps)) > 1e-6:
|
||||
raise ValueError(f'{self.name}: 指令不符合分辨率 {self.resolution}')
|
||||
return int(value) if value.is_integer() else value
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeferredLower:
|
||||
"""Keep this channel above a split during group scans; find its low end last."""
|
||||
joint: str
|
||||
split: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Task:
|
||||
name: str
|
||||
joints: tuple[str, ...]
|
||||
clearances: tuple[str, ...] = ()
|
||||
allow_override: bool = True
|
||||
# Restore one joint at a time in this order, waiting for feedback at each step.
|
||||
restore_after: tuple[str, ...] = ()
|
||||
deferred_lower: DeferredLower | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Clearance:
|
||||
joints: tuple[str, ...]
|
||||
targets: dict[str, float] | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Profile:
|
||||
model: str
|
||||
side: str
|
||||
command_unit: str
|
||||
adapter: str
|
||||
joints: tuple[Joint, ...]
|
||||
tasks: tuple[Task, ...]
|
||||
clearances: dict[str, Clearance]
|
||||
sdk: dict
|
||||
raw: dict
|
||||
edge_in_link: dict = None
|
||||
kinematics: object = None
|
||||
source_dir: str = ''
|
||||
|
||||
@property
|
||||
def by_name(self):
|
||||
return {j.name: j for j in self.joints}
|
||||
|
||||
@property
|
||||
def views(self):
|
||||
return tuple(dict.fromkeys(j.view for j in self.joints))
|
||||
|
||||
@property
|
||||
def teaching_clearances(self):
|
||||
return {name: clearance for name, clearance in self.clearances.items()
|
||||
if clearance.targets is None}
|
||||
|
||||
def vector(self, values):
|
||||
if len(values) != len(self.joints):
|
||||
raise ValueError(f'位置数组必须包含 {len(self.joints)} 项')
|
||||
return tuple(j.validate_value(values[j.index]) for j in self.joints)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Settings:
|
||||
endpoint_repetitions: int = 1
|
||||
# Accepted for historical journals/station files; live scans use endpoint searches.
|
||||
coarse_step: float = 8
|
||||
fine_radius: float = 16
|
||||
fine_repetitions: int = 2
|
||||
rate: float = 20
|
||||
settle_seconds: float = 0.3
|
||||
stable_frames: int = 8
|
||||
stable_seconds: float = 0.5
|
||||
reference_seconds: float = 1.5 # Verify the endpoint across independent time blocks.
|
||||
motion_frames: int = 2 # Consecutive valid frames at this command, independent of direction.
|
||||
point_timeout: float = 6
|
||||
reference_timeout: float = 30 # Endpoint positioning can settle slower than a one-unit step.
|
||||
freshness: float = 0.5
|
||||
feedback_tolerance: float = 2
|
||||
non_target_tolerance: float = 3 # Legacy configuration only; unrelated motion does not gate scans.
|
||||
repeat_tolerance: float = 2
|
||||
motion_floor_px: float = 0.5
|
||||
noise_multiplier: float = 5 # Historical endpoint algorithms only.
|
||||
stability_noise_px: float = 0.5
|
||||
stability_drift_px: float = 0.1
|
||||
stability_drift_sigma: float = 3
|
||||
confirmation_points: int = 3 # Historical journals only; live scans stop at first motion.
|
||||
max_hamming: int = 0
|
||||
min_margin: float = 30
|
||||
min_edge_px: float = 30
|
||||
angle_motion_rad: float = 0.005
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, data):
|
||||
data = dict(data)
|
||||
# Historical journals and custom stations used a shared tolerance.
|
||||
# Keep its noise limit; drift now has an independent, explicit default.
|
||||
if 'stability_px' in data:
|
||||
legacy = data.pop('stability_px')
|
||||
if not isinstance(legacy, (float, int)) or not math.isfinite(legacy) or legacy <= 0:
|
||||
raise ValueError('旧扫描参数 stability_px 必须为正有限数值')
|
||||
data.setdefault('stability_noise_px', legacy)
|
||||
unknown = set(data) - {f.name for f in fields(cls)}
|
||||
if unknown:
|
||||
raise ValueError(f'未知扫描参数: {sorted(unknown)}')
|
||||
result = cls(**data)
|
||||
for name, value in asdict(result).items():
|
||||
if not isinstance(value, (float, int)) or not math.isfinite(value):
|
||||
raise ValueError(f'扫描参数 {name} 必须为有限数值')
|
||||
if value < 0 or (name != 'max_hamming' and value == 0):
|
||||
raise ValueError(f'扫描参数 {name} 范围不合法')
|
||||
for name in ('endpoint_repetitions', 'fine_repetitions', 'stable_frames', 'motion_frames', 'confirmation_points', 'max_hamming'):
|
||||
if not isinstance(getattr(result, name), int):
|
||||
raise ValueError(f'{name} 必须为整数')
|
||||
if result.stable_frames < 4 or result.fine_repetitions < 2:
|
||||
raise ValueError('稳定帧至少4张,细扫复测至少2轮')
|
||||
if result.motion_frames < 2:
|
||||
raise ValueError('运动过程检测至少需要2张连续有效图像')
|
||||
if result.point_timeout <= result.settle_seconds + result.stable_seconds:
|
||||
raise ValueError('单点超时必须大于最短等待与稳定观察时长之和')
|
||||
if result.reference_seconds < result.stable_seconds:
|
||||
raise ValueError('端点参考观察时长不能短于普通稳定观察时长')
|
||||
if result.reference_timeout <= result.settle_seconds + result.reference_seconds:
|
||||
raise ValueError('端点参考超时必须大于最短等待与端点参考观察时长之和')
|
||||
return result
|
||||
|
||||
|
||||
def resources():
|
||||
try:
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
return Path(get_package_share_directory('linkerhand_range_calibration'))
|
||||
except (ImportError, LookupError):
|
||||
return Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def _hands_root() -> Path:
|
||||
candidates = (
|
||||
Path(__file__).resolve().parents[1] / 'hands',
|
||||
resources() / 'hands',
|
||||
)
|
||||
for root in candidates:
|
||||
if root.is_dir() and any(root.glob('*/*.yaml')):
|
||||
return root
|
||||
raise ValueError('找不到型号目录 hands')
|
||||
|
||||
|
||||
def available_models() -> list[str]:
|
||||
root = _hands_root()
|
||||
return sorted(path.name for path in root.iterdir() if path.is_dir() and any(path.glob('*.yaml')))
|
||||
|
||||
|
||||
def load_profile(selection='o30_right'):
|
||||
path = Path(selection).expanduser()
|
||||
if not path.is_file():
|
||||
if not re.fullmatch(r'[A-Za-z0-9_-]+', selection):
|
||||
raise ValueError(f'型号配置不存在: {selection}')
|
||||
matches = list(_hands_root().glob('*/' + selection + '.yaml'))
|
||||
if len(matches) != 1:
|
||||
raise ValueError(f'型号配置不存在: {selection}')
|
||||
path = matches[0]
|
||||
data = yaml.safe_load(path.read_text(encoding='utf-8'))
|
||||
profile = profile_from_dict(data)
|
||||
urdf = data.get('urdf') if isinstance(data, dict) else None
|
||||
if not urdf:
|
||||
raise ValueError(f'{path.name} 缺少 urdf')
|
||||
urdf_path = (path.parent / urdf).resolve()
|
||||
if not urdf_path.is_file():
|
||||
raise ValueError(f'URDF 不存在: {urdf_path}')
|
||||
from .kinematic import KinematicModel
|
||||
return replace(profile, kinematics=KinematicModel(urdf_path), source_dir=str(path.parent))
|
||||
|
||||
|
||||
def profile_from_dict(data):
|
||||
if not isinstance(data, dict) or not re.fullmatch(r'[A-Za-z0-9_-]+', str(data.get('model', ''))):
|
||||
raise ValueError('型号配置必须为对象,型号名称必须为字母、数字或下划线')
|
||||
if data.get('schema_version') != 1 or data.get('side') not in ('left', 'right'):
|
||||
raise ValueError('型号配置版本或左右手无效')
|
||||
domain = data['command']
|
||||
joint_rows = []
|
||||
edges = {}
|
||||
for item in data['joints']:
|
||||
item = dict(item)
|
||||
edge = item.pop('edge_in_link', None)
|
||||
if edge is not None:
|
||||
edges[item['name']] = tuple(float(value) for value in edge)
|
||||
joint_rows.append(item)
|
||||
joints = tuple(Joint(**j, **domain) for j in joint_rows)
|
||||
joints = tuple(sorted(joints, key=lambda j: j.index))
|
||||
if not joints or [j.index for j in joints] != list(range(len(joints))):
|
||||
raise ValueError('SDK 下标必须连续且唯一,从0开始')
|
||||
names = [j.name for j in joints]
|
||||
if len(set(names)) != len(names) or len({j.sdk_name for j in joints}) != len(joints):
|
||||
raise ValueError('关节名称重复')
|
||||
for j in joints:
|
||||
if j.minimum >= j.maximum or j.resolution <= 0 or j.tag_id < 0:
|
||||
raise ValueError(f'{j.name}: 指令范围或 Tag 配置无效')
|
||||
j.validate_value(j.maximum)
|
||||
tasks = tuple(Task(t['name'], tuple(t['joints']), tuple(t.get('clearances', [])),
|
||||
t.get('allow_override', True), tuple(t.get('restore_after', [])),
|
||||
DeferredLower(**t['deferred_lower']) if 'deferred_lower' in t else None)
|
||||
for t in data['tasks'])
|
||||
used = [name for t in tasks for name in t.joints]
|
||||
if sorted(used) != sorted(names) or len({t.name for t in tasks}) != len(tasks):
|
||||
raise ValueError('每个关节必须恰好出现在一个任务中,任务名称必须唯一')
|
||||
clearances = {}
|
||||
by_name = {joint.name: joint for joint in joints}
|
||||
for group, specification in data.get('clearances', {}).items():
|
||||
if isinstance(specification, list):
|
||||
members, targets = specification, None
|
||||
elif (isinstance(specification, dict) and set(specification) == {'targets'}
|
||||
and isinstance(specification['targets'], dict)):
|
||||
targets = specification['targets']
|
||||
members = list(targets)
|
||||
else:
|
||||
raise ValueError(f'避让组 {group} 必须为示教关节列表或固定 targets 配置')
|
||||
if not members or len(set(members)) != len(members) or not set(members) <= set(names):
|
||||
raise ValueError(f'避让组 {group} 无效')
|
||||
if targets is not None:
|
||||
targets = {name: by_name[name].validate_value(value) for name, value in targets.items()}
|
||||
clearances[group] = Clearance(tuple(members), targets)
|
||||
for t in tasks:
|
||||
if not t.joints or not set(t.clearances) <= set(clearances):
|
||||
raise ValueError(f'任务 {t.name} 无效')
|
||||
if len(set(t.restore_after)) != len(t.restore_after) or not set(t.restore_after) <= set(names):
|
||||
raise ValueError(f'任务 {t.name} 的测后恢复关节无效或重复')
|
||||
if any(set(clearances[c].joints) & set(t.joints) for c in t.clearances):
|
||||
raise ValueError(f'任务 {t.name} 的主动关节不能同时参与避让')
|
||||
observations = [(j.view, j.tag_id) for j in joints if j.name in t.joints]
|
||||
if len(set(observations)) != len(observations):
|
||||
raise ValueError(f'并行任务 {t.name} 必须有独立观测源')
|
||||
if t.deferred_lower is not None:
|
||||
deferred = t.deferred_lower
|
||||
if len(t.joints) < 2 or deferred.joint not in t.joints:
|
||||
raise ValueError(f'任务 {t.name} 的延后下限关节必须属于多关节任务')
|
||||
joint = by_name[deferred.joint]
|
||||
split = joint.validate_value(deferred.split)
|
||||
if not joint.minimum < split < joint.maximum:
|
||||
raise ValueError(f'任务 {t.name} 的下限搜索分段值必须位于指令范围内部')
|
||||
return Profile(data['model'], data['side'], data['command_unit'], data['adapter'],
|
||||
joints, tasks, clearances, data['sdk'], data, edges or {})
|
||||
|
||||
|
||||
def _workspace_root() -> Path:
|
||||
seeds = [Path(__file__).resolve(), Path.cwd().resolve()]
|
||||
seen = set()
|
||||
for seed in seeds:
|
||||
for candidate in (seed, *seed.parents):
|
||||
if candidate in seen:
|
||||
continue
|
||||
seen.add(candidate)
|
||||
marker = candidate / 'src' / 'camera_parameters' / 'package.xml'
|
||||
if marker.is_file() and (candidate / 'output_camera').is_dir():
|
||||
return candidate
|
||||
raise FileNotFoundError('找不到仓库里的 output_camera')
|
||||
|
||||
|
||||
def _fingerprint(path: Path) -> str:
|
||||
import sys
|
||||
source = _workspace_root() / 'src'
|
||||
if str(source) not in sys.path:
|
||||
sys.path.insert(0, str(source))
|
||||
from camera_parameters.extrinsics_geometry import camera_info_fingerprint
|
||||
document = yaml.safe_load(path.read_text(encoding='utf-8'))
|
||||
return camera_info_fingerprint(
|
||||
width=int(document['image_width']),
|
||||
height=int(document['image_height']),
|
||||
camera_matrix=document['camera_matrix']['data'],
|
||||
distortion=document['distortion_coefficients']['data'],
|
||||
rectification=document['rectification_matrix']['data'],
|
||||
projection=document['projection_matrix']['data'],
|
||||
)
|
||||
|
||||
|
||||
def _intrinsic_for(workspace: Path, serial: str, expected: str, extrinsics_path: Path) -> Path:
|
||||
import sys
|
||||
source = workspace / 'src'
|
||||
if str(source) not in sys.path:
|
||||
sys.path.insert(0, str(source))
|
||||
from camera_parameters.output_paths import intrinsic_paths
|
||||
paths = intrinsic_paths(workspace, serial)
|
||||
if not paths:
|
||||
raise FileNotFoundError(f'output_camera/intrinsics 里没有 {serial}')
|
||||
for path in reversed(paths):
|
||||
if _fingerprint(path) == expected:
|
||||
return path
|
||||
newest = paths[-1]
|
||||
raise ValueError(
|
||||
f'{serial} 的内参指纹与 {extrinsics_path.name} 不一致:'
|
||||
f'外参记录 {expected},最新文件 {newest.name} 是 {_fingerprint(newest)}'
|
||||
)
|
||||
|
||||
|
||||
def _apply_camera_station(station: dict, workspace: Path) -> None:
|
||||
"""帧率、曝光和贴纸边长跟 URDF 标定用同一份工位配置。"""
|
||||
path = workspace / 'src' / 'camera_parameters' / 'config' / 'cameras.yaml'
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f'找不到相机配置: {path}')
|
||||
document = yaml.safe_load(path.read_text(encoding='utf-8'))
|
||||
if not isinstance(document, dict):
|
||||
raise ValueError(f'相机配置不完整: {path}')
|
||||
camera = station.setdefault('camera', {})
|
||||
for key in ('frame_rate', 'exposure_time_us', 'tag_size_m'):
|
||||
if key not in document:
|
||||
raise ValueError(f'相机配置缺少 {key}: {path}')
|
||||
camera[key] = float(document[key])
|
||||
|
||||
|
||||
def bind_output_camera(station: dict) -> None:
|
||||
"""用 output_camera 里最新外参,以及指纹相符的内参,填上三台相机。"""
|
||||
import sys
|
||||
workspace = _workspace_root()
|
||||
source = workspace / 'src'
|
||||
if str(source) not in sys.path:
|
||||
sys.path.insert(0, str(source))
|
||||
from camera_parameters.output_paths import latest_extrinsic_path
|
||||
extrinsics_path = latest_extrinsic_path(workspace)
|
||||
if extrinsics_path is None:
|
||||
raise FileNotFoundError(f'{workspace}/output_camera/extrinsics 里没有外参')
|
||||
document = yaml.safe_load(extrinsics_path.read_text(encoding='utf-8'))
|
||||
recorded = document.get('cameras') if isinstance(document, dict) else None
|
||||
if not isinstance(recorded, dict):
|
||||
raise ValueError(f'外参文件不完整: {extrinsics_path}')
|
||||
cameras = station['cameras']
|
||||
for view, device in cameras.items():
|
||||
camera = recorded.get(view)
|
||||
if not isinstance(camera, dict):
|
||||
raise ValueError(f'外参缺少 {view}: {extrinsics_path}')
|
||||
serial = str(camera.get('serial_number') or '')
|
||||
expected = str(camera.get('intrinsics_sha256') or '')
|
||||
if not serial or not expected:
|
||||
raise ValueError(f'外参缺少 {view} 的序列号或内参指纹: {extrinsics_path}')
|
||||
path = _intrinsic_for(workspace, serial, expected, extrinsics_path)
|
||||
intrinsic = yaml.safe_load(path.read_text(encoding='utf-8'))
|
||||
device['serial'] = serial
|
||||
device['intrinsics'] = str(path)
|
||||
station.setdefault('camera', {})
|
||||
station['camera']['width'] = int(intrinsic['image_width'])
|
||||
station['camera']['height'] = int(intrinsic['image_height'])
|
||||
station['extrinsics'] = str(extrinsics_path)
|
||||
_apply_camera_station(station, workspace)
|
||||
|
||||
|
||||
def load_station(path=None):
|
||||
path = Path(path).expanduser() if path else resources() / 'config' / 'station.yaml'
|
||||
data = yaml.safe_load(path.read_text())
|
||||
Settings.from_dict(data.get('scan', {}))
|
||||
bind_output_camera(data)
|
||||
cameras = data['cameras']
|
||||
if len({c['serial'] for c in cameras.values()}) != len(cameras):
|
||||
raise ValueError('相机序列号重复')
|
||||
for key in ('speed', 'torque'):
|
||||
value = data['motion'][key]
|
||||
if not isinstance(value, int) or not 0 <= value <= 255:
|
||||
raise ValueError(f'{key} 必须为0~255整数')
|
||||
return data
|
||||
@@ -0,0 +1,108 @@
|
||||
"""Recompute completed tasks from their samples, not from saved result values."""
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
from .core.analysis import aggregate, direction_range, Unmeasurable, RefineWindow
|
||||
from .core.angle_scan import ANGLE_COUNT, JointCurveScan, angle_targets, command_knots
|
||||
from .core.endpoints import ENDPOINT_METHODS, analyze_endpoint_samples
|
||||
from .profiles import profile_from_dict, Settings
|
||||
from .storage import write_json
|
||||
|
||||
|
||||
def replay(journal, output):
|
||||
samples, thresholds, angle_samples = {}, {}, {}
|
||||
document, profile, settings = None, None, None
|
||||
scan_method = None
|
||||
limits = {}
|
||||
with Path(journal).open(encoding='utf-8') as stream:
|
||||
for line in stream:
|
||||
event = json.loads(line)
|
||||
kind = event['kind']
|
||||
if kind == 'metadata':
|
||||
profile = profile_from_dict(event['profile'])
|
||||
settings = Settings.from_dict(event['settings'])
|
||||
scan_method = event.get('scan_method', 'coarse_fine_v1')
|
||||
if scan_method == 'angle_curve_v1':
|
||||
limits = event['limits']
|
||||
knots = list(event.get('command_knots') or command_knots(int(profile.joints[0].maximum)))
|
||||
document = {'schema_version': 2, 'model': profile.model, 'side': profile.side,
|
||||
'device_uid': event['uid'], 'command_unit': profile.command_unit,
|
||||
'angle_unit': 'rad', 'command_knots': knots,
|
||||
'joints': {j.name: {
|
||||
'angles_rad': [float(v) for v in angle_targets(*limits[j.name])],
|
||||
'commands': [None] * ANGLE_COUNT,
|
||||
} for j in profile.joints}}
|
||||
elif scan_method in (*ENDPOINT_METHODS, 'coarse_fine_v1'):
|
||||
document = {'schema_version': 1, 'model': profile.model, 'side': profile.side,
|
||||
'device_uid': event['uid'], 'command_unit': profile.command_unit,
|
||||
'joints': {j.name: {'min': None, 'max': None} for j in profile.joints}}
|
||||
else:
|
||||
raise ValueError(f'不支持的日志扫描方法: {scan_method}')
|
||||
elif document is None:
|
||||
raise ValueError('日志缺少首行 metadata')
|
||||
elif kind == 'task_started' and scan_method == 'angle_curve_v1':
|
||||
for name in event['joints']:
|
||||
angle_samples[(event['task'], name)] = []
|
||||
elif kind == 'task_started':
|
||||
samples[event['task']] = {}
|
||||
for name in event['joints']:
|
||||
document['joints'][name] = {'min': None, 'max': None}
|
||||
elif kind == 'baseline':
|
||||
thresholds[event['task']] = event['thresholds']
|
||||
elif kind == 'fine_reset':
|
||||
data = samples[event['task']]
|
||||
samples[event['task']] = {k: v for k, v in data.items() if k[0] == 'coarse'}
|
||||
elif kind == 'sample':
|
||||
key = event['stage'], event['repeat'], event['direction']
|
||||
samples[event['task']].setdefault(key, []).append(event)
|
||||
elif kind == 'angle_sample':
|
||||
angle_samples.setdefault((event['task'], event['joint']), []).append(event)
|
||||
elif kind == 'task_result' and scan_method == 'angle_curve_v1':
|
||||
task = next(t for t in profile.tasks if t.name == event['task'])
|
||||
for name in task.joints:
|
||||
document['joints'][name]['commands'] = _replay_angle_commands(
|
||||
profile.by_name[name], settings, limits[name],
|
||||
angle_samples.get((task.name, name), []))
|
||||
elif kind == 'task_result':
|
||||
task = next(t for t in profile.tasks if t.name == event['task'])
|
||||
data = samples[task.name]
|
||||
for name in task.joints:
|
||||
joint = profile.by_name[name]
|
||||
try:
|
||||
if scan_method in ENDPOINT_METHODS:
|
||||
sweeps = {(r, d): v for (s, r, d), v in data.items() if s == 'endpoint'}
|
||||
result = analyze_endpoint_samples(joint, sweeps, settings, scan_method)
|
||||
else:
|
||||
threshold = thresholds[task.name][name]
|
||||
for direction in ('up', 'down'):
|
||||
direction_range(data[('coarse', 0, direction)], joint, threshold, settings, coarse=True)
|
||||
result = aggregate(joint, {(r, d): v for (s, r, d), v in data.items() if s == 'fine'},
|
||||
threshold, settings)
|
||||
document['joints'][name] = {k: result[k] for k in ('min', 'max')}
|
||||
except (Unmeasurable, RefineWindow, KeyError):
|
||||
document['joints'][name] = {'min': None, 'max': None}
|
||||
if document is None:
|
||||
raise ValueError('空日志')
|
||||
write_json(output, document)
|
||||
return document
|
||||
|
||||
|
||||
def _replay_angle_commands(joint, settings, limits, samples):
|
||||
lower, upper = limits
|
||||
scan = JointCurveScan(joint, settings, angle_targets(lower, upper))
|
||||
scan.start()
|
||||
for sample in samples:
|
||||
if sample.get('reason'):
|
||||
continue
|
||||
if scan.commands is not None or scan.error:
|
||||
break
|
||||
if int(sample['command']) != int(scan.command):
|
||||
return [None] * ANGLE_COUNT
|
||||
status = scan.accept(sample['theta_rad'])
|
||||
if status == 'fail':
|
||||
return [None] * ANGLE_COUNT
|
||||
if status == 'done':
|
||||
break
|
||||
if scan.commands is None:
|
||||
return [None] * ANGLE_COUNT
|
||||
return list(scan.commands)
|
||||
@@ -0,0 +1,386 @@
|
||||
"""ROS host. All engine/adapter mutations run on the ROS executor thread."""
|
||||
from collections import OrderedDict
|
||||
import copy
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
from queue import Empty, Queue
|
||||
from threading import Lock
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from apriltag_msgs.msg import AprilTagDetectionArray
|
||||
from cv_bridge import CvBridge
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
|
||||
from .adapters.fake import FakeAdapter
|
||||
from .adapters.registry import adapter_type
|
||||
from .angle_model import identity_angle_model, load_angle_model
|
||||
from .core.diagnostics import tag_label
|
||||
from .core.engine import Engine
|
||||
from .offset import cleared_offset, load_ranges_document, ranges_to_set_offset
|
||||
from .profiles import Settings
|
||||
from .simulation import simulated_teaching
|
||||
from .storage import Session, Teaching, safe_uid
|
||||
from .vision.observations import quality_error
|
||||
|
||||
|
||||
NAMESPACE = '/linkerhand_range_calibration'
|
||||
|
||||
|
||||
class CalibrationRuntime(Node):
|
||||
def __init__(self, profile, station, demo=False):
|
||||
super().__init__('range_controller', namespace=NAMESPACE)
|
||||
self.profile, self.station, self.demo = profile, station, demo
|
||||
self.settings = Settings.from_dict(station.get('scan', {}))
|
||||
self.adapter = FakeAdapter(profile) if demo else adapter_type(profile.adapter)(self, profile, self.settings)
|
||||
self.angle_model = identity_angle_model(profile) if demo else load_angle_model(profile, station)
|
||||
self.engine = Engine(profile, self.settings, self.adapter, self.angle_model)
|
||||
self.queue, self.lock = Queue(), Lock()
|
||||
self.input_lock = Lock()
|
||||
self.pending_jog, self.manual_epoch = {}, 0
|
||||
self.applied_epoch = 0
|
||||
self.snapshot_data = {}
|
||||
self.bridge = CvBridge()
|
||||
self.session, self.teaching, self.teaching_path = None, None, None
|
||||
self.device_uid, self.message = '', ''
|
||||
self.motion_settings_key = None
|
||||
self.frames, self.tag_frames, self.tags = {}, {}, {}
|
||||
self.camera_info, self.camera_received, self.image_times = {}, {}, {}
|
||||
self.camera_error = {}
|
||||
self.subscriptions_owned = []
|
||||
self.preview_at = {}
|
||||
if not demo:
|
||||
for view in profile.views:
|
||||
prefix = f'{NAMESPACE}/{view}'
|
||||
self.subscriptions_owned.extend([
|
||||
self.create_subscription(CameraInfo, prefix+'/camera/camera_info',
|
||||
lambda m,v=view:self._camera(v,m), qos_profile_sensor_data),
|
||||
self.create_subscription(Image, prefix+'/camera/image_rect',
|
||||
lambda m,v=view:self._image(v,m), qos_profile_sensor_data),
|
||||
self.create_subscription(AprilTagDetectionArray, prefix+'/apriltag/detections',
|
||||
lambda m,v=view:self._detections(v,m), qos_profile_sensor_data),
|
||||
])
|
||||
self.timer = self.create_timer(1/30, self._tick)
|
||||
|
||||
def submit(self, action, **data):
|
||||
with self.input_lock:
|
||||
if action == 'jog':
|
||||
# One latest target per joint, consumed at the existing 30 Hz tick.
|
||||
if data['epoch'] == self.manual_epoch:
|
||||
self.pending_jog.update(data['targets'])
|
||||
else:
|
||||
# A control action invalidates gestures from the previous UI state.
|
||||
self.manual_epoch += 1
|
||||
self.pending_jog.clear()
|
||||
self.queue.put((action, data))
|
||||
|
||||
def _take_inputs(self):
|
||||
with self.input_lock:
|
||||
actions = []
|
||||
while True:
|
||||
try:
|
||||
actions.append(self.queue.get_nowait())
|
||||
except Empty:
|
||||
break
|
||||
targets, self.pending_jog = self.pending_jog, {}
|
||||
if targets:
|
||||
actions.append(('jog', {'targets': targets, 'epoch': self.manual_epoch}))
|
||||
return actions, self.manual_epoch
|
||||
|
||||
def snapshot(self):
|
||||
with self.lock:
|
||||
return copy.deepcopy(self.snapshot_data)
|
||||
|
||||
def previews(self):
|
||||
with self.lock:
|
||||
return {view: (stamp, array, copy.deepcopy(self.tag_frames.get(view, {}).get(stamp, {})))
|
||||
for view, (stamp, array) in self.frames.items()}
|
||||
|
||||
def _camera(self, view, message):
|
||||
signature = (message.width, message.height, tuple(message.k), tuple(message.d),
|
||||
tuple(message.r), tuple(message.p))
|
||||
valid = (message.width == self.station['camera']['width'] and
|
||||
message.height == self.station['camera']['height'] and message.k[0] > 0 and
|
||||
message.k[4] > 0 and message.p[0] > 0 and message.p[5] > 0 and
|
||||
all(np.isfinite(v).all() for v in (message.k, message.d, message.r, message.p)))
|
||||
if not valid:
|
||||
self.camera_error[view] = '相机内参或图像尺寸无效'
|
||||
else:
|
||||
self.camera_error.pop(view, None)
|
||||
if view in self.camera_info and signature != self.camera_info[view] and self.engine.active:
|
||||
self.engine.pause(f'{view} 相机参数发生变化,请重新检查')
|
||||
self.camera_info[view], self.camera_received[view] = signature, time.monotonic()
|
||||
|
||||
def _image(self, view, message):
|
||||
now = time.monotonic()
|
||||
if now-self.preview_at.get(view, float('-inf')) < .1:
|
||||
return
|
||||
self.preview_at[view] = now
|
||||
stamp = message.header.stamp.sec*1_000_000_000+message.header.stamp.nanosec
|
||||
try:
|
||||
frame = self.bridge.imgmsg_to_cv2(message, desired_encoding='bgr8')
|
||||
with self.lock:
|
||||
self.frames[view] = (stamp, np.ascontiguousarray(frame))
|
||||
except Exception as error:
|
||||
self.message = f'预览解码失败: {error}'
|
||||
|
||||
def _detections(self, view, message):
|
||||
now = time.monotonic()
|
||||
stamp = message.header.stamp.sec*1_000_000_000+message.header.stamp.nanosec
|
||||
if abs(self.get_clock().now().nanoseconds-stamp) > int(self.settings.freshness*1e9):
|
||||
return
|
||||
if view not in self.camera_info or view in self.camera_error:
|
||||
return
|
||||
width, height = self.camera_info[view][:2]
|
||||
expected = {j.tag_id for j in self.profile.joints if j.view == view}
|
||||
statuses = {tag: {'error': '未检测到', 'corners': []} for tag in expected}
|
||||
accepted, seen = {}, set()
|
||||
for detection in message.detections:
|
||||
tag = int(detection.id)
|
||||
if tag not in expected:
|
||||
continue
|
||||
points = [(float(p.x), float(p.y)) for p in detection.corners]
|
||||
error = quality_error(points, width, height, detection.hamming,
|
||||
detection.decision_margin, self.settings)
|
||||
if detection.family.removeprefix('tag') != self.station['camera']['family'].removeprefix('tag'):
|
||||
error = 'Tag 家族不符'
|
||||
if tag in seen:
|
||||
error = '同一帧出现重复 Tag ID'
|
||||
seen.add(tag)
|
||||
statuses[tag] = {'error': error, 'corners': points}
|
||||
if error:
|
||||
accepted.pop(tag, None)
|
||||
else:
|
||||
accepted[tag] = points
|
||||
self.tags[view], self.image_times[view] = statuses, now
|
||||
with self.lock:
|
||||
cache = self.tag_frames.setdefault(view, OrderedDict())
|
||||
cache[stamp] = statuses
|
||||
while len(cache) > 40:
|
||||
cache.popitem(last=False)
|
||||
rejected = {tag: statuses[tag]['error'] for tag in seen if statuses[tag]['error']}
|
||||
self.engine.observe(view, stamp, accepted, now, rejected=rejected)
|
||||
|
||||
def _vision_error(self, requirements, now):
|
||||
if self.demo:
|
||||
return None
|
||||
for view in dict.fromkeys(view for view, tag in requirements.values()):
|
||||
if view in self.camera_error:
|
||||
reason = self.camera_error[view]
|
||||
elif now-self.camera_received.get(view, float('-inf')) > 1:
|
||||
reason = 'CameraInfo 未收到或断流'
|
||||
elif now-self.image_times.get(view, float('-inf')) > self.settings.freshness:
|
||||
reason = '有效时间戳的检测流未收到或断流'
|
||||
else:
|
||||
continue
|
||||
affected = '、'.join(tag_label(v, tag, name) for name, (v, tag) in requirements.items()
|
||||
if v == view)
|
||||
return f'{affected}:{reason}'
|
||||
return None
|
||||
|
||||
def _device(self):
|
||||
uid = self.adapter.uid
|
||||
if not uid or uid == self.device_uid or self.engine.active or self.engine.state == 'PAUSED':
|
||||
return
|
||||
safe_uid(uid)
|
||||
with self.input_lock:
|
||||
self.manual_epoch += 1
|
||||
self.pending_jog.clear()
|
||||
if self.session:
|
||||
self.session.close()
|
||||
self.session = None
|
||||
self.engine = Engine(self.profile, self.settings, self.adapter, self.angle_model)
|
||||
root = Path(self.station['teaching_root']).expanduser()
|
||||
self.teaching_path = root / uid / 'teaching.yaml'
|
||||
self.teaching = (simulated_teaching(self.profile, uid) if self.demo else
|
||||
Teaching.load(self.teaching_path, self.profile, uid))
|
||||
self.device_uid = uid
|
||||
self.motion_settings_key = None
|
||||
|
||||
def _ensure_motion_settings(self, force=False):
|
||||
motion = self.station['motion']
|
||||
key = (self.adapter.uid, motion['speed'], motion['torque'])
|
||||
if force or key != self.motion_settings_key:
|
||||
self.motion_settings_key = None
|
||||
self.adapter.set_motion(**motion)
|
||||
self.motion_settings_key = key
|
||||
|
||||
def _action(self, action, data, now, stamp):
|
||||
e = self.engine
|
||||
if action == 'cancel':
|
||||
e.cancel()
|
||||
elif action == 'pause':
|
||||
if e.active:
|
||||
e.pause('用户暂停;继续时重新采集当前任务')
|
||||
elif action == 'resume':
|
||||
e.resume(now, stamp)
|
||||
elif action in ('jog', 'start', 'save'):
|
||||
error = self.adapter.control_error(now)
|
||||
if error:
|
||||
self.motion_settings_key = None
|
||||
raise ValueError(error)
|
||||
if not self.teaching:
|
||||
raise ValueError('设备示教配置尚未加载')
|
||||
if action == 'jog':
|
||||
with self.input_lock:
|
||||
if data['epoch'] != self.manual_epoch:
|
||||
return
|
||||
e.manual_adjust(data['targets'], now)
|
||||
try:
|
||||
self._ensure_motion_settings()
|
||||
except Exception:
|
||||
self.motion_settings_key = None
|
||||
e.pause('速度或力矩设置失败,请检查 SDK 后取消并重新示教')
|
||||
raise
|
||||
elif action == 'save':
|
||||
if not e.can_capture_teaching(now):
|
||||
raise ValueError('请等待运动完成和反馈稳定后保存示教')
|
||||
target = e.command if e.command is not None else self.adapter.feedback.positions
|
||||
self.teaching.capture(data['kind'], target, self.adapter.feedback.positions, data.get('key'))
|
||||
self.teaching.save(self.teaching_path)
|
||||
self.message = f'示教已保存: {self.teaching_path}'
|
||||
else:
|
||||
if e.active or e.state == 'PAUSED':
|
||||
raise ValueError('请先结束当前任务')
|
||||
names = data.get('tasks')
|
||||
tasks = [t for t in self.profile.tasks if names is None or t.name in names]
|
||||
if not tasks or (names and set(names) != {t.name for t in tasks}):
|
||||
raise ValueError('扫描任务不存在')
|
||||
missing = self.teaching.missing(tasks)
|
||||
if missing:
|
||||
raise ValueError('缺少示教: ' + ', '.join(missing))
|
||||
requirements = {name: (self.profile.by_name[name].view, self.profile.by_name[name].tag_id)
|
||||
for task in tasks for name in task.joints}
|
||||
error = self._vision_error(requirements, now)
|
||||
if error:
|
||||
raise ValueError(error)
|
||||
session = Session(self.station['output_root'], self.profile, self.device_uid,
|
||||
asdict(self.settings), self.teaching, self.station['motion'])
|
||||
if self.session:
|
||||
self.session.close()
|
||||
self.session = session
|
||||
e.emit = self.session.emit
|
||||
self._ensure_motion_settings(force=True)
|
||||
e.start(self.teaching, now, stamp, names)
|
||||
self.message = f'结果目录: {self.session.directory}'
|
||||
elif action == 'apply_offset':
|
||||
if e.active or e.state == 'PAUSED':
|
||||
raise ValueError('请先结束或取消当前标定任务,再下发行程偏移')
|
||||
error = self.adapter.control_error(now)
|
||||
if error:
|
||||
raise ValueError(error)
|
||||
path = Path(data['path']).expanduser()
|
||||
document = load_ranges_document(path)
|
||||
if (self.device_uid and document.get('device_uid')
|
||||
and document['device_uid'] != self.device_uid):
|
||||
raise ValueError(
|
||||
f'结果设备 UID {document["device_uid"]} 与当前连接 {self.device_uid} 不一致')
|
||||
payload = ranges_to_set_offset(document, self.profile)
|
||||
self.adapter.set_offset(payload['params']['offset'])
|
||||
self.message = f'已下发行程偏移到设备({path})'
|
||||
elif action == 'clear_offset':
|
||||
if e.active or e.state == 'PAUSED':
|
||||
raise ValueError('请先结束或取消当前标定任务,再清除行程偏移')
|
||||
error = self.adapter.control_error(now)
|
||||
if error:
|
||||
raise ValueError(error)
|
||||
self.adapter.set_offset(cleared_offset(self.profile))
|
||||
self.message = '已清除行程偏移(还原为全行程)'
|
||||
else:
|
||||
raise ValueError(f'未知操作 {action}')
|
||||
|
||||
def _demo_frame(self, now, stamp):
|
||||
self.adapter.advance(now, stamp)
|
||||
active = None
|
||||
if self.engine.state == 'SCANNING' and self.engine.angle_task is not None:
|
||||
active = self.engine.angle_task.joint_name
|
||||
for view, detections in self.adapter.frames(angle_model=self.angle_model, active=active).items():
|
||||
self.engine.observe(view, stamp, detections, now)
|
||||
self.image_times[view] = now
|
||||
if now-self.preview_at.get(view, float('-inf')) < .1:
|
||||
continue
|
||||
self.preview_at[view] = now
|
||||
frame = np.full((1240, 1624, 3), 245, dtype=np.uint8)
|
||||
statuses = {}
|
||||
for tag, points in detections.items():
|
||||
cv2.fillConvexPoly(frame, np.asarray(points, dtype=np.int32), (70,70,70))
|
||||
statuses[tag] = {'error': None, 'corners': points}
|
||||
self.tags[view] = statuses
|
||||
with self.lock:
|
||||
self.frames[view] = (stamp, frame)
|
||||
self.tag_frames[view] = {stamp: statuses}
|
||||
|
||||
def _tick(self):
|
||||
now, stamp = time.monotonic(), self.get_clock().now().nanoseconds
|
||||
try:
|
||||
if self.demo:
|
||||
self._demo_frame(now, stamp)
|
||||
self._device()
|
||||
actions, epoch = self._take_inputs()
|
||||
for action, data in actions:
|
||||
try:
|
||||
self._action(action, data, now, stamp)
|
||||
except (ValueError, OSError) as error:
|
||||
self.message = str(error)
|
||||
self.applied_epoch = epoch
|
||||
if self.engine.state in ('PREPARING', 'SCANNING'):
|
||||
error = self._vision_error(self.engine.keys, now)
|
||||
if error:
|
||||
self.engine.pause(error)
|
||||
self.engine.tick(now, stamp)
|
||||
except Exception as error:
|
||||
self.message = f'运行异常: {error}'
|
||||
if self.engine.active:
|
||||
try:
|
||||
self.engine.pause(self.message)
|
||||
except OSError as storage_error:
|
||||
self.message += f';日志写入失败: {storage_error}'
|
||||
self.get_logger().error(self.message)
|
||||
feedback = self.adapter.feedback
|
||||
if self.engine.state == 'TEACHING':
|
||||
manual_target = self.engine.manual_target
|
||||
else:
|
||||
manual_target = self.engine.command or (feedback.positions if feedback else None)
|
||||
tags = copy.deepcopy(self.tags)
|
||||
control_error = self.adapter.control_error(now)
|
||||
if control_error:
|
||||
self.motion_settings_key = None
|
||||
for view, entries in tags.items():
|
||||
if now-self.image_times.get(view, float('-inf')) > self.settings.freshness:
|
||||
for entry in entries.values():
|
||||
entry['error'] = '观测过期'
|
||||
with self.lock:
|
||||
self.snapshot_data = {
|
||||
'state': self.engine.state, 'reason': self.engine.reason, 'message': self.message,
|
||||
'uid': self.adapter.uid,
|
||||
'control_error': control_error,
|
||||
'positions': list(feedback.positions) if feedback else None,
|
||||
'command': self.engine.command, 'results': copy.deepcopy(self.engine.results),
|
||||
'manual_target': manual_target, 'manual_epoch': self.applied_epoch,
|
||||
'manual_allowed': self.engine.manual_allowed,
|
||||
'task': self.engine.task.name if self.engine.task else '',
|
||||
'task_index': self.engine.task_index, 'task_count': len(self.engine.tasks),
|
||||
'stage': self.engine.stage, 'direction': self.engine.direction,
|
||||
'restoring_joint': self.engine.restoring_joint,
|
||||
'scan_segment': (self.engine.segment.label
|
||||
if self.engine.state == 'SCANNING' and self.engine.segment else ''),
|
||||
'scan_commands': ({joint.name: self.engine.command[joint.index]
|
||||
for joint in self.engine.specs}
|
||||
if self.engine.state == 'SCANNING' else {}),
|
||||
'repeat': self.engine.repeat, 'progress': self.engine.progress,
|
||||
'active': self.engine.active,
|
||||
'can_save': self.engine.can_capture_teaching(now),
|
||||
'missing': self.teaching.missing(self.profile.tasks) if self.teaching else ['等待设备身份'],
|
||||
'tags': tags,
|
||||
'output': str(self.session.result_path) if self.session else '',
|
||||
}
|
||||
|
||||
def close_session(self):
|
||||
self.timer.cancel()
|
||||
if self.engine.active or self.engine.state == 'PAUSED':
|
||||
self.engine.cancel()
|
||||
if self.session:
|
||||
self.session.close()
|
||||
@@ -0,0 +1,91 @@
|
||||
"""Fast virtual-time end-to-end exercise. Never imports ROS or a real adapter."""
|
||||
from dataclasses import asdict, replace
|
||||
|
||||
from .adapters.fake import FakeAdapter, synthetic_theta
|
||||
from .angle_model import identity_angle_model
|
||||
from .core.angle_scan import JointCurveScan, angle_targets
|
||||
from .core.engine import Engine
|
||||
from .storage import Teaching, Session
|
||||
|
||||
|
||||
def simulated_teaching(profile, uid):
|
||||
teaching = Teaching(profile, uid)
|
||||
base = [j.minimum + round((j.maximum-j.minimum)/j.resolution/4)*j.resolution for j in profile.joints]
|
||||
teaching.capture('baseline', base, base)
|
||||
for group, clearance in profile.teaching_clearances.items():
|
||||
folded = list(base)
|
||||
for name in clearance.joints:
|
||||
j = profile.by_name[name]
|
||||
folded[j.index] = j.maximum
|
||||
teaching.capture('clearances', folded, folded, group)
|
||||
return teaching
|
||||
|
||||
|
||||
def expected_commands(joint, settings, lower, upper, travel_min, travel_max):
|
||||
"""用和假手相同的角度,单独跑一遍扫描,得到应当写出的 10 个指令。"""
|
||||
scan = JointCurveScan(joint, settings, angle_targets(lower, upper))
|
||||
scan.start()
|
||||
for _ in range(4000):
|
||||
if scan.commands is not None:
|
||||
return list(scan.commands)
|
||||
if scan.error:
|
||||
raise RuntimeError(scan.error)
|
||||
theta = synthetic_theta(scan.command, travel_min, travel_max, lower, upper)
|
||||
status = scan.accept(theta)
|
||||
if status == 'fail':
|
||||
raise RuntimeError(scan.error or '角度扫描失败')
|
||||
if status == 'done':
|
||||
return list(scan.commands)
|
||||
raise RuntimeError(f'{joint.name} 的期望曲线没有收敛')
|
||||
|
||||
|
||||
def simulate(profile, settings, output, task_names=None):
|
||||
ranges = {}
|
||||
for i, joint in enumerate(profile.joints):
|
||||
if joint.minimum == 0 and joint.maximum == 255:
|
||||
ranges[joint.name] = [(0, 230), (7, 255), (6, 243), (0, 255)][i % 4]
|
||||
else:
|
||||
ranges[joint.name] = (joint.minimum, joint.maximum)
|
||||
adapter = FakeAdapter(profile, ranges)
|
||||
teaching = simulated_teaching(profile, adapter.uid)
|
||||
# 一步指令的角度变化大约千分之几弧度,门槛必须低于它,否则假手会被判成没有行程。
|
||||
settings = replace(settings, rate=10000, settle_seconds=0.01, stable_frames=4,
|
||||
angle_motion_rad=1e-6)
|
||||
angle_model = identity_angle_model(profile)
|
||||
session = Session(output, profile, adapter.uid, asdict(settings), teaching, {'simulated': True})
|
||||
engine = Engine(profile, settings, adapter, angle_model, session.emit)
|
||||
now = 1.0
|
||||
adapter.advance(now, int(now*1e9))
|
||||
engine.start(teaching, now, int(now*1e9), task_names)
|
||||
try:
|
||||
for _ in range(300000):
|
||||
now += 0.1
|
||||
stamp = round(now*1e9)
|
||||
adapter.advance(now, stamp)
|
||||
active = None
|
||||
if engine.state == 'SCANNING' and engine.angle_task is not None:
|
||||
active = engine.angle_task.joint_name
|
||||
for view, detections in adapter.frames(angle_model=angle_model, active=active).items():
|
||||
engine.observe(view, stamp, detections, now)
|
||||
engine.tick(now, stamp)
|
||||
if engine.state in ('COMPLETED', 'PAUSED', 'CANCELLED'):
|
||||
break
|
||||
if engine.state != 'COMPLETED':
|
||||
raise RuntimeError(f'模拟流程未完成: {engine.state} {engine.reason}')
|
||||
selected = {name for task in engine.tasks for name in task.joints}
|
||||
document = session.document()
|
||||
if 'min' in document or any('min' in entry or 'max' in entry for entry in document['joints'].values()):
|
||||
raise RuntimeError('角度曲线结果里不应再写独立的 min/max')
|
||||
for name in selected:
|
||||
result = engine.results[name]
|
||||
travel_min, travel_max = ranges[name]
|
||||
lower, upper = angle_model.kinematics.limits(name)
|
||||
expected = expected_commands(
|
||||
profile.by_name[name], settings, lower, upper, travel_min, travel_max)
|
||||
if result.get('status') != '成功' or result['commands'] != expected:
|
||||
raise RuntimeError(f'模拟曲线不符: {name}: {result}; expected={expected}')
|
||||
if document['joints'][name]['commands'] != expected:
|
||||
raise RuntimeError(f'结果文件与扫描不一致: {name}')
|
||||
return session.result_path
|
||||
finally:
|
||||
session.close()
|
||||
@@ -0,0 +1,183 @@
|
||||
"""Small public result files; full provenance stays in an append-only journal."""
|
||||
from datetime import datetime
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import tempfile
|
||||
|
||||
import yaml
|
||||
|
||||
from .core.angle_scan import ANGLE_COUNT, SCAN_METHOD, angle_targets, command_knots
|
||||
from .vision.observations import SAMPLING_METHOD
|
||||
|
||||
|
||||
def safe_uid(uid):
|
||||
if not isinstance(uid, str) or not re.fullmatch(r'[A-Za-z0-9_-][A-Za-z0-9_.-]*', uid):
|
||||
raise ValueError('设备 UID 为空或含不支持的路径字符')
|
||||
return uid
|
||||
|
||||
|
||||
def atomic_text(path, text):
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fd, temporary = tempfile.mkstemp(prefix=path.name + '.', dir=path.parent)
|
||||
try:
|
||||
with os.fdopen(fd, 'w', encoding='utf-8') as stream:
|
||||
stream.write(text)
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
os.replace(temporary, path)
|
||||
finally:
|
||||
if os.path.exists(temporary):
|
||||
os.unlink(temporary)
|
||||
|
||||
|
||||
def write_json(path, value):
|
||||
atomic_text(path, json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False) + '\n')
|
||||
|
||||
|
||||
class Teaching:
|
||||
def __init__(self, profile, uid, data=None):
|
||||
self.profile, self.uid = profile, safe_uid(uid)
|
||||
self.data = data or {'schema_version': 1, 'model': profile.model,
|
||||
'side': profile.side, 'device_uid': uid,
|
||||
'baseline': None, 'clearances': {}, 'overrides': {}}
|
||||
if (self.data.get('schema_version'), self.data.get('model'), self.data.get('side'),
|
||||
self.data.get('device_uid')) != (1, profile.model, profile.side, uid):
|
||||
raise ValueError('示教文件与当前型号/左右手/设备 UID 不符')
|
||||
if self.data['baseline']:
|
||||
profile.vector(self.data['baseline']['target'])
|
||||
profile.vector(self.data['baseline']['feedback'])
|
||||
tasks = {t.name for t in profile.tasks}
|
||||
for kind in ('clearances', 'overrides'):
|
||||
for key, record in self.data[kind].items():
|
||||
if key not in (profile.clearances if kind == 'clearances' else tasks):
|
||||
raise ValueError(f'未知示教项 {key}')
|
||||
if kind == 'overrides' and not next(t for t in profile.tasks if t.name == key).allow_override:
|
||||
raise ValueError(f'{key} 固定使用基础姿态,不允许覆盖')
|
||||
values = record['target']
|
||||
expected = set(profile.clearances[key].joints) if kind == 'clearances' else set(profile.by_name)
|
||||
if set(values) != expected:
|
||||
raise ValueError(f'示教项 {key} 的关节集合不完整')
|
||||
for name, value in values.items():
|
||||
profile.by_name[name].validate_value(value)
|
||||
|
||||
@classmethod
|
||||
def load(cls, path, profile, uid):
|
||||
path = Path(path)
|
||||
return cls(profile, uid, yaml.safe_load(path.read_text()) if path.exists() else None)
|
||||
|
||||
def save(self, path):
|
||||
atomic_text(path, yaml.safe_dump(self.data, allow_unicode=True, sort_keys=False))
|
||||
|
||||
def capture(self, kind, target, feedback, key=None):
|
||||
target, feedback = self.profile.vector(target), self.profile.vector(feedback)
|
||||
record = {'target': list(target), 'feedback': list(feedback)}
|
||||
if kind == 'baseline':
|
||||
self.data['baseline'] = record
|
||||
return
|
||||
if kind == 'clearances':
|
||||
clearance = self.profile.clearances[key]
|
||||
if clearance.targets is not None:
|
||||
raise ValueError('该避让姿态由型号配置固定,无需手动示教')
|
||||
names = clearance.joints
|
||||
elif kind == 'overrides' and key in {t.name for t in self.profile.tasks}:
|
||||
if not next(t for t in self.profile.tasks if t.name == key).allow_override:
|
||||
raise ValueError('该任务固定使用基础姿态,不应用姿态覆盖或避让')
|
||||
names = self.profile.by_name
|
||||
else:
|
||||
raise ValueError('未知示教类别')
|
||||
self.data[kind][key] = {field: {name: values[self.profile.by_name[name].index]
|
||||
for name in names} for field, values in record.items()}
|
||||
|
||||
def missing(self, tasks):
|
||||
missing = [] if self.data['baseline'] else ['全手基础姿态']
|
||||
missing.extend(c for c in dict.fromkeys(c for t in tasks for c in t.clearances)
|
||||
if self.profile.clearances[c].targets is None and c not in self.data['clearances'])
|
||||
return missing
|
||||
|
||||
def prepare(self, task):
|
||||
missing = self.missing([task])
|
||||
if missing:
|
||||
raise ValueError('缺少示教: ' + ', '.join(missing))
|
||||
pose = list(self.data['baseline']['target'])
|
||||
# Task overrides describe the visible working pose; accumulated clearance
|
||||
# always wins, so an override cannot accidentally unfold an occluder.
|
||||
updates = dict(self.data['overrides'].get(task.name, {}).get('target', {}))
|
||||
for group in task.clearances:
|
||||
clearance = self.profile.clearances[group]
|
||||
targets = (clearance.targets if clearance.targets is not None
|
||||
else self.data['clearances'][group]['target'])
|
||||
updates.update(targets)
|
||||
for name, value in updates.items():
|
||||
pose[self.profile.by_name[name].index] = value
|
||||
return self.profile.vector(pose)
|
||||
|
||||
def restoration_steps(self, task, current):
|
||||
"""Build ordered baseline moves, changing only one joint per step."""
|
||||
pose = list(self.profile.vector(current))
|
||||
if task.restore_after and not self.data['baseline']:
|
||||
raise ValueError('缺少示教: 全手基础姿态')
|
||||
steps = []
|
||||
for name in task.restore_after:
|
||||
index = self.profile.by_name[name].index
|
||||
pose[index] = self.data['baseline']['target'][index]
|
||||
steps.append((name, self.profile.vector(pose)))
|
||||
return steps
|
||||
|
||||
|
||||
class Session:
|
||||
def __init__(self, root, profile, uid, settings, teaching, motion):
|
||||
self.profile, self.uid = profile, safe_uid(uid)
|
||||
started = datetime.now()
|
||||
self.directory = Path(root).expanduser() / uid / started.strftime('%Y%m%d_%H%M%S_%f')
|
||||
self.directory.mkdir(parents=True, exist_ok=False)
|
||||
# O30_<device_uid>_<YYYYMMDDHHMM>.json — UID already distinguishes left/right hands.
|
||||
self.result_path = self.directory / f'{profile.model}_{self.uid}_{started.strftime("%Y%m%d%H%M")}.json'
|
||||
self.journal = (self.directory / 'samples.jsonl').open('a', encoding='utf-8', buffering=1)
|
||||
if profile.kinematics is None:
|
||||
raise ValueError('型号配置缺少 URDF,不能写角度曲线')
|
||||
self.knots = command_knots(int(profile.joints[0].maximum))
|
||||
limits = {}
|
||||
self.results = {}
|
||||
for joint in profile.joints:
|
||||
lower, upper = profile.kinematics.limits(joint.name)
|
||||
limits[joint.name] = [lower, upper]
|
||||
self.results[joint.name] = {
|
||||
'angles_rad': [float(value) for value in angle_targets(lower, upper)],
|
||||
'commands': [None] * ANGLE_COUNT,
|
||||
}
|
||||
self.emit({'kind': 'metadata', 'profile': profile.raw, 'settings': settings,
|
||||
'teaching': teaching.data, 'motion': motion, 'uid': uid, 'scan_method': SCAN_METHOD,
|
||||
'sampling_method': SAMPLING_METHOD, 'limits': limits,
|
||||
'command_knots': list(self.knots)})
|
||||
self.save()
|
||||
|
||||
def emit(self, event):
|
||||
self.journal.write(json.dumps(event, ensure_ascii=False, allow_nan=False) + '\n')
|
||||
if event['kind'] == 'task_result':
|
||||
for name, result in event['results'].items():
|
||||
commands = result.get('commands') or [None] * ANGLE_COUNT
|
||||
self.results[name] = {
|
||||
'angles_rad': list(result['angles_rad']) if result.get('angles_rad') is not None
|
||||
else self.results[name]['angles_rad'],
|
||||
'commands': [None if item is None else int(item) for item in commands],
|
||||
}
|
||||
self.save()
|
||||
elif event['kind'] in ('cancelled', 'paused', 'completed'):
|
||||
self.save()
|
||||
|
||||
def document(self):
|
||||
return {'schema_version': 2, 'model': self.profile.model, 'side': self.profile.side,
|
||||
'device_uid': self.uid, 'command_unit': self.profile.command_unit,
|
||||
'angle_unit': 'rad', 'command_knots': list(self.knots),
|
||||
'joints': self.results}
|
||||
|
||||
def save(self):
|
||||
write_json(self.result_path, self.document())
|
||||
|
||||
def close(self):
|
||||
if not self.journal.closed:
|
||||
self.save()
|
||||
self.journal.close()
|
||||
@@ -0,0 +1 @@
|
||||
"""Image-coordinate observations; no extrinsics or pose reconstruction."""
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Remember motion during a command, including motion that returns to its start."""
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .observations import rms_delta
|
||||
from .reference import jitter_radius
|
||||
|
||||
|
||||
class MotionTrace:
|
||||
"""Keep the command's observations until its settled noise can be evaluated.
|
||||
|
||||
Unlike a two-frame latch, an early noisy excursion is not irreversible.
|
||||
Missing detections are retained as gaps so replay cannot join unrelated frames.
|
||||
"""
|
||||
|
||||
def __init__(self, gate_ns):
|
||||
self.gate_ns = self.last_stamp = gate_ns
|
||||
self.frames = []
|
||||
|
||||
def observe(self, stamp_ns, observation):
|
||||
if stamp_ns <= self.last_stamp:
|
||||
return
|
||||
self.last_stamp = stamp_ns
|
||||
self.frames.append({'stamp_ns': stamp_ns,
|
||||
'corners': np.asarray(observation.corners).tolist() if observation else None})
|
||||
|
||||
def snapshot(self):
|
||||
return {'gate_stamp_ns': self.gate_ns, 'frames': list(self.frames)}
|
||||
|
||||
|
||||
def transient_evidence(trace, reference, reference_radius, settled_frames, settings):
|
||||
"""Require movement beyond both endpoint and current-point jitter envelopes.
|
||||
|
||||
The settled position and a raw frame have different uncertainty. This
|
||||
check uses full, timestamped evidence and never compares motion direction.
|
||||
"""
|
||||
if not trace or not settled_frames:
|
||||
return None
|
||||
radius = jitter_radius([f['corners'] for f in settled_frames])
|
||||
threshold = settings.motion_floor_px + reference_radius + radius
|
||||
pending = deque(maxlen=settings.motion_frames)
|
||||
previous = trace['gate_stamp_ns']
|
||||
for frame in trace['frames']:
|
||||
stamp = frame['stamp_ns']
|
||||
if stamp <= previous:
|
||||
continue
|
||||
if stamp - previous > round(settings.freshness * 1e9):
|
||||
pending.clear()
|
||||
previous = stamp
|
||||
points = np.asarray(frame['corners'], dtype=float)
|
||||
if (points.shape != (4, 2) or not np.isfinite(points).all()
|
||||
or rms_delta(points, reference) <= threshold):
|
||||
pending.clear()
|
||||
continue
|
||||
pending.append(frame)
|
||||
if len(pending) == settings.motion_frames:
|
||||
return {'threshold_px': threshold, 'reference_jitter_radius_px': reference_radius,
|
||||
'point_jitter_radius_px': radius, 'frames': list(pending)}
|
||||
return None
|
||||
|
||||
|
||||
class MotionCapture:
|
||||
"""Historical v9 two-frame latch; retained for reproducing old journals."""
|
||||
def __init__(self, reference, threshold, gate_ns, count, freshness):
|
||||
self.reference, self.threshold = reference, threshold
|
||||
self.gate_ns, self.count = gate_ns, count
|
||||
self.max_gap_ns = round(freshness * 1e9)
|
||||
self.pending = deque(maxlen=count)
|
||||
self.last_stamp = gate_ns
|
||||
self.evidence = None
|
||||
|
||||
def observe(self, observation):
|
||||
"""Return True once; a return to the reference never erases evidence."""
|
||||
if self.evidence is not None:
|
||||
return False
|
||||
if observation is None:
|
||||
self.pending.clear()
|
||||
return False
|
||||
if observation.stamp_ns <= self.last_stamp:
|
||||
return False
|
||||
if observation.stamp_ns - self.last_stamp > self.max_gap_ns:
|
||||
self.pending.clear()
|
||||
self.last_stamp = observation.stamp_ns
|
||||
points = np.asarray(observation.corners, dtype=float)
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
self.pending.clear()
|
||||
return False
|
||||
if rms_delta(observation.corners, self.reference) <= self.threshold:
|
||||
self.pending.clear()
|
||||
return False
|
||||
self.pending.append({'stamp_ns': observation.stamp_ns,
|
||||
'corners': np.asarray(observation.corners).tolist()})
|
||||
if len(self.pending) < self.count:
|
||||
return False
|
||||
self.evidence = {'gate_stamp_ns': self.gate_ns, 'frames': list(self.pending)}
|
||||
return True
|
||||
|
||||
|
||||
def witnessed_motion(evidence, reference, threshold, count, freshness):
|
||||
"""Replay recorded frame evidence without trusting a saved motion flag."""
|
||||
if not evidence or len(evidence.get('frames', [])) < count:
|
||||
return False
|
||||
previous = evidence['gate_stamp_ns']
|
||||
for index, frame in enumerate(evidence['frames']):
|
||||
stamp, points = frame['stamp_ns'], np.asarray(frame['corners'], dtype=float)
|
||||
if stamp <= previous or (index and stamp - previous > round(freshness * 1e9)):
|
||||
return False
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
return False
|
||||
if rms_delta(points, reference) <= threshold:
|
||||
return False
|
||||
previous = stamp
|
||||
return True
|
||||
@@ -0,0 +1,167 @@
|
||||
"""Tag quality and stable windows in image coordinates."""
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
SAMPLING_METHOD = 'separate_noise_drift_v2'
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Observation:
|
||||
stamp_ns: int
|
||||
received: float
|
||||
corners: tuple
|
||||
|
||||
|
||||
def rms_delta(a, b):
|
||||
delta = np.asarray(a, dtype=float) - np.asarray(b, dtype=float)
|
||||
return float(np.sqrt(np.mean(np.sum(delta * delta, axis=-1))))
|
||||
|
||||
|
||||
def noise_sigma(points):
|
||||
values = np.asarray(points, dtype=float)
|
||||
median = np.median(values, axis=0)
|
||||
mad = 1.4826 * np.median(np.abs(values - median), axis=0)
|
||||
return float(np.sqrt(np.mean(np.sum(mad * mad, axis=-1))))
|
||||
|
||||
|
||||
def robust_position(points):
|
||||
"""Average the middle half of each coordinate's observations.
|
||||
|
||||
A median can switch between vibration peaks when a window gains one frame.
|
||||
A trimmed average reduces this phase/count bias while removing isolated
|
||||
corner spikes. Instantaneous motion is checked separately on raw frames.
|
||||
"""
|
||||
values = np.sort(np.asarray(points, dtype=float), axis=0)
|
||||
trim = len(values) // 4
|
||||
return np.mean(values[trim:len(values) - trim], axis=0)
|
||||
|
||||
|
||||
def temporal_motion(points, stamps, drift_sigma):
|
||||
"""Separate a robust time trend from jitter, retaining all four corners.
|
||||
|
||||
Long-baseline pairwise slopes suppress individual bad detections. Residual
|
||||
and frame-difference MAD estimate jitter without counting smooth movement
|
||||
as random noise. Check the whole window and its halves so that slow motion
|
||||
reversing in the middle cannot cancel out. The allowance is heuristic,
|
||||
not a guarantee that camera noise is statistically independent.
|
||||
"""
|
||||
times = (np.asarray(stamps, dtype=np.int64) - stamps[0]) / 1e9
|
||||
centered = times - np.mean(times)
|
||||
velocity = _median_velocity(points, times)
|
||||
residual = points - centered[:, None, None] * velocity
|
||||
difference_mad = 1.4826 * np.median(np.abs(np.diff(residual, axis=0)), axis=0) / np.sqrt(2)
|
||||
difference_noise = float(np.sqrt(np.mean(np.sum(difference_mad ** 2, axis=-1))))
|
||||
jitter = min(noise_sigma(residual), difference_noise)
|
||||
sections = [(points, times)]
|
||||
if len(points) >= 8:
|
||||
middle = len(points) // 2
|
||||
sections.extend([(points[:middle], times[:middle]), (points[middle:], times[middle:])])
|
||||
trends = []
|
||||
for section, clock in sections:
|
||||
span = clock[-1] - clock[0]
|
||||
velocity = _median_velocity(section, clock)
|
||||
trend = float(np.sqrt(np.mean(np.sum((velocity * span) ** 2, axis=-1))))
|
||||
allowance = float(drift_sigma * jitter * span / np.sqrt(np.sum((clock - np.mean(clock)) ** 2)))
|
||||
trends.append((max(0.0, trend - allowance), trend, allowance, span))
|
||||
drift, trend, allowance, span = max(trends, key=lambda value: value[0])
|
||||
return {'jitter_px': jitter, 'trend_px': trend, 'trend_allowance_px': allowance,
|
||||
'trend_span_seconds': float(span), 'drift_px': drift}
|
||||
|
||||
|
||||
def _median_velocity(points, times):
|
||||
span = times[-1] - times[0]
|
||||
left, right = np.triu_indices(len(times), 1)
|
||||
dt = times[right] - times[left]
|
||||
keep = dt >= span / 2
|
||||
slopes = (points[right[keep]] - points[left[keep]]) / dt[keep, None, None]
|
||||
return np.median(slopes, axis=0)
|
||||
|
||||
|
||||
def quality_error(corners, width, height, hamming, margin, settings):
|
||||
p = np.asarray(corners, dtype=float)
|
||||
if p.shape != (4, 2) or not np.isfinite(p).all() or not np.isfinite(margin):
|
||||
return '角点或质量数值无效'
|
||||
if hamming > settings.max_hamming or margin < settings.min_margin:
|
||||
return 'Tag 解码质量不足'
|
||||
if width <= 0 or height <= 0 or (p[:, 0] < 1).any() or (p[:, 0] >= width-1).any() or (
|
||||
p[:, 1] < 1).any() or (p[:, 1] >= height-1).any():
|
||||
return 'Tag 角点超出画面边界'
|
||||
edges = np.roll(p, -1, axis=0) - p
|
||||
if np.linalg.norm(edges, axis=1).min() < settings.min_edge_px:
|
||||
return 'Tag 像素尺寸不足'
|
||||
following = np.roll(edges, -1, axis=0)
|
||||
cross = edges[:, 0]*following[:, 1] - edges[:, 1]*following[:, 0]
|
||||
if not ((cross > 0).all() or (cross < 0).all()):
|
||||
return 'Tag 四边形无效'
|
||||
return None
|
||||
|
||||
|
||||
class StableWindow:
|
||||
def __init__(self, count, minimum_seconds=0.0):
|
||||
self.values = deque()
|
||||
self.last_stamp = -1
|
||||
self.count = count
|
||||
self.duration_ns = round(minimum_seconds * 1e9)
|
||||
self.last_quality = None
|
||||
|
||||
def clear(self):
|
||||
self.values.clear()
|
||||
self.last_quality = None
|
||||
|
||||
def snapshot(self):
|
||||
"""Small timestamped corner arrays for diagnosing rejected windows offline."""
|
||||
return [{'stamp_ns': value.stamp_ns, 'corners': np.asarray(value.corners).tolist()}
|
||||
for value in self.values]
|
||||
|
||||
def add(self, observation):
|
||||
if observation.stamp_ns <= self.last_stamp:
|
||||
return False
|
||||
self.last_stamp = observation.stamp_ns
|
||||
self.values.append(observation)
|
||||
# Keep the shortest suffix satisfying both frame count and elapsed time.
|
||||
# This works at different camera rates without assuming a fixed FPS.
|
||||
while (len(self.values) > self.count and
|
||||
observation.stamp_ns - self.values[1].stamp_ns >= self.duration_ns):
|
||||
self.values.popleft()
|
||||
return True
|
||||
|
||||
def summary(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
if (len(self.values) < self.count or
|
||||
self.values[-1].stamp_ns - self.values[0].stamp_ns < self.duration_ns):
|
||||
return None
|
||||
points = np.asarray([v.corners for v in self.values])
|
||||
motion = temporal_motion(points, [v.stamp_ns for v in self.values], drift_sigma)
|
||||
# Preserve raw spread for diagnostics and historical comparisons. The
|
||||
# current endpoint detector evaluates position and raw-frame noise separately.
|
||||
self.last_quality = {'noise': noise_sigma(points), **motion,
|
||||
'noise_limit_px': noise_limit, 'drift_limit_px': drift_limit,
|
||||
'drift_sigma': drift_sigma, 'sampling_method': SAMPLING_METHOD,
|
||||
'first_stamp_ns': self.values[0].stamp_ns,
|
||||
'last_stamp_ns': self.values[-1].stamp_ns, 'frames': len(points)}
|
||||
if motion['drift_px'] > drift_limit or motion['jitter_px'] > noise_limit:
|
||||
return None
|
||||
return {'corners': robust_position(points).tolist(), **self.last_quality}
|
||||
|
||||
def problem(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
"""Explain the current window without using a previous window's metrics."""
|
||||
if self.summary(noise_limit, drift_limit, drift_sigma) is not None:
|
||||
return None
|
||||
frames = len(self.values)
|
||||
span_ns = self.values[-1].stamp_ns - self.values[0].stamp_ns if frames else 0
|
||||
if frames < self.count:
|
||||
return {'reason': f'有效新图像不足:{frames}/{self.count} 帧', 'frames': frames}
|
||||
if span_ns < self.duration_ns:
|
||||
span = span_ns / 1e9
|
||||
return {'reason': f'稳定观察时长不足:{span:.3f}/{self.duration_ns / 1e9:.3f} 秒',
|
||||
'frames': frames, 'span_seconds': span}
|
||||
quality = self.last_quality
|
||||
details = []
|
||||
if quality['jitter_px'] > noise_limit:
|
||||
details.append(f'随机抖动 {quality["jitter_px"]:.3f} > {noise_limit:.3f} 像素')
|
||||
if quality['drift_px'] > drift_limit:
|
||||
details.append(f'持续漂移 {quality["drift_px"]:.3f} > {drift_limit:.3f} 像素'
|
||||
f'(趋势 {quality["trend_px"]:.3f},抖动裕量 {quality["trend_allowance_px"]:.3f})')
|
||||
return {'reason': ';'.join(details), **quality}
|
||||
@@ -0,0 +1,44 @@
|
||||
"""Establish a quiet endpoint before distinguishing command motion from jitter."""
|
||||
import itertools
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .observations import StableWindow, rms_delta, robust_position
|
||||
|
||||
|
||||
def block_shift(points):
|
||||
"""Compare disjoint early/middle/late positions, without cancelling drift."""
|
||||
positions = [robust_position(block) for block in np.array_split(points, 3)]
|
||||
return max(rms_delta(a, b) for a, b in itertools.combinations(positions, 2))
|
||||
|
||||
|
||||
def jitter_radius(points):
|
||||
"""Observed frame excursions; no independent-noise assumption."""
|
||||
values = np.asarray(points, dtype=float)
|
||||
radii = np.sqrt(np.mean(np.sum((values - robust_position(values)) ** 2, axis=-1), axis=-1))
|
||||
return float(np.quantile(radii, .99))
|
||||
|
||||
|
||||
class ReferenceWindow(StableWindow):
|
||||
def __init__(self, settings):
|
||||
super().__init__(3 * settings.stable_frames, settings.reference_seconds)
|
||||
# Reserve half of the movement threshold for reference uncertainty.
|
||||
# This concerns block positions, not per-frame jitter or drift velocity.
|
||||
self.shift_limit = settings.motion_floor_px / 2
|
||||
|
||||
def summary(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
summary = super().summary(noise_limit, drift_limit, drift_sigma)
|
||||
if summary is None:
|
||||
return None
|
||||
shift = block_shift(np.asarray([v.corners for v in self.values]))
|
||||
self.last_quality.update(reference_shift_px=shift, reference_shift_limit_px=self.shift_limit)
|
||||
if shift > self.shift_limit:
|
||||
return None
|
||||
return {**summary, **self.last_quality}
|
||||
|
||||
def problem(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
problem = super().problem(noise_limit, drift_limit, drift_sigma)
|
||||
if problem and self.last_quality and self.last_quality.get('reference_shift_px', 0) > self.shift_limit:
|
||||
problem['reason'] = (f'端点参考尚未稳定:前、中、后三段位置差 '
|
||||
f'{self.last_quality["reference_shift_px"]:.3f} > {self.shift_limit:.3f} 像素')
|
||||
return problem
|
||||
@@ -0,0 +1,143 @@
|
||||
"""窗口打开时拉起三台相机、图像校正和 AprilTag。界面本身不是这个进程。"""
|
||||
import os
|
||||
from pathlib import Path
|
||||
import subprocess
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
NAMESPACE = '/linkerhand_range_calibration'
|
||||
|
||||
|
||||
def build_description(station):
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import ComposableNodeContainer, Node
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
|
||||
actions = []
|
||||
components = []
|
||||
camera = station['camera']
|
||||
for view, device in station['cameras'].items():
|
||||
intrinsics = Path(device['intrinsics']).expanduser().resolve()
|
||||
if not intrinsics.is_file():
|
||||
raise ValueError(f'{view} 内参文件不存在: {intrinsics}')
|
||||
prefix = f'{NAMESPACE}/{view}'
|
||||
actions.append(Node(
|
||||
package='camera_parameters',
|
||||
executable='hikrobot_camera_node',
|
||||
name='hikrobot_camera',
|
||||
namespace=prefix + '/camera',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'serial_number': device['serial'],
|
||||
'camera_name': 'hikrobot_' + device['serial'],
|
||||
'frame_id': f'range_{view}_optical_frame',
|
||||
'image_width': camera['width'],
|
||||
'image_height': camera['height'],
|
||||
'camera_info_url': intrinsics.as_uri(),
|
||||
'frame_rate': float(camera['frame_rate']),
|
||||
'exposure_time_us': float(camera['exposure_time_us']),
|
||||
'gain_db': 0.0,
|
||||
'auto_exposure': False,
|
||||
}],
|
||||
))
|
||||
components.extend([
|
||||
ComposableNode(
|
||||
package='image_proc',
|
||||
plugin='image_proc::RectifyNode',
|
||||
name='rectify',
|
||||
namespace=prefix + '/camera',
|
||||
remappings=[
|
||||
('image', prefix + '/camera/image_raw'),
|
||||
('camera_info', prefix + '/camera/camera_info'),
|
||||
('image_rect', prefix + '/camera/image_rect'),
|
||||
],
|
||||
parameters=[{'queue_size': 1}],
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
ComposableNode(
|
||||
package='apriltag_ros',
|
||||
plugin='AprilTagNode',
|
||||
name='apriltag',
|
||||
namespace=prefix + '/apriltag',
|
||||
remappings=[
|
||||
('image_rect', prefix + '/camera/image_rect'),
|
||||
('camera_info', prefix + '/camera/camera_info'),
|
||||
],
|
||||
parameters=[{
|
||||
'family': camera['family'],
|
||||
'size': float(camera['tag_size_m']),
|
||||
'qos_profile': 'sensor_data',
|
||||
'max_hamming': 0,
|
||||
'detector.threads': 2,
|
||||
'detector.decimate': 1.0,
|
||||
'detector.refine': True,
|
||||
}],
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
])
|
||||
actions.append(ComposableNodeContainer(
|
||||
name='range_vision',
|
||||
namespace=NAMESPACE,
|
||||
package='rclcpp_components',
|
||||
executable='component_container_mt',
|
||||
composable_node_descriptions=components,
|
||||
output='screen',
|
||||
))
|
||||
return LaunchDescription(actions)
|
||||
|
||||
|
||||
def start_vision() -> subprocess.Popen:
|
||||
"""在独立进程里拉起相机和 AprilTag。调用方关掉窗口时结束这一组进程。"""
|
||||
from calibration.profiles import _workspace_root
|
||||
|
||||
workspace = _workspace_root()
|
||||
config_path = workspace / 'src' / 'camera_parameters' / 'config' / 'cameras.yaml'
|
||||
if not config_path.is_file():
|
||||
raise FileNotFoundError(f'找不到相机配置: {config_path}')
|
||||
document = yaml.safe_load(config_path.read_text(encoding='utf-8'))
|
||||
setup = Path(str(document.get('setup') if isinstance(document, dict) else ''))
|
||||
if not setup.is_file():
|
||||
raise FileNotFoundError(f'找不到相机驱动环境: {setup}')
|
||||
root = Path(__file__).resolve().parents[1]
|
||||
log_dir = workspace / 'output_range_calibration' / 'cameras'
|
||||
log_dir.mkdir(parents=True, exist_ok=True)
|
||||
log_file = (log_dir / 'vision.log').open('w', encoding='utf-8')
|
||||
script = (
|
||||
'set +u\n'
|
||||
'_ament="$AMENT_PREFIX_PATH"\n'
|
||||
'_cmake="$CMAKE_PREFIX_PATH"\n'
|
||||
'_path="$PATH"\n'
|
||||
'_py="$PYTHONPATH"\n'
|
||||
'_ld="$LD_LIBRARY_PATH"\n'
|
||||
f'source "{setup}"\n'
|
||||
'export AMENT_PREFIX_PATH="${_ament:+$_ament:}$AMENT_PREFIX_PATH"\n'
|
||||
'export CMAKE_PREFIX_PATH="${_cmake:+$_cmake:}$CMAKE_PREFIX_PATH"\n'
|
||||
'export PATH="${_path:+$_path:}$PATH"\n'
|
||||
'export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:+$LD_LIBRARY_PATH:}$_ld"\n'
|
||||
f'export PYTHONPATH="{root}${{_py:+:$_py}}${{PYTHONPATH:+:$PYTHONPATH}}"\n'
|
||||
'exec python3 -m calibration.vision_stack\n'
|
||||
)
|
||||
process = subprocess.Popen(
|
||||
['bash', '-lc', script],
|
||||
start_new_session=True,
|
||||
stdout=log_file,
|
||||
stderr=subprocess.STDOUT,
|
||||
env=os.environ.copy(),
|
||||
)
|
||||
process._camera_log = log_file
|
||||
return process
|
||||
|
||||
|
||||
def main():
|
||||
from launch import LaunchService
|
||||
|
||||
from calibration.profiles import load_station
|
||||
|
||||
service = LaunchService()
|
||||
service.include_launch_description(build_description(load_station(None)))
|
||||
return service.run()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main() or 0)
|
||||
@@ -0,0 +1,36 @@
|
||||
# 序列号和内外参文件在加载时从仓库 output_camera/ 填入,这里只保留视角名。
|
||||
cameras:
|
||||
front: {}
|
||||
side: {}
|
||||
top: {}
|
||||
camera:
|
||||
width: 1624
|
||||
height: 1240
|
||||
family: 36h11
|
||||
# 帧率、曝光和贴纸边长在加载时从 src/camera_parameters/config/cameras.yaml 填入。
|
||||
sdk: {comm_type: libcanbus, canfd_device: 0}
|
||||
motion: {speed: 200, torque: 200} # 设备速度和力矩;手动示教直接跟随滑块目标。
|
||||
output_root: output_range_calibration
|
||||
teaching_root: '~/.ros/linkerhand_range_calibration'
|
||||
scan:
|
||||
endpoint_repetitions: 1 # 每轮从0向上、从255向下各寻找一次;可设2进行复核。
|
||||
rate: 20 # 仅用于自动标定的准备姿态、避让及扫描轨迹,不限制手动示教。
|
||||
settle_seconds: 0.3
|
||||
stable_frames: 8
|
||||
stable_seconds: 0.5 # 新图像必须覆盖连续静止时间,不能只凭几帧局部平稳。
|
||||
reference_seconds: 1.5 # 每端开始前额外核对前、中、后三段参考位置一致。
|
||||
motion_frames: 2 # 瞬时运动须超过参考和当前点的抖动包络;不看方向。
|
||||
point_timeout: 6 # 普通逐点采样的最长等待;稳定后立即推进。
|
||||
reference_timeout: 30 # 到达端点后允许更长时间稳定;达标立即开始,不固定等满30秒。
|
||||
freshness: 0.5
|
||||
feedback_tolerance: 2
|
||||
repeat_tolerance: 2
|
||||
motion_floor_px: 0.5
|
||||
noise_multiplier: 5 # 仅用于旧日志复算;不再用单次原始噪声抬高稳定位置运动门槛。
|
||||
stability_noise_px: 0.5 # 去除时间趋势后,允许的随机角点抖动(像素)。
|
||||
stability_drift_px: 0.1 # 扣除抖动裕量后,允许的窗口累计漂移(像素)。
|
||||
stability_drift_sigma: 3 # 时间趋势估计的随机抖动裕量倍数。
|
||||
max_hamming: 0
|
||||
min_margin: 30
|
||||
min_edge_px: 30
|
||||
angle_motion_rad: 0.005 # 相对端点平台,角度变化超过该值才算离开行程死区。
|
||||
@@ -0,0 +1 @@
|
||||
"""Qt user interface."""
|
||||
@@ -0,0 +1,48 @@
|
||||
"""启动行程标定界面:python -m gui"""
|
||||
|
||||
import signal
|
||||
import sys
|
||||
from threading import Thread
|
||||
|
||||
|
||||
def main(argv=None):
|
||||
import argparse
|
||||
|
||||
import rclpy
|
||||
from PyQt5.QtWidgets import QApplication
|
||||
from rclpy.executors import ExternalShutdownException, SingleThreadedExecutor
|
||||
|
||||
from gui.window import CalibrationWindow
|
||||
|
||||
parser = argparse.ArgumentParser(description='LinkerHand 行程标定')
|
||||
parser.add_argument('--demo', action='store_true')
|
||||
parser.add_argument('--station')
|
||||
args, ros_args = parser.parse_known_args(argv)
|
||||
rclpy.init(args=ros_args)
|
||||
executor = SingleThreadedExecutor()
|
||||
app = QApplication.instance() or QApplication([sys.argv[0]])
|
||||
window = CalibrationWindow(executor, demo=args.demo, station_path=args.station)
|
||||
|
||||
def spin():
|
||||
try:
|
||||
executor.spin()
|
||||
except ExternalShutdownException:
|
||||
pass
|
||||
|
||||
thread = Thread(target=spin, daemon=True)
|
||||
thread.start()
|
||||
window.show()
|
||||
signal.signal(signal.SIGINT, lambda *unused: app.quit())
|
||||
signal.signal(signal.SIGTERM, lambda *unused: app.quit())
|
||||
try:
|
||||
return app.exec_()
|
||||
finally:
|
||||
window.shutdown()
|
||||
executor.shutdown()
|
||||
thread.join(timeout=3)
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main() or 0)
|
||||
@@ -0,0 +1,51 @@
|
||||
"""Joint slider with a precise numeric editor and silent display updates."""
|
||||
from decimal import Decimal
|
||||
|
||||
from PyQt5.QtCore import QSignalBlocker, Qt, pyqtSignal
|
||||
from PyQt5.QtWidgets import QDoubleSpinBox, QHBoxLayout, QSlider, QWidget
|
||||
|
||||
|
||||
class JointControl(QWidget):
|
||||
target_changed = pyqtSignal(float)
|
||||
|
||||
def __init__(self, joint, parent=None):
|
||||
super().__init__(parent)
|
||||
self.joint = joint
|
||||
self.slider = QSlider(Qt.Horizontal)
|
||||
self.slider.setRange(0, round((joint.maximum - joint.minimum) / joint.resolution))
|
||||
self.slider.setPageStep(max(1, self.slider.maximum() // 25))
|
||||
self.slider.setMinimumWidth(160)
|
||||
self.slider.setAccessibleName(joint.name + ' 目标滑块')
|
||||
self.editor = QDoubleSpinBox()
|
||||
self.editor.setDecimals(max(0, min(8, -Decimal(str(joint.resolution)).as_tuple().exponent)))
|
||||
self.editor.setRange(joint.minimum, joint.maximum)
|
||||
self.editor.setSingleStep(joint.resolution)
|
||||
self.editor.setKeyboardTracking(False)
|
||||
self.editor.setFixedWidth(80)
|
||||
self.editor.setAccessibleName(joint.name + ' 精确目标')
|
||||
layout = QHBoxLayout(self)
|
||||
layout.setContentsMargins(6, 0, 6, 0)
|
||||
layout.addWidget(self.slider, 1)
|
||||
layout.addWidget(self.editor)
|
||||
self.slider.valueChanged.connect(self._slider_changed)
|
||||
self.editor.valueChanged.connect(self._editor_changed)
|
||||
self.set_target(joint.minimum)
|
||||
|
||||
def set_target(self, value):
|
||||
"""Refresh from runtime without issuing a motion request."""
|
||||
tick = round((value - self.joint.minimum) / self.joint.resolution)
|
||||
tick = max(self.slider.minimum(), min(self.slider.maximum(), tick))
|
||||
slider_block = QSignalBlocker(self.slider)
|
||||
editor_block = QSignalBlocker(self.editor)
|
||||
self.slider.setValue(tick)
|
||||
self.editor.setValue(self.joint.minimum + tick * self.joint.resolution)
|
||||
del slider_block, editor_block
|
||||
|
||||
def _slider_changed(self, tick):
|
||||
value = self.joint.minimum + tick * self.joint.resolution
|
||||
self.set_target(value)
|
||||
self.target_changed.emit(value)
|
||||
|
||||
def _editor_changed(self, value):
|
||||
self.set_target(value)
|
||||
self.target_changed.emit(self.editor.value())
|
||||
@@ -0,0 +1,475 @@
|
||||
"""Thin Qt view: queue commands and render immutable runtime snapshots."""
|
||||
import os
|
||||
import signal
|
||||
import threading
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from PyQt5.QtCore import Qt, QTimer, pyqtSignal
|
||||
from PyQt5.QtGui import QImage, QPixmap
|
||||
from PyQt5.QtWidgets import (
|
||||
QComboBox, QFileDialog, QHBoxLayout, QLabel, QLineEdit, QMainWindow,
|
||||
QMessageBox, QProgressBar, QPushButton, QTableWidget, QTableWidgetItem,
|
||||
QVBoxLayout, QWidget, QHeaderView,
|
||||
)
|
||||
|
||||
from calibration.offset import load_ranges_document
|
||||
from calibration.profiles import available_models, load_profile, load_station
|
||||
from calibration.runtime import CalibrationRuntime
|
||||
from gui.joint_control import JointControl
|
||||
from hands import start_sdk
|
||||
|
||||
|
||||
STATES = {'IDLE': '待操作', 'PREPARING': '准备姿态 / 静止采样', 'SCANNING': '扫描中',
|
||||
'RESTORING': '测后恢复基础姿态', 'TEACHING': '示教运动中', 'PAUSED': '已暂停',
|
||||
'CANCELLED': '已取消', 'COMPLETED': '已完成'}
|
||||
|
||||
|
||||
class CalibrationWindow(QMainWindow):
|
||||
connect_finished = pyqtSignal(bool, str)
|
||||
|
||||
def __init__(self, executor, demo=False, station_path=None):
|
||||
super().__init__()
|
||||
self.executor, self.demo, self.station_path = executor, demo, station_path
|
||||
self.runtime, self.profile = None, None
|
||||
self.sdk_process = None
|
||||
self.station = load_station(station_path)
|
||||
self.last_frames, self.initialized_uid = {}, ''
|
||||
self.display_epoch, self.was_manual_ready = None, False
|
||||
self.offset_path_locked = False
|
||||
self.setWindowTitle('手部行程标定' + (' — 模拟设备' if demo else ''))
|
||||
self.resize(1280, 930)
|
||||
root = QWidget(); self.setCentralWidget(root)
|
||||
layout = QVBoxLayout(root)
|
||||
bar = QHBoxLayout()
|
||||
self.model_box = QComboBox()
|
||||
self.side_box = QComboBox()
|
||||
self.side_box.addItems(['右手', '左手'])
|
||||
self.connect_button = QPushButton('连接')
|
||||
self.start_button = QPushButton('开始标定')
|
||||
self.start_button.setEnabled(False)
|
||||
bar.addWidget(QLabel('型号'))
|
||||
bar.addWidget(self.model_box)
|
||||
bar.addWidget(QLabel('左右'))
|
||||
bar.addWidget(self.side_box)
|
||||
bar.addWidget(self.connect_button)
|
||||
bar.addWidget(self.start_button)
|
||||
bar.addStretch(1)
|
||||
layout.addLayout(bar)
|
||||
self.connection_status = QLabel('尚未连接。先选择型号和左右手,再点连接。')
|
||||
self.connection_status.setWordWrap(True)
|
||||
layout.addWidget(self.connection_status)
|
||||
self.identity, self.status, self.message, self.teaching_status = QLabel(), QLabel(), QLabel(), QLabel()
|
||||
self.identity.setWordWrap(True)
|
||||
self.status.setWordWrap(True)
|
||||
self.message.setWordWrap(True)
|
||||
self.message.setTextInteractionFlags(Qt.TextSelectableByMouse)
|
||||
for label in (self.identity, self.status, self.teaching_status):
|
||||
layout.addWidget(label)
|
||||
previews = QHBoxLayout(); self.previews_by_view = {}; self.tag_labels = {}
|
||||
names = {'front': '正面', 'side': '侧面', 'top': '顶部'}
|
||||
for view in self.station['cameras']:
|
||||
column = QVBoxLayout(); column.addWidget(QLabel(names.get(view, view)))
|
||||
preview = QLabel('等待图像'); preview.setAlignment(Qt.AlignCenter)
|
||||
preview.setMinimumSize(240, 165); preview.setMaximumHeight(230)
|
||||
preview.setStyleSheet('background:#18222c; color:#d5e1ea; border-radius:4px')
|
||||
column.addWidget(preview); previews.addLayout(column)
|
||||
self.previews_by_view[view] = preview
|
||||
tag_status = QLabel('等待 Tag 检测')
|
||||
tag_status.setWordWrap(True)
|
||||
column.addWidget(tag_status)
|
||||
self.tag_labels[view] = tag_status
|
||||
layout.addLayout(previews)
|
||||
controls = QHBoxLayout()
|
||||
self.task = QComboBox()
|
||||
controls.addWidget(self.task)
|
||||
self.start_one = self.button('试标定 / 重测所选任务', lambda: self._submit('start', tasks=[self.task.currentData()]), controls)
|
||||
self.pause = self.button('暂停', lambda: self._submit('pause'), controls)
|
||||
self.resume = self.button('继续(重做当前任务)', lambda: self._submit('resume'), controls)
|
||||
self.cancel = self.button('取消 / 保持目标', lambda: self._submit('cancel'), controls)
|
||||
layout.addLayout(controls)
|
||||
self.progress = QProgressBar(); layout.addWidget(self.progress)
|
||||
teach_controls = QHBoxLayout()
|
||||
self.fill = self.button('同步当前目标', self.sync_targets, teach_controls)
|
||||
self.save_kind = QComboBox()
|
||||
teach_controls.addWidget(self.save_kind)
|
||||
self.save = self.button('保存当前已到达姿态', self.save_teaching, teach_controls)
|
||||
layout.addLayout(teach_controls)
|
||||
hint = QLabel('拖动滑块直接控制关节;数值框支持精调,输入后按回车确认。等待运动完成、反馈稳定后保存姿态。')
|
||||
hint.setWordWrap(True)
|
||||
layout.addWidget(hint)
|
||||
self.joints_table = QTableWidget(0, 7)
|
||||
self.joints_table.setHorizontalHeaderLabels(
|
||||
['关节', 'SDK 下标', '示教目标(拖动即运动)', '实际反馈', '已记录', '指令曲线', '状态 / 原因'])
|
||||
self.joint_controls = []
|
||||
self.joints_table.setEditTriggers(QTableWidget.NoEditTriggers)
|
||||
self.joints_table.setSelectionBehavior(QTableWidget.SelectRows)
|
||||
self.joints_table.setWordWrap(False)
|
||||
self.joints_table.verticalHeader().setVisible(False)
|
||||
header = self.joints_table.horizontalHeader()
|
||||
header.setSectionResizeMode(QHeaderView.ResizeToContents)
|
||||
header.setSectionResizeMode(2, QHeaderView.Stretch)
|
||||
header.setSectionResizeMode(6, QHeaderView.Interactive)
|
||||
self.joints_table.setColumnWidth(6, max(220, self.fontMetrics().horizontalAdvance('状') * 18))
|
||||
layout.addWidget(self.joints_table, 1)
|
||||
layout.addWidget(self.message)
|
||||
self.output = QLabel(); self.output.setWordWrap(True)
|
||||
self.output.setTextInteractionFlags(Qt.TextSelectableByMouse); layout.addWidget(self.output)
|
||||
offset_row = QHBoxLayout()
|
||||
offset_row.addWidget(QLabel('行程结果 JSON:'))
|
||||
self.offset_path = QLineEdit()
|
||||
self.offset_path.setPlaceholderText('标定完成后自动填入,也可手动选择结果 JSON')
|
||||
self.offset_path.textEdited.connect(self._offset_path_edited)
|
||||
offset_row.addWidget(self.offset_path, 1)
|
||||
self.browse_offset = self.button('选择…', self.browse_offset_file, offset_row)
|
||||
self.apply_offset = self.button('下发行程偏移到设备', self.apply_offset_file, offset_row)
|
||||
self.apply_offset.setToolTip(
|
||||
'只接受旧版带 min/max 的结果。新的角度曲线由嵌入式烧录 commands,不能转成 set_offset')
|
||||
self.clear_offset = self.button('清除行程偏移', self.clear_offset_on_device, offset_row)
|
||||
self.clear_offset.setToolTip('向 SDK 发送还原偏移:全部关节 [0, 255]')
|
||||
layout.addLayout(offset_row)
|
||||
self.connect_finished.connect(self._on_connect_finished)
|
||||
self.connect_button.clicked.connect(self._connect)
|
||||
self.start_button.clicked.connect(lambda: self._submit('start'))
|
||||
self.model_box.currentTextChanged.connect(self._selection_changed)
|
||||
self.side_box.currentTextChanged.connect(self._selection_changed)
|
||||
self._reload_models()
|
||||
self._set_actions_enabled(False)
|
||||
self._vision_process = None
|
||||
self._camera_note = ''
|
||||
self._start_vision()
|
||||
self.timer = QTimer(self); self.timer.timeout.connect(self.refresh); self.timer.start(100)
|
||||
|
||||
def _start_vision(self):
|
||||
if self.demo:
|
||||
return
|
||||
try:
|
||||
from calibration.vision_stack import start_vision
|
||||
self._vision_process = start_vision()
|
||||
self._camera_note = '相机和 AprilTag 已拉起。'
|
||||
except Exception as error:
|
||||
self._camera_note = f'相机没有拉起:{error}。'
|
||||
self.connection_status.setText(
|
||||
self._camera_note + '尚未连接。先选择型号和左右手,再点连接。'
|
||||
)
|
||||
|
||||
def _selected_side(self):
|
||||
return 'right' if self.side_box.currentText() == '右手' else 'left'
|
||||
|
||||
def _reload_models(self):
|
||||
self.model_box.blockSignals(True)
|
||||
self.model_box.clear()
|
||||
models = available_models()
|
||||
self.model_box.addItems(models or ['(没有型号)'])
|
||||
self.model_box.blockSignals(False)
|
||||
self._selection_changed()
|
||||
|
||||
def _selection_changed(self):
|
||||
self._disconnect()
|
||||
note = getattr(self, '_camera_note', '')
|
||||
self.connection_status.setText(note + '尚未连接。先选择型号和左右手,再点连接。')
|
||||
self._fill_profile()
|
||||
|
||||
def _fill_profile(self):
|
||||
self.task.clear()
|
||||
self.save_kind.clear()
|
||||
self.joints_table.setRowCount(0)
|
||||
self.joint_controls.clear()
|
||||
model = self.model_box.currentText()
|
||||
if not model or model.startswith('('):
|
||||
self.profile = None
|
||||
return
|
||||
try:
|
||||
self.profile = load_profile(f'{model}_{self._selected_side()}')
|
||||
except (ValueError, OSError) as error:
|
||||
self.profile = None
|
||||
self.connection_status.setText(str(error))
|
||||
return
|
||||
for task in self.profile.tasks:
|
||||
self.task.addItem('四指侧摆(分阶段)' if task.name == 'four_finger_roll' else task.name, task.name)
|
||||
self.save_kind.addItem('全手基础姿态', ('baseline', None))
|
||||
for group, clearance in self.profile.teaching_clearances.items():
|
||||
self.save_kind.addItem('避让姿态:' + group, ('clearances', group))
|
||||
self.save_kind.setItemData(self.save_kind.count() - 1,
|
||||
'仅保存这些关节:' + '、'.join(clearance.joints), Qt.ToolTipRole)
|
||||
self.save_kind.addItem('当前任务准备姿态覆盖', ('overrides', None))
|
||||
self.joints_table.setRowCount(len(self.profile.joints))
|
||||
for row, joint in enumerate(self.profile.joints):
|
||||
for col, value in enumerate((joint.name, str(joint.index), None, '—', '—', '—', '未完成')):
|
||||
if value is not None:
|
||||
self.joints_table.setItem(row, col, QTableWidgetItem(value))
|
||||
self.joints_table.item(row, 0).setToolTip(joint.sdk_name)
|
||||
control = JointControl(joint)
|
||||
control.setEnabled(False)
|
||||
control.target_changed.connect(lambda value, name=joint.name: self.jog(name, value))
|
||||
self.joints_table.setCellWidget(row, 2, control)
|
||||
self.joint_controls.append(control)
|
||||
self.joints_table.setRowHeight(row, max(34, control.sizeHint().height() + 4))
|
||||
|
||||
def _submit(self, action, **data):
|
||||
if self.runtime is not None:
|
||||
self.runtime.submit(action, **data)
|
||||
|
||||
def _set_actions_enabled(self, enabled):
|
||||
for control in (self.start_button, self.start_one, self.pause, self.resume, self.cancel,
|
||||
self.fill, self.task, self.save_kind, self.save, self.apply_offset, self.clear_offset):
|
||||
control.setEnabled(enabled)
|
||||
for control in self.joint_controls:
|
||||
control.setEnabled(enabled)
|
||||
|
||||
def _connect(self):
|
||||
if self.profile is None:
|
||||
self.connection_status.setText('没有可用的型号配置。')
|
||||
return
|
||||
self._disconnect()
|
||||
self._fill_profile()
|
||||
profile = self.profile
|
||||
if profile is None:
|
||||
return
|
||||
self.connect_button.setEnabled(False)
|
||||
self.connection_status.setText('正在连接驱动…')
|
||||
|
||||
def work():
|
||||
try:
|
||||
model = self.model_box.currentText()
|
||||
process = None if self.demo else start_sdk(model, profile, self.station)
|
||||
self._pending_connection = (profile, process)
|
||||
self.connect_finished.emit(True, '驱动已拉起。等待真机身份回报后即可开始标定。')
|
||||
except Exception as error:
|
||||
self.connect_finished.emit(False, str(error))
|
||||
|
||||
threading.Thread(target=work, daemon=True).start()
|
||||
|
||||
def _on_connect_finished(self, ok, message):
|
||||
self.connect_button.setEnabled(True)
|
||||
self.connection_status.setText(message)
|
||||
if not ok:
|
||||
self._set_actions_enabled(False)
|
||||
QMessageBox.warning(self, '连接失败', message)
|
||||
return
|
||||
profile, process = self._pending_connection
|
||||
self.profile = profile
|
||||
self.sdk_process = process
|
||||
runtime = CalibrationRuntime(profile, self.station, demo=self.demo)
|
||||
self.executor.add_node(runtime)
|
||||
self.runtime = runtime
|
||||
|
||||
def _disconnect(self):
|
||||
runtime = self.runtime
|
||||
self.runtime = None
|
||||
if runtime is not None:
|
||||
self.executor.remove_node(runtime)
|
||||
runtime.close_session()
|
||||
runtime.destroy_node()
|
||||
process = self.sdk_process
|
||||
self.sdk_process = None
|
||||
if process is not None and process.poll() is None:
|
||||
if os.name == 'nt':
|
||||
process.terminate()
|
||||
else:
|
||||
os.killpg(process.pid, signal.SIGTERM)
|
||||
self._set_actions_enabled(False)
|
||||
|
||||
def shutdown(self):
|
||||
self.timer.stop()
|
||||
self._disconnect()
|
||||
process = getattr(self, '_vision_process', None)
|
||||
self._vision_process = None
|
||||
if process is not None and process.poll() is None:
|
||||
if os.name == 'nt':
|
||||
process.terminate()
|
||||
else:
|
||||
os.killpg(process.pid, signal.SIGTERM)
|
||||
|
||||
def button(self, text, callback, layout):
|
||||
button = QPushButton(text); button.clicked.connect(callback); layout.addWidget(button)
|
||||
return button
|
||||
|
||||
def _offset_path_edited(self, _text):
|
||||
self.offset_path_locked = True
|
||||
|
||||
def browse_offset_file(self):
|
||||
start = self.offset_path.text().strip() or str(self.runtime.station.get('output_root', '.'))
|
||||
path, _ = QFileDialog.getOpenFileName(
|
||||
self, '选择行程标定结果 JSON', start, 'JSON (*.json);;All (*)')
|
||||
if path:
|
||||
self.offset_path_locked = True
|
||||
self.offset_path.setText(path)
|
||||
|
||||
def apply_offset_file(self):
|
||||
path = self.offset_path.text().strip()
|
||||
if not path:
|
||||
QMessageBox.warning(self, '下发行程偏移', '请先选择或填写结果 JSON 路径')
|
||||
return
|
||||
snap = self.runtime.snapshot() or {}
|
||||
current_uid = snap.get('uid') or ''
|
||||
if not current_uid:
|
||||
QMessageBox.warning(self, '下发行程偏移', '当前尚未识别到设备 UID,请等待 SDK 就绪后再试')
|
||||
return
|
||||
try:
|
||||
document = load_ranges_document(path)
|
||||
except ValueError as error:
|
||||
QMessageBox.warning(self, '下发行程偏移', str(error))
|
||||
return
|
||||
if document.get('schema_version') == 2:
|
||||
QMessageBox.warning(
|
||||
self, '下发行程偏移',
|
||||
'这是角度曲线结果,不能转换成 set_offset。\n嵌入式只烧录 commands 数组。')
|
||||
return
|
||||
file_uid = document.get('device_uid') or ''
|
||||
if not file_uid:
|
||||
QMessageBox.warning(self, '下发行程偏移', '结果 JSON 缺少 device_uid 字段')
|
||||
return
|
||||
if file_uid != current_uid:
|
||||
QMessageBox.warning(
|
||||
self, '设备 UID 不一致',
|
||||
f'当前连接设备与结果文件不是同一台,已取消下发。\n\n'
|
||||
f'当前设备:{current_uid}\n'
|
||||
f'JSON 内:{file_uid}\n\n'
|
||||
f'文件:{path}')
|
||||
return
|
||||
reply = QMessageBox.question(
|
||||
self, '确认下发行程偏移',
|
||||
f'将把以下文件转换为 set_offset 并写入当前 {self.profile.side} 手设备:\n\n{path}\n\n'
|
||||
f'设备 UID:{current_uid}(与 JSON 一致)\n'
|
||||
'请确认文件正确,且当前未在标定扫描中。',
|
||||
QMessageBox.Yes | QMessageBox.No, QMessageBox.No)
|
||||
if reply != QMessageBox.Yes:
|
||||
return
|
||||
self.runtime.submit('apply_offset', path=path)
|
||||
|
||||
def clear_offset_on_device(self):
|
||||
snap = self.runtime.snapshot() or {}
|
||||
current_uid = snap.get('uid') or ''
|
||||
if not current_uid:
|
||||
QMessageBox.warning(self, '清除行程偏移', '当前尚未识别到设备 UID,请等待 SDK 就绪后再试')
|
||||
return
|
||||
reply = QMessageBox.question(
|
||||
self, '确认清除行程偏移',
|
||||
f'将向当前 {self.profile.side} 手设备发送还原偏移(全部关节 '
|
||||
f'[{int(self.profile.joints[0].minimum)}, {int(self.profile.joints[0].maximum)}])。\n\n'
|
||||
f'设备 UID:{current_uid}\n'
|
||||
'请确认当前未在标定扫描中。',
|
||||
QMessageBox.Yes | QMessageBox.No, QMessageBox.No)
|
||||
if reply != QMessageBox.Yes:
|
||||
return
|
||||
self.runtime.submit('clear_offset')
|
||||
|
||||
def sync_targets(self):
|
||||
if self.runtime is None:
|
||||
return
|
||||
values = self.runtime.snapshot().get('manual_target')
|
||||
if values:
|
||||
for control, value in zip(self.joint_controls, values):
|
||||
control.set_target(value)
|
||||
|
||||
def jog(self, name, value):
|
||||
if self.runtime is None:
|
||||
return
|
||||
snap = self.runtime.snapshot()
|
||||
if not snap or snap['control_error'] or not snap['uid'] or not snap['manual_allowed']:
|
||||
return
|
||||
self.runtime.submit('jog', targets={name: value}, epoch=snap['manual_epoch'])
|
||||
|
||||
def save_teaching(self):
|
||||
kind, key = self.save_kind.currentData()
|
||||
self.runtime.submit('save', kind=kind, key=self.task.currentData() if kind == 'overrides' else key)
|
||||
|
||||
def refresh(self):
|
||||
if self.runtime is None or self.profile is None:
|
||||
return
|
||||
snap = self.runtime.snapshot()
|
||||
if not snap:
|
||||
return
|
||||
control_error = snap['control_error']
|
||||
self.identity.setText(f'设备:{self.profile.model} / {self.profile.side} / {snap["uid"] or "等待UID"} | '
|
||||
f'{control_error or "SDK 控制连接已就绪"}')
|
||||
restore_label = f'{snap["restoring_joint"]} 恢复基础姿态' if snap['restoring_joint'] else ''
|
||||
short_names = {'index_mcp_roll': '食指', 'middle_mcp_roll': '中指',
|
||||
'ring_mcp_roll': '无名指', 'pinky_mcp_roll': '小指'}
|
||||
commands = snap['scan_commands']
|
||||
command_text = (' / '.join(f'{short_names.get(name, name)}={value:g}' for name, value in commands.items())
|
||||
if len(commands) > 1 else str(next(iter(commands.values()), '—')))
|
||||
stage_text = {
|
||||
'prepare': '准备姿态',
|
||||
'low_anchor': '低端平台',
|
||||
'low_search': '低端寻行程',
|
||||
'high_anchor': '高端平台',
|
||||
'high_search': '高端寻行程',
|
||||
'probe': '二分目标角',
|
||||
'done': '曲线完成',
|
||||
'restore': restore_label,
|
||||
}.get(snap['stage'], '')
|
||||
self.status.setText(f'{STATES.get(snap["state"], snap["state"])} | {snap["task"]} | '
|
||||
f'{stage_text} '
|
||||
f'{ {"up":"从低端向上", "down":"从高端向下"}.get(snap["direction"], "")} '
|
||||
f'指令 {command_text} | '
|
||||
f'任务 {min(snap["task_index"]+1,snap["task_count"])}/{snap["task_count"]}')
|
||||
self.teaching_status.setText('待示教:' + '、'.join(snap['missing']) if snap['missing'] else '所需示教已完成')
|
||||
self.message.setText(snap['reason'] + ('\n' if snap['reason'] else '') + snap['message'])
|
||||
self.output.setText('结果文件:' + (snap['output'] or '开始标定后创建'))
|
||||
if snap['output'] and not self.offset_path_locked:
|
||||
self.offset_path.setText(snap['output'])
|
||||
value, maximum = snap['progress']; self.progress.setRange(0, max(1, maximum)); self.progress.setValue(value)
|
||||
blocked = snap['active'] or snap['state'] == 'PAUSED'
|
||||
ready = not blocked and not control_error
|
||||
for control in (self.fill, self.task, self.save_kind):
|
||||
control.setEnabled(ready)
|
||||
self.start_button.setEnabled(ready and bool(snap['uid']))
|
||||
self.start_one.setEnabled(ready and bool(snap['uid']))
|
||||
self.save.setEnabled(snap['can_save'])
|
||||
self.pause.setEnabled(snap['active']); self.cancel.setEnabled(blocked)
|
||||
self.resume.setEnabled(snap['state'] == 'PAUSED' and bool(snap['task']) and not control_error)
|
||||
offset_ready = ready and bool(snap['uid'])
|
||||
self.browse_offset.setEnabled(True)
|
||||
self.offset_path.setEnabled(True)
|
||||
self.apply_offset.setEnabled(offset_ready)
|
||||
self.clear_offset.setEnabled(offset_ready)
|
||||
manual_ready = (not control_error and bool(snap['uid']) and
|
||||
snap['manual_allowed'])
|
||||
for control in self.joint_controls:
|
||||
control.setEnabled(manual_ready)
|
||||
if (snap['manual_epoch'] != self.display_epoch or snap['uid'] != self.initialized_uid or
|
||||
not manual_ready or not self.was_manual_ready):
|
||||
self.sync_targets()
|
||||
self.display_epoch, self.initialized_uid = snap['manual_epoch'], snap['uid']
|
||||
self.was_manual_ready = manual_ready
|
||||
for row, joint in enumerate(self.profile.joints):
|
||||
if snap['positions']:
|
||||
self.joints_table.item(row, 3).setText(f'{snap["positions"][row]:g}')
|
||||
result = snap['results'][joint.name]
|
||||
curve = result.get('commands') or []
|
||||
filled = [item for item in curve if item is not None]
|
||||
count = f'{len(filled)}/10' if curve else '—'
|
||||
text = ' '.join('—' if item is None else str(item) for item in curve) if curve else '—'
|
||||
self.joints_table.item(row, 4).setText(count)
|
||||
self.joints_table.item(row, 5).setText(text)
|
||||
self.joints_table.item(row, 5).setToolTip(text)
|
||||
status = result.get('reason') or result['status']
|
||||
self.joints_table.item(row, 6).setText(status)
|
||||
self.joints_table.item(row, 6).setToolTip(status)
|
||||
for view, label in self.tag_labels.items():
|
||||
tags = snap['tags'].get(view, {})
|
||||
label.setText(' '.join(f'ID{tag} {"✓" if not entry["error"] else "×"}'
|
||||
for tag, entry in sorted(tags.items())) or '等待 Tag 检测')
|
||||
label.setToolTip('\n'.join(f'ID{tag}: {entry["error"] or "有效"}' for tag, entry in sorted(tags.items())))
|
||||
for view, (stamp, frame, tags) in self.runtime.previews().items():
|
||||
if self.last_frames.get(view) == stamp:
|
||||
continue
|
||||
self.last_frames[view] = stamp
|
||||
draw = frame.copy()
|
||||
for tag, entry in tags.items():
|
||||
if not entry['corners']:
|
||||
continue
|
||||
points = np.asarray(entry['corners'], dtype=np.int32)
|
||||
color = (70,180,60) if not entry['error'] else (40,40,230)
|
||||
cv2.polylines(draw,[points],True,color,3)
|
||||
cv2.putText(draw,str(tag),tuple(points[0]),cv2.FONT_HERSHEY_SIMPLEX,1,color,2)
|
||||
rgb = cv2.cvtColor(draw,cv2.COLOR_BGR2RGB)
|
||||
image = QImage(rgb.data,rgb.shape[1],rgb.shape[0],rgb.strides[0],QImage.Format_RGB888).copy()
|
||||
label = self.previews_by_view[view]
|
||||
label.setPixmap(QPixmap.fromImage(image).scaled(label.size(),Qt.KeepAspectRatio,Qt.SmoothTransformation))
|
||||
|
||||
def closeEvent(self, event):
|
||||
self.shutdown()
|
||||
event.accept()
|
||||
@@ -0,0 +1,33 @@
|
||||
"""各型号手的业务。采集层只按文件夹发现型号,不写具体驱动。"""
|
||||
import importlib
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def _root() -> Path:
|
||||
return Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def available_models() -> list[str]:
|
||||
names = []
|
||||
for path in sorted(_root().iterdir()):
|
||||
if path.is_dir() and any(path.glob("*.yaml")):
|
||||
names.append(path.name)
|
||||
return names
|
||||
|
||||
|
||||
def _module(model: str):
|
||||
if model not in available_models():
|
||||
raise ValueError(f"未知型号: {model}")
|
||||
return importlib.import_module(f"hands.{model}")
|
||||
|
||||
|
||||
def adapter_type(key: str):
|
||||
for model in available_models():
|
||||
adapters = getattr(_module(model), "ADAPTERS", {})
|
||||
if key in adapters:
|
||||
return adapters[key]
|
||||
raise ValueError(f"未注册 SDK 适配器: {key}")
|
||||
|
||||
|
||||
def start_sdk(model: str, profile, station):
|
||||
return _module(model).start_sdk(profile, station)
|
||||
@@ -0,0 +1,35 @@
|
||||
"""O30 的行程标定适配和左右手配置。"""
|
||||
from pathlib import Path
|
||||
import subprocess
|
||||
|
||||
import yaml
|
||||
|
||||
from hands.o30.o30_ros import O30RosAdapter
|
||||
|
||||
|
||||
ADAPTERS = {"o30_ros": O30RosAdapter}
|
||||
|
||||
|
||||
def _render_parameter(value):
|
||||
if isinstance(value, bool):
|
||||
return "true" if value else "false"
|
||||
if isinstance(value, (list, tuple)):
|
||||
return "[" + ",".join(_render_parameter(item) for item in value) + "]"
|
||||
return str(value)
|
||||
|
||||
|
||||
def start_sdk(profile, station):
|
||||
config_path = Path(__file__).resolve().parent / "config.yaml"
|
||||
document = yaml.safe_load(config_path.read_text(encoding="utf-8"))
|
||||
setup = Path(str(document.get("setup") if isinstance(document, dict) else ""))
|
||||
if not setup.is_file():
|
||||
raise FileNotFoundError(f"找不到 O30 驱动环境: {setup}")
|
||||
parameters = O30RosAdapter.launch_parameters(profile, station)
|
||||
arguments = " ".join(
|
||||
f"-p {name}:={_render_parameter(value)}" for name, value in parameters.items()
|
||||
)
|
||||
script = (
|
||||
f'source "{setup}" && exec ros2 run {profile.sdk["package"]} {profile.sdk["executable"]} '
|
||||
f"--ros-args {arguments}"
|
||||
)
|
||||
return subprocess.Popen(["bash", "-lc", script], start_new_session=True)
|
||||
@@ -0,0 +1,13 @@
|
||||
# 这台机器上 O30 驱动工程的 install/setup.bash。
|
||||
setup: /home/huangwen/WorkSpace/Linker_Hand_O30_ROS2_SDK/Linker_Hand_O30_ROS2_SDK/install/setup.bash
|
||||
|
||||
# 手掌相对工装、工装相对正面相机。下面是占位单位阵,实机必须改成这套工装的实测值。
|
||||
# 单位阵配合真实 URDF 轴时,轴和相机光轴夹角过大,扫描会暂停,不会把该帧当成到达。
|
||||
fixture_from_hand_base_link:
|
||||
xyz: [0, 0, 0]
|
||||
rpy: [0, 0, 0]
|
||||
front_from_fixture:
|
||||
- [1, 0, 0, 0]
|
||||
- [0, 1, 0, 0]
|
||||
- [0, 0, 1, 0]
|
||||
- [0, 0, 0, 1]
|
||||
@@ -0,0 +1,69 @@
|
||||
schema_version: 1
|
||||
model: O30
|
||||
side: left
|
||||
command_unit: u8
|
||||
# 仓库里还没有左手 URDF。关节名与右手相同,先共用这份文件;镜像轴要等左手模型再换。
|
||||
urdf: urdf/linkerhand_O30i_right-V2_0819.urdf
|
||||
adapter: o30_ros
|
||||
command: {minimum: 0, maximum: 255, resolution: 1}
|
||||
sdk:
|
||||
package: linker_hand_o30_ros2_sdk
|
||||
executable: linker_hand_o30_ros2_sdk
|
||||
node: /linkerhand_range_sdk
|
||||
command_topic: /cb_left_hand_control_cmd
|
||||
feedback_topic: /cb_left_hand_state
|
||||
setting_topic: /cb_left_hand_setting_cmd
|
||||
info_topic: /cb_left_hand_info
|
||||
joints:
|
||||
- {name: thumb_cmc_roll, sdk_name: thumb_roll, index: 0, view: front, tag_id: 21}
|
||||
- {name: thumb_cmc_yaw, sdk_name: thumb_yaw, index: 1, view: top, tag_id: 30}
|
||||
- {name: index_mcp_roll, sdk_name: index_yaw, index: 2, view: front, tag_id: 22}
|
||||
- {name: middle_mcp_roll, sdk_name: middle_yaw, index: 3, view: front, tag_id: 23}
|
||||
- {name: ring_mcp_roll, sdk_name: ring_yaw, index: 4, view: front, tag_id: 24}
|
||||
- {name: pinky_mcp_roll, sdk_name: little_yaw, index: 5, view: front, tag_id: 25}
|
||||
- {name: thumb_mcp, sdk_name: thumb_root1, index: 6, view: front, tag_id: 21}
|
||||
- {name: index_mcp_pitch, sdk_name: index_root1, index: 7, view: side, tag_id: 26}
|
||||
- {name: middle_mcp_pitch, sdk_name: middle_root1, index: 8, view: side, tag_id: 27}
|
||||
- {name: ring_mcp_pitch, sdk_name: ring_root1, index: 9, view: side, tag_id: 28}
|
||||
- {name: pinky_mcp_pitch, sdk_name: little_root1, index: 10, view: side, tag_id: 29}
|
||||
- {name: index_pip, sdk_name: index_root2, index: 11, view: side, tag_id: 26}
|
||||
- {name: middle_pip, sdk_name: middle_root2, index: 12, view: side, tag_id: 27}
|
||||
- {name: ring_pip, sdk_name: ring_root2, index: 13, view: side, tag_id: 28}
|
||||
- {name: pinky_pip, sdk_name: little_root2, index: 14, view: side, tag_id: 29}
|
||||
- {name: thumb_ip, sdk_name: thumb_tip, index: 15, view: front, tag_id: 21}
|
||||
- {name: index_dip, sdk_name: index_tip, index: 16, view: side, tag_id: 26}
|
||||
- {name: middle_dip, sdk_name: middle_tip, index: 17, view: side, tag_id: 27}
|
||||
- {name: ring_dip, sdk_name: ring_tip, index: 18, view: side, tag_id: 28}
|
||||
- {name: pinky_dip, sdk_name: little_tip, index: 19, view: side, tag_id: 29}
|
||||
clearances:
|
||||
index_roll_for_thumb:
|
||||
targets: {index_mcp_roll: 0}
|
||||
thumb_yaw_for_mcp:
|
||||
targets: {thumb_cmc_yaw: 80}
|
||||
index_fold:
|
||||
targets: {index_mcp_pitch: 255, index_pip: 255, index_dip: 255}
|
||||
middle_fold:
|
||||
targets: {middle_mcp_pitch: 255, middle_pip: 255, middle_dip: 255}
|
||||
ring_fold:
|
||||
targets: {ring_mcp_pitch: 255, ring_pip: 255, ring_dip: 255}
|
||||
tasks:
|
||||
- {name: thumb_cmc_roll, joints: [thumb_cmc_roll], clearances: [index_roll_for_thumb], restore_after: [thumb_cmc_roll, index_mcp_roll]}
|
||||
- {name: thumb_mcp, joints: [thumb_mcp], clearances: [thumb_yaw_for_mcp]}
|
||||
- {name: thumb_ip, joints: [thumb_ip]}
|
||||
- name: four_finger_roll
|
||||
joints: [index_mcp_roll, middle_mcp_roll, ring_mcp_roll, pinky_mcp_roll]
|
||||
allow_override: false
|
||||
deferred_lower: {joint: middle_mcp_roll, split: 80}
|
||||
- {name: index_mcp_pitch, joints: [index_mcp_pitch]}
|
||||
- {name: index_pip, joints: [index_pip]}
|
||||
- {name: index_dip, joints: [index_dip]}
|
||||
- {name: middle_mcp_pitch, joints: [middle_mcp_pitch], clearances: [index_fold]}
|
||||
- {name: middle_pip, joints: [middle_pip], clearances: [index_fold]}
|
||||
- {name: middle_dip, joints: [middle_dip], clearances: [index_fold]}
|
||||
- {name: ring_mcp_pitch, joints: [ring_mcp_pitch], clearances: [index_fold, middle_fold]}
|
||||
- {name: ring_pip, joints: [ring_pip], clearances: [index_fold, middle_fold]}
|
||||
- {name: ring_dip, joints: [ring_dip], clearances: [index_fold, middle_fold]}
|
||||
- {name: pinky_mcp_pitch, joints: [pinky_mcp_pitch], clearances: [index_fold, middle_fold, ring_fold]}
|
||||
- {name: pinky_pip, joints: [pinky_pip], clearances: [index_fold, middle_fold, ring_fold]}
|
||||
- {name: pinky_dip, joints: [pinky_dip], clearances: [index_fold, middle_fold, ring_fold]}
|
||||
- {name: thumb_cmc_yaw, joints: [thumb_cmc_yaw]}
|
||||
@@ -0,0 +1,68 @@
|
||||
schema_version: 1
|
||||
model: O30
|
||||
side: right
|
||||
command_unit: u8
|
||||
urdf: urdf/linkerhand_O30i_right-V2_0819.urdf
|
||||
adapter: o30_ros
|
||||
command: {minimum: 0, maximum: 255, resolution: 1}
|
||||
sdk:
|
||||
package: linker_hand_o30_ros2_sdk
|
||||
executable: linker_hand_o30_ros2_sdk
|
||||
node: /linkerhand_range_sdk
|
||||
command_topic: /cb_right_hand_control_cmd
|
||||
feedback_topic: /cb_right_hand_state
|
||||
setting_topic: /cb_right_hand_setting_cmd
|
||||
info_topic: /cb_right_hand_info
|
||||
joints:
|
||||
- {name: thumb_cmc_roll, sdk_name: thumb_roll, index: 0, view: front, tag_id: 21}
|
||||
- {name: thumb_cmc_yaw, sdk_name: thumb_yaw, index: 1, view: top, tag_id: 30}
|
||||
- {name: index_mcp_roll, sdk_name: index_yaw, index: 2, view: front, tag_id: 25}
|
||||
- {name: middle_mcp_roll, sdk_name: middle_yaw, index: 3, view: front, tag_id: 24}
|
||||
- {name: ring_mcp_roll, sdk_name: ring_yaw, index: 4, view: front, tag_id: 23}
|
||||
- {name: pinky_mcp_roll, sdk_name: little_yaw, index: 5, view: front, tag_id: 22}
|
||||
- {name: thumb_mcp, sdk_name: thumb_root1, index: 6, view: front, tag_id: 21}
|
||||
- {name: index_mcp_pitch, sdk_name: index_root1, index: 7, view: side, tag_id: 29}
|
||||
- {name: middle_mcp_pitch, sdk_name: middle_root1, index: 8, view: side, tag_id: 28}
|
||||
- {name: ring_mcp_pitch, sdk_name: ring_root1, index: 9, view: side, tag_id: 27}
|
||||
- {name: pinky_mcp_pitch, sdk_name: little_root1, index: 10, view: side, tag_id: 26}
|
||||
- {name: index_pip, sdk_name: index_root2, index: 11, view: side, tag_id: 29}
|
||||
- {name: middle_pip, sdk_name: middle_root2, index: 12, view: side, tag_id: 28}
|
||||
- {name: ring_pip, sdk_name: ring_root2, index: 13, view: side, tag_id: 27}
|
||||
- {name: pinky_pip, sdk_name: little_root2, index: 14, view: side, tag_id: 26}
|
||||
- {name: thumb_ip, sdk_name: thumb_tip, index: 15, view: front, tag_id: 21}
|
||||
- {name: index_dip, sdk_name: index_tip, index: 16, view: side, tag_id: 29}
|
||||
- {name: middle_dip, sdk_name: middle_tip, index: 17, view: side, tag_id: 28}
|
||||
- {name: ring_dip, sdk_name: ring_tip, index: 18, view: side, tag_id: 27}
|
||||
- {name: pinky_dip, sdk_name: little_tip, index: 19, view: side, tag_id: 26}
|
||||
clearances:
|
||||
index_roll_for_thumb:
|
||||
targets: {index_mcp_roll: 0}
|
||||
thumb_yaw_for_mcp:
|
||||
targets: {thumb_cmc_yaw: 80}
|
||||
pinky_fold:
|
||||
targets: {pinky_mcp_pitch: 255, pinky_pip: 255, pinky_dip: 255}
|
||||
ring_fold:
|
||||
targets: {ring_mcp_pitch: 255, ring_pip: 255, ring_dip: 255}
|
||||
middle_fold:
|
||||
targets: {middle_mcp_pitch: 255, middle_pip: 255, middle_dip: 255}
|
||||
tasks:
|
||||
- {name: thumb_cmc_roll, joints: [thumb_cmc_roll], clearances: [index_roll_for_thumb], restore_after: [thumb_cmc_roll, index_mcp_roll]}
|
||||
- {name: thumb_mcp, joints: [thumb_mcp], clearances: [thumb_yaw_for_mcp]}
|
||||
- {name: thumb_ip, joints: [thumb_ip]}
|
||||
- name: four_finger_roll
|
||||
joints: [index_mcp_roll, middle_mcp_roll, ring_mcp_roll, pinky_mcp_roll]
|
||||
allow_override: false
|
||||
deferred_lower: {joint: middle_mcp_roll, split: 80}
|
||||
- {name: pinky_mcp_pitch, joints: [pinky_mcp_pitch]}
|
||||
- {name: pinky_pip, joints: [pinky_pip]}
|
||||
- {name: pinky_dip, joints: [pinky_dip]}
|
||||
- {name: ring_mcp_pitch, joints: [ring_mcp_pitch], clearances: [pinky_fold]}
|
||||
- {name: ring_pip, joints: [ring_pip], clearances: [pinky_fold]}
|
||||
- {name: ring_dip, joints: [ring_dip], clearances: [pinky_fold]}
|
||||
- {name: middle_mcp_pitch, joints: [middle_mcp_pitch], clearances: [pinky_fold, ring_fold]}
|
||||
- {name: middle_pip, joints: [middle_pip], clearances: [pinky_fold, ring_fold]}
|
||||
- {name: middle_dip, joints: [middle_dip], clearances: [pinky_fold, ring_fold]}
|
||||
- {name: index_mcp_pitch, joints: [index_mcp_pitch], clearances: [pinky_fold, ring_fold, middle_fold]}
|
||||
- {name: index_pip, joints: [index_pip], clearances: [pinky_fold, ring_fold, middle_fold]}
|
||||
- {name: index_dip, joints: [index_dip], clearances: [pinky_fold, ring_fold, middle_fold]}
|
||||
- {name: thumb_cmc_yaw, joints: [thumb_cmc_yaw]}
|
||||
@@ -0,0 +1,194 @@
|
||||
"""O30 ROS boundary. Does not import, copy, patch or open the vendor SDK."""
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
|
||||
from rcl_interfaces.srv import GetParameters
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import String
|
||||
|
||||
from calibration.adapters.base import Feedback
|
||||
|
||||
|
||||
class O30RosAdapter:
|
||||
@staticmethod
|
||||
def launch_parameters(profile, station):
|
||||
return {**station['sdk'], 'hand_type': profile.side, 'hand_joint': profile.model,
|
||||
'auto_init_pose': False, 'is_touch': False, 'state_rate': 30.0,
|
||||
'strict_device_check': True, 'ignore_joint_faults': False,
|
||||
'joint_limit_min': [int(j.minimum) for j in profile.joints],
|
||||
'joint_limit_max': [int(j.maximum) for j in profile.joints], 'cmd_timeout': 0.0}
|
||||
|
||||
def __init__(self, node, profile, settings):
|
||||
self.node, self.profile, self.settings = node, profile, settings
|
||||
self.joints_by_sdk_name = {joint.sdk_name: joint for joint in profile.joints}
|
||||
self.uid, self.feedback, self.info = '', None, None
|
||||
self.feedback_error, self.info_error = '', ''
|
||||
self.info_at = float('-inf')
|
||||
self.parameters_error = '尚未确认 SDK 启动参数和限位'
|
||||
self.parameters_ok = False
|
||||
self.next_check = 0
|
||||
self.next_graph = 0
|
||||
self.graph_error = '尚未检查控制话题'
|
||||
self.parameter_future = None
|
||||
self.publisher = node.create_publisher(JointState, profile.sdk['command_topic'], 1)
|
||||
self.setting_pub = node.create_publisher(String, profile.sdk['setting_topic'], 10)
|
||||
self.state_sub = node.create_subscription(JointState, profile.sdk['feedback_topic'], self._feedback, 20)
|
||||
self.info_sub = node.create_subscription(String, profile.sdk['info_topic'], self._info, 10)
|
||||
self.param_client = node.create_client(GetParameters, profile.sdk['node'].rstrip('/') + '/get_parameters')
|
||||
self.timer = node.create_timer(0.5, self._check_parameters)
|
||||
|
||||
def _feedback(self, message):
|
||||
expected = [j.sdk_name for j in self.profile.joints]
|
||||
positions = list(message.position)
|
||||
if list(message.name) != expected or len(positions) != len(expected):
|
||||
self.feedback_error = 'SDK 反馈关节名称、顺序或数量不符'
|
||||
return
|
||||
if any(not math.isfinite(x) or x < j.minimum or x > j.maximum
|
||||
for j, x in zip(self.profile.joints, positions)):
|
||||
self.feedback_error = 'SDK 反馈数值无效'
|
||||
return
|
||||
stamp = message.header.stamp.sec*1_000_000_000 + message.header.stamp.nanosec
|
||||
now_stamp = self.node.get_clock().now().nanoseconds
|
||||
if stamp <= 0 or abs(now_stamp-stamp) > int(self.settings.freshness*1e9):
|
||||
self.feedback_error = 'SDK 反馈时间戳过期或不在同一时钟域'
|
||||
return
|
||||
if self.feedback and stamp <= self.feedback.stamp_ns:
|
||||
return
|
||||
self.feedback_error = ''
|
||||
self.feedback = Feedback(stamp, time.monotonic(), tuple(positions))
|
||||
|
||||
def _info(self, message):
|
||||
try:
|
||||
data = json.loads(message.data)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError('诊断信息不是对象')
|
||||
self.info, self.info_at = data, time.monotonic()
|
||||
self.uid = str(data.get('uid') or '').strip()
|
||||
self.info_error = ''
|
||||
except (ValueError, TypeError) as error:
|
||||
self.info_error = f'SDK 诊断解析失败: {error}'
|
||||
|
||||
def _check_parameters(self):
|
||||
now = time.monotonic()
|
||||
if self.parameter_future and not self.parameter_future.done():
|
||||
if now-self.request_at > 3:
|
||||
self.parameter_future.cancel()
|
||||
self.parameter_future = None
|
||||
self.parameters_ok = False
|
||||
self.parameters_error = '读取 SDK 参数超时'
|
||||
return
|
||||
if now < self.next_check or not self.param_client.service_is_ready():
|
||||
return
|
||||
self.next_check, self.request_at = now+2, now
|
||||
names = ['hand_type', 'hand_joint', 'auto_init_pose', 'joint_limit_min',
|
||||
'joint_limit_max', 'cmd_timeout']
|
||||
self.parameter_future = self.param_client.call_async(GetParameters.Request(names=names))
|
||||
self.parameter_future.add_done_callback(self._parameters)
|
||||
|
||||
def _parameters(self, future):
|
||||
try:
|
||||
values = future.result().values
|
||||
if len(values) != 6:
|
||||
raise ValueError('参数数量不符')
|
||||
if values[0].string_value != self.profile.side or values[1].string_value != self.profile.model:
|
||||
raise ValueError('SDK 型号或左右手参数不符')
|
||||
if values[2].bool_value or values[5].double_value != 0:
|
||||
raise ValueError('SDK 必须关闭自动摆位和命令超时自动动作')
|
||||
lows, highs = list(values[3].integer_array_value), list(values[4].integer_array_value)
|
||||
if lows != [int(j.minimum) for j in self.profile.joints] or highs != [int(j.maximum) for j in self.profile.joints]:
|
||||
raise ValueError('SDK 软件限位未覆盖完整扫描范围')
|
||||
self.parameters_ok, self.parameters_error = True, ''
|
||||
except Exception as error:
|
||||
self.parameters_ok, self.parameters_error = False, f'SDK 参数检查失败: {error}'
|
||||
|
||||
def control_error(self, now):
|
||||
"""Require the command endpoint and valid feedback, as used by calibration.
|
||||
|
||||
Runtime SDK diagnostic flags are telemetry, not another motion interlock.
|
||||
SDK initialization and device protections remain the SDK's responsibility.
|
||||
"""
|
||||
if now >= self.next_graph:
|
||||
self.next_graph = now+0.2
|
||||
publishers = self.node.get_publishers_info_by_topic(self.profile.sdk['command_topic'])
|
||||
others = [p for p in publishers
|
||||
if (p.node_name, p.node_namespace) != (self.node.get_name(), self.node.get_namespace())]
|
||||
self.graph_error = ('存在其他位置命令发布者或发布者数量异常,请关闭其他控制器' if others or len(publishers) != 1 else
|
||||
('SDK 未订阅位置命令' if self.publisher.get_subscription_count() != 1 else ''))
|
||||
if self.graph_error:
|
||||
return self.graph_error
|
||||
if not self.parameters_ok:
|
||||
return self.parameters_error
|
||||
if self.feedback_error or self.info_error:
|
||||
return self.feedback_error or self.info_error
|
||||
if not self.feedback or now-self.feedback.received > self.settings.freshness:
|
||||
return 'SDK 位置反馈未收到或已断流'
|
||||
if self.info is None or now-self.info_at > 3:
|
||||
return 'SDK 诊断未收到或已断流'
|
||||
info = self.info
|
||||
if (info.get('model'), str(info.get('side', '')).lower(), info.get('hand_type')) != (
|
||||
self.profile.model, self.profile.side, self.profile.side):
|
||||
return '实机型号或左右手身份不符'
|
||||
if not self.uid or info.get('joint_names') != [j.sdk_name for j in self.profile.joints]:
|
||||
return '设备 UID 或诊断关节名称不符'
|
||||
return None
|
||||
|
||||
def _joint_label(self, sdk_name):
|
||||
joint = self.joints_by_sdk_name.get(sdk_name)
|
||||
return f'{joint.name}(SDK: {sdk_name})' if joint else sdk_name
|
||||
|
||||
def diagnostic_warning(self):
|
||||
"""Keep runtime diagnostics in the journal without gating commands."""
|
||||
info = self.info or {}
|
||||
reasons = []
|
||||
faults = []
|
||||
for sdk_name, entries in info.get('joint_faults', {}).items():
|
||||
if entries:
|
||||
detail = '、'.join(map(str, entries)) if isinstance(entries, list) else str(entries)
|
||||
faults.append(f'{self._joint_label(sdk_name)}:{detail}')
|
||||
if faults:
|
||||
reasons.append('SDK 关节故障:' + ';'.join(faults))
|
||||
temperatures = []
|
||||
for entry in info.get('over_temp', []):
|
||||
sdk_name, separator, value = str(entry).partition(':')
|
||||
temperatures.append(f'{self._joint_label(sdk_name)}:{value}°C' if separator else str(entry))
|
||||
if temperatures:
|
||||
reasons.append('SDK 温度异常:' + ';'.join(temperatures))
|
||||
if info.get('online') is False:
|
||||
reasons.append('SDK 心跳诊断:未确认在线')
|
||||
comm_error = info.get('comm_error', {})
|
||||
if comm_error.get('code', 0):
|
||||
reasons.append('SDK 通信诊断:' + json.dumps(comm_error, ensure_ascii=False, sort_keys=True))
|
||||
return ';'.join(reasons) or None
|
||||
|
||||
def send_positions(self, positions):
|
||||
values = self.profile.vector(positions)
|
||||
msg = JointState()
|
||||
msg.header.stamp = self.node.get_clock().now().to_msg()
|
||||
msg.name = [j.sdk_name for j in self.profile.joints]
|
||||
msg.position = [float(x) for x in values]
|
||||
self.publisher.publish(msg)
|
||||
|
||||
def set_motion(self, speed, torque):
|
||||
for command, field, value in [('set_speed', 'speed', speed),
|
||||
('set_max_torque_limits', 'torque', torque)]:
|
||||
if not isinstance(value, int) or not 0 <= value <= 255:
|
||||
raise ValueError('速度和力矩必须为0~255整数')
|
||||
self.setting_pub.publish(String(data=json.dumps({'setting_cmd': command, 'params': {
|
||||
'hand_type': self.profile.side, field: [value]*len(self.profile.joints)}})))
|
||||
|
||||
def set_offset(self, offset):
|
||||
"""Publish calibrated [min, max] pairs on the existing setting topic."""
|
||||
if not isinstance(offset, list) or len(offset) != len(self.profile.joints):
|
||||
raise ValueError(f'偏移数组长度必须为 {len(self.profile.joints)}')
|
||||
pairs = []
|
||||
for index, pair in enumerate(offset):
|
||||
if (not isinstance(pair, (list, tuple)) or len(pair) != 2 or
|
||||
any(not isinstance(value, int) or not 0 <= value <= 255 for value in pair) or
|
||||
pair[0] > pair[1]):
|
||||
raise ValueError(f'第 {index} 项偏移非法:需要整数 [min, max] 且 0≤min≤max≤255')
|
||||
pairs.append([pair[0], pair[1]])
|
||||
self.setting_pub.publish(String(data=json.dumps({
|
||||
'setting_cmd': 'set_offset',
|
||||
'params': {'hand_type': self.profile.side, 'offset': pairs},
|
||||
}, ensure_ascii=False)))
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1 @@
|
||||
"""与型号无关的标定主程序:采集、拟合、会话编排和相机投影。"""
|
||||
@@ -0,0 +1 @@
|
||||
"""把图像里的贴纸转角还原成绕 URDF 轴的关节角。"""
|
||||
@@ -0,0 +1,157 @@
|
||||
"""URDF 轴已知时,用图像边的转角还原关节角。
|
||||
|
||||
不读 ROS,不估计新的转轴。轴由运动学模型、工装和外参变到相机。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from calibration.urdf_model import UrdfKinematicModel
|
||||
|
||||
OPTICAL_AXIS = np.array([0.0, 0.0, 1.0])
|
||||
MAX_AXIS_ANGLE_RAD = math.radians(20.0)
|
||||
MIN_AXIS_COSINE = math.cos(MAX_AXIS_ANGLE_RAD)
|
||||
MAX_FRAME_JUMP_RAD = math.radians(35.0)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AngleReading:
|
||||
theta_rad: float | None
|
||||
axis_camera: np.ndarray | None
|
||||
axis_palm: np.ndarray | None
|
||||
cosine: float | None
|
||||
phi_rad: float | None
|
||||
reason: str = ""
|
||||
|
||||
@property
|
||||
def ok(self) -> bool:
|
||||
return self.reason == ""
|
||||
|
||||
|
||||
def edge_angle(corners: np.ndarray) -> float:
|
||||
"""AprilTag 固定顺序下,角点 1 减角点 0 的图像角,弧度。"""
|
||||
points = np.asarray(corners, dtype=float)
|
||||
if points.shape != (4, 2) or not np.all(np.isfinite(points)):
|
||||
raise ValueError("tag_not_visible")
|
||||
edge = points[1] - points[0]
|
||||
return float(math.atan2(edge[1], edge[0]))
|
||||
|
||||
|
||||
def unwrap_edge_angle(previous_rad: float, current_rad: float) -> float | None:
|
||||
"""相对上一帧展开。跳变超过 35° 时丢掉这一帧。"""
|
||||
delta = (current_rad - previous_rad + math.pi) % (2.0 * math.pi) - math.pi
|
||||
if abs(delta) > MAX_FRAME_JUMP_RAD:
|
||||
return None
|
||||
return previous_rad + delta
|
||||
|
||||
|
||||
def point_in_camera(
|
||||
point_palm: np.ndarray,
|
||||
*,
|
||||
camera: str,
|
||||
fixture_from_hand: np.ndarray,
|
||||
front_from_fixture: np.ndarray,
|
||||
front_from_view: dict[str, np.ndarray],
|
||||
) -> np.ndarray:
|
||||
"""把掌心中的点变到负责拍摄的相机。"""
|
||||
if camera not in front_from_view:
|
||||
raise ValueError(f"missing_camera_extrinsic:{camera}")
|
||||
point = np.ones(4)
|
||||
point[:3] = np.asarray(point_palm, dtype=float)
|
||||
point = fixture_from_hand @ point
|
||||
point = front_from_fixture @ point
|
||||
point = np.linalg.inv(front_from_view[camera]) @ point
|
||||
return point[:3]
|
||||
|
||||
|
||||
def axis_in_camera(
|
||||
axis_palm: np.ndarray,
|
||||
*,
|
||||
camera: str,
|
||||
fixture_from_hand: np.ndarray,
|
||||
front_from_fixture: np.ndarray,
|
||||
front_from_view: dict[str, np.ndarray],
|
||||
) -> np.ndarray:
|
||||
"""把掌心坐标系里的轴变到负责拍摄的相机。"""
|
||||
if camera not in front_from_view:
|
||||
raise ValueError(f"missing_camera_extrinsic:{camera}")
|
||||
axis = np.asarray(axis_palm, dtype=float)
|
||||
axis = fixture_from_hand[:3, :3] @ axis
|
||||
axis = front_from_fixture[:3, :3] @ axis
|
||||
front_from_camera = front_from_view[camera][:3, :3]
|
||||
camera_from_front = np.linalg.inv(front_from_camera)
|
||||
axis = camera_from_front @ axis
|
||||
norm = float(np.linalg.norm(axis))
|
||||
if norm <= 1e-12 or not math.isfinite(norm):
|
||||
raise ValueError("camera_axis_degenerate")
|
||||
return axis / norm
|
||||
|
||||
|
||||
def reading_from_corners(
|
||||
corners: np.ndarray | None,
|
||||
*,
|
||||
model: UrdfKinematicModel,
|
||||
joint: str,
|
||||
camera: str,
|
||||
zero_offsets: dict[str, float],
|
||||
required_ancestors: tuple[str, ...] | list[str],
|
||||
fixture_from_hand: np.ndarray | None,
|
||||
front_from_fixture: np.ndarray | None,
|
||||
front_from_view: dict[str, np.ndarray],
|
||||
phi_zero_rad: float | None,
|
||||
previous_phi_rad: float | None = None,
|
||||
palm_pose_valid: bool = True,
|
||||
unwrapped_phi: float | None = None,
|
||||
) -> AngleReading:
|
||||
"""由四个角点得到绕 URDF 轴的关节角。失败时 reason 非空,不退回铰链求解。"""
|
||||
missing = [name for name in required_ancestors if name not in zero_offsets]
|
||||
if missing:
|
||||
return AngleReading(None, None, None, None, None, "parent_zero_missing")
|
||||
if not palm_pose_valid or fixture_from_hand is None or front_from_fixture is None:
|
||||
return AngleReading(None, None, None, None, None, "fixture_pose_missing")
|
||||
if corners is None:
|
||||
return AngleReading(None, None, None, None, None, "tag_not_visible")
|
||||
if unwrapped_phi is None:
|
||||
try:
|
||||
phi = edge_angle(corners)
|
||||
except ValueError:
|
||||
return AngleReading(None, None, None, None, None, "tag_not_visible")
|
||||
if previous_phi_rad is not None:
|
||||
unwrapped = unwrap_edge_angle(previous_phi_rad, phi)
|
||||
if unwrapped is None:
|
||||
return AngleReading(None, None, None, None, phi, "corner_jump")
|
||||
phi = unwrapped
|
||||
else:
|
||||
phi = float(unwrapped_phi)
|
||||
axis_palm, _point = model.axis_line(
|
||||
joint, zero_offsets=zero_offsets, joint_angles={}
|
||||
)
|
||||
try:
|
||||
axis_camera = axis_in_camera(
|
||||
axis_palm,
|
||||
camera=camera,
|
||||
fixture_from_hand=fixture_from_hand,
|
||||
front_from_fixture=front_from_fixture,
|
||||
front_from_view=front_from_view,
|
||||
)
|
||||
except ValueError as error:
|
||||
return AngleReading(None, None, axis_palm, None, phi, str(error))
|
||||
cosine = float(np.dot(axis_camera, OPTICAL_AXIS))
|
||||
if abs(cosine) < MIN_AXIS_COSINE:
|
||||
return AngleReading(None, axis_camera, axis_palm, cosine, phi, "axis_camera_angle_too_large")
|
||||
if phi_zero_rad is None:
|
||||
return AngleReading(None, axis_camera, axis_palm, cosine, phi, "zero_reference_missing")
|
||||
theta = (phi - phi_zero_rad) / cosine
|
||||
return AngleReading(theta, axis_camera, axis_palm, cosine, phi, "")
|
||||
|
||||
|
||||
def quaternion_about_axis(axis_palm: np.ndarray, theta_rad: float) -> np.ndarray:
|
||||
"""绕掌心中这根 URDF 轴的旋转,scipy 四元数顺序 xyzw。"""
|
||||
axis = np.asarray(axis_palm, dtype=float)
|
||||
axis = axis / np.linalg.norm(axis)
|
||||
return Rotation.from_rotvec(axis * float(theta_rad)).as_quat()
|
||||
@@ -0,0 +1,48 @@
|
||||
"""工装变换只从配置读取。缺字段或几何不合法时直接失败。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def transform_from_xyz_rpy(translation_xyz_m, rpy_rad) -> np.ndarray:
|
||||
rotation = np.asarray(rpy_rad, dtype=float)
|
||||
translation = np.asarray(translation_xyz_m, dtype=float)
|
||||
if rotation.shape != (3,) or translation.shape != (3,):
|
||||
raise ValueError("fixture_transform_invalid")
|
||||
if not np.all(np.isfinite(rotation)) or not np.all(np.isfinite(translation)):
|
||||
raise ValueError("fixture_transform_invalid")
|
||||
pose = np.eye(4)
|
||||
pose[:3, :3] = Rotation.from_euler("xyz", rotation).as_matrix()
|
||||
pose[:3, 3] = translation
|
||||
return pose
|
||||
|
||||
|
||||
def transform_from_xyz_quat(translation_xyz_m, quaternion_xyzw) -> np.ndarray:
|
||||
quaternion = np.asarray(quaternion_xyzw, dtype=float)
|
||||
translation = np.asarray(translation_xyz_m, dtype=float)
|
||||
if quaternion.shape != (4,) or translation.shape != (3,):
|
||||
raise ValueError("fixture_transform_invalid")
|
||||
if not np.all(np.isfinite(quaternion)) or not np.all(np.isfinite(translation)):
|
||||
raise ValueError("fixture_transform_invalid")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if not math.isfinite(norm) or abs(norm - 1.0) > 1e-6:
|
||||
raise ValueError("fixture_quaternion_not_unit")
|
||||
pose = np.eye(4)
|
||||
pose[:3, :3] = Rotation.from_quat(quaternion).as_matrix()
|
||||
pose[:3, 3] = translation
|
||||
return pose
|
||||
|
||||
|
||||
def load_fixture(product: dict) -> np.ndarray:
|
||||
"""读取手掌到工装的固定变换。没有这块配置就失败,不用单位阵假装对齐。"""
|
||||
block = product.get("fixture_from_hand_base_link")
|
||||
if not isinstance(block, dict):
|
||||
raise ValueError("fixture_from_hand_missing")
|
||||
translation = block.get("xyz", block.get("translation_xyz_m"))
|
||||
if "rpy" in block:
|
||||
return transform_from_xyz_rpy(translation, block.get("rpy"))
|
||||
return transform_from_xyz_quat(translation, block.get("quaternion_xyzw"))
|
||||
@@ -0,0 +1,60 @@
|
||||
"""样本是否够用来写行程。数字来自 profile 的 acquisition。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
|
||||
def check_samples(samples: list[dict], *, start_u8: int, end_u8: int, baseline_u8: int, acquisition: dict) -> None:
|
||||
minimum = int(acquisition["minimum_valid_samples"])
|
||||
bins = int(acquisition["minimum_bins"])
|
||||
unobserved_limit = float(acquisition["maximum_unobserved_fraction"])
|
||||
span_fraction = float(acquisition["physical_first_cycle_minimum_span_01"])
|
||||
repeat_fraction = float(acquisition["physical_repeat_minimum_fraction"])
|
||||
steady_nodes = int(acquisition["steady_training_nodes"])
|
||||
if len(samples) < minimum:
|
||||
raise ValueError("minimum_valid_samples")
|
||||
directions = {sample["direction"] for sample in samples}
|
||||
if not {"increasing", "decreasing"} <= directions:
|
||||
raise ValueError("directional_samples_incomplete")
|
||||
if not any(sample["direction"] == "decreasing" and int(sample["command_u8"]) == int(baseline_u8) for sample in samples):
|
||||
raise ValueError("zero_reference_missing")
|
||||
lo, hi = min(int(start_u8), int(end_u8)), max(int(start_u8), int(end_u8))
|
||||
if hi <= lo:
|
||||
raise ValueError("command_span_empty")
|
||||
if acquisition.get("require_endpoint_observations", True):
|
||||
commands = {int(sample["command_u8"]) for sample in samples}
|
||||
if lo not in commands or hi not in commands:
|
||||
raise ValueError("endpoint_missing")
|
||||
occupied = set()
|
||||
for sample in samples:
|
||||
fraction = (int(sample["command_u8"]) - lo) / (hi - lo)
|
||||
occupied.add(min(bins - 1, int(fraction * bins)))
|
||||
if (bins - len(occupied)) / bins > unobserved_limit:
|
||||
raise ValueError("bins_unobserved")
|
||||
by_cycle: dict[int, list[int]] = {}
|
||||
for sample in samples:
|
||||
by_cycle.setdefault(int(sample.get("cycle", 0)), []).append(int(sample["command_u8"]))
|
||||
if 0 not in by_cycle:
|
||||
raise ValueError("first_cycle_missing")
|
||||
first_span = max(by_cycle[0]) - min(by_cycle[0])
|
||||
if first_span <= 0 or first_span / (hi - lo) < span_fraction:
|
||||
raise ValueError("first_cycle_span")
|
||||
repeats = [span for cycle, commands_in_cycle in by_cycle.items() if cycle != 0
|
||||
for span in (max(commands_in_cycle) - min(commands_in_cycle),)]
|
||||
if not repeats or max(repeats) / first_span < repeat_fraction:
|
||||
raise ValueError("repeat_span")
|
||||
if sum(1 for sample in samples if sample.get("steady")) < steady_nodes:
|
||||
raise ValueError("steady_nodes")
|
||||
|
||||
|
||||
def reference_drift_px(frames: list) -> float:
|
||||
"""相对第一帧,四个角点里最大的位移,单位像素。"""
|
||||
import numpy as np
|
||||
|
||||
if len(frames) < 2:
|
||||
return 0.0
|
||||
origin = np.asarray(frames[0], dtype=float)
|
||||
worst = 0.0
|
||||
for frame in frames[1:]:
|
||||
delta = np.asarray(frame, dtype=float) - origin
|
||||
worst = max(worst, float(np.sqrt(np.max(np.sum(delta * delta, axis=1)))))
|
||||
return worst
|
||||
@@ -0,0 +1,39 @@
|
||||
"""准备段估一次贴纸安装,扫行程时用已知轴和已知角度重投影。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from calibration.cameras.fixture_overlay import square_object_points, tag_pose
|
||||
|
||||
|
||||
def mount_from_corners(corners: np.ndarray, *, tag_size_m: float, camera_matrix: np.ndarray, distortion: np.ndarray) -> np.ndarray:
|
||||
pose = tag_pose(corners, tag_size_m=tag_size_m, camera_matrix=camera_matrix, distortion=distortion)
|
||||
if pose is None:
|
||||
raise ValueError("tag_mount_unresolved")
|
||||
rotation, translation = pose
|
||||
object_points = square_object_points(tag_size_m)
|
||||
return (rotation @ object_points.T).T + translation.reshape(1, 3)
|
||||
|
||||
|
||||
def reprojection_rms(
|
||||
mount_points: np.ndarray,
|
||||
*,
|
||||
theta_rad: float,
|
||||
axis_point: np.ndarray,
|
||||
axis_direction: np.ndarray,
|
||||
corners: np.ndarray,
|
||||
camera_matrix: np.ndarray,
|
||||
distortion: np.ndarray,
|
||||
) -> float:
|
||||
import cv2
|
||||
|
||||
axis = np.asarray(axis_direction, dtype=float)
|
||||
axis = axis / np.linalg.norm(axis)
|
||||
rotated = Rotation.from_rotvec(axis * float(theta_rad)).apply(mount_points - axis_point) + axis_point
|
||||
projected, _ = cv2.projectPoints(
|
||||
np.asarray(rotated, dtype=float), np.zeros(3), np.zeros(3), camera_matrix, distortion,
|
||||
)
|
||||
delta = projected.reshape(4, 2) - np.asarray(corners, dtype=float)
|
||||
return float(np.sqrt(np.mean(np.sum(delta * delta, axis=1))))
|
||||
@@ -0,0 +1,23 @@
|
||||
"""把关节角写成拟合仍能读取的样本。旋转已经绕着模型轴。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from calibration.acquisition.angle import quaternion_about_axis
|
||||
|
||||
|
||||
def joint_sample(*, joint: str, command_u8: int, direction: str, cycle: int,
|
||||
theta_rad: float, axis_palm, view: str) -> dict:
|
||||
if direction not in {"increasing", "decreasing"}:
|
||||
raise ValueError(f"unknown direction: {direction}")
|
||||
quaternion = quaternion_about_axis(axis_palm, theta_rad)
|
||||
return {
|
||||
"kind": "joint_sample",
|
||||
"joint": joint,
|
||||
"command_u8": int(command_u8),
|
||||
"direction": direction,
|
||||
"cycle": int(cycle),
|
||||
"view": view,
|
||||
"theta_rad": float(theta_rad),
|
||||
"axis_palm_xyz": [float(value) for value in axis_palm],
|
||||
"relative_quaternion_xyzw": [float(value) for value in quaternion],
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
"""打开相机画面,并把关节坐标系投影到像素。不决定哪台相机看哪个关节。"""
|
||||
@@ -0,0 +1,91 @@
|
||||
"""标定读取 output_camera 里的内外参。外参取最新一份,内参必须对上其中的指纹。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
from calibration.acquisition.fixture import transform_from_xyz_quat
|
||||
from camera_parameters.extrinsics_geometry import camera_info_fingerprint
|
||||
from camera_parameters.output_paths import intrinsic_paths, latest_extrinsic_path
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraFiles:
|
||||
extrinsics_path: Path
|
||||
intrinsics: dict[str, Path]
|
||||
front_from_view: dict
|
||||
document: dict
|
||||
|
||||
|
||||
def camera_station_path(workspace: Path) -> Path:
|
||||
"""两套标定共用的工位相机配置,放在相机包里。"""
|
||||
return Path(workspace) / "src" / "camera_parameters" / "config" / "cameras.yaml"
|
||||
|
||||
|
||||
def load_camera_station(workspace: Path) -> dict:
|
||||
path = camera_station_path(workspace)
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"找不到相机配置: {path}")
|
||||
document = yaml.safe_load(path.read_text(encoding="utf-8"))
|
||||
if not isinstance(document, dict):
|
||||
raise ValueError(f"相机配置不完整: {path}")
|
||||
return document
|
||||
|
||||
|
||||
def load_camera_files(workspace: Path) -> CameraFiles:
|
||||
extrinsics_path = latest_extrinsic_path(workspace)
|
||||
if extrinsics_path is None:
|
||||
raise FileNotFoundError(f"{workspace}/output_camera/extrinsics 里没有外参")
|
||||
document = yaml.safe_load(extrinsics_path.read_text(encoding="utf-8"))
|
||||
cameras = document.get("cameras") if isinstance(document, dict) else None
|
||||
transforms = document.get("front_from_view") if isinstance(document, dict) else None
|
||||
if not isinstance(cameras, dict) or not isinstance(transforms, dict):
|
||||
raise ValueError(f"外参文件不完整: {extrinsics_path}")
|
||||
intrinsics = {}
|
||||
front_from_view = {}
|
||||
for view in VIEWS:
|
||||
camera = cameras.get(view)
|
||||
block = transforms.get(view)
|
||||
if not isinstance(camera, dict) or not isinstance(block, dict):
|
||||
raise ValueError(f"外参缺少 {view}: {extrinsics_path}")
|
||||
serial = str(camera.get("serial_number") or "")
|
||||
expected = str(camera.get("intrinsics_sha256") or "")
|
||||
if not serial or not expected:
|
||||
raise ValueError(f"外参缺少 {view} 的序列号或内参指纹: {extrinsics_path}")
|
||||
intrinsics[view] = _intrinsic_for(workspace, serial, expected, extrinsics_path)
|
||||
front_from_view[view] = transform_from_xyz_quat(
|
||||
block["translation_xyz_m"], block["quaternion_xyzw"],
|
||||
)
|
||||
return CameraFiles(extrinsics_path, intrinsics, front_from_view, document)
|
||||
|
||||
|
||||
def _fingerprint(path: Path) -> str:
|
||||
document = yaml.safe_load(path.read_text(encoding="utf-8"))
|
||||
return camera_info_fingerprint(
|
||||
width=int(document["image_width"]),
|
||||
height=int(document["image_height"]),
|
||||
camera_matrix=document["camera_matrix"]["data"],
|
||||
distortion=document["distortion_coefficients"]["data"],
|
||||
rectification=document["rectification_matrix"]["data"],
|
||||
projection=document["projection_matrix"]["data"],
|
||||
)
|
||||
|
||||
|
||||
def _intrinsic_for(workspace: Path, serial: str, expected: str, extrinsics_path: Path) -> Path:
|
||||
paths = intrinsic_paths(workspace, serial)
|
||||
if not paths:
|
||||
raise FileNotFoundError(f"output_camera/intrinsics 里没有 {serial}")
|
||||
for path in reversed(paths):
|
||||
if _fingerprint(path) == expected:
|
||||
return path
|
||||
newest = paths[-1]
|
||||
raise ValueError(
|
||||
f"{serial} 的内参指纹与 {extrinsics_path.name} 不一致:"
|
||||
f"外参记录 {expected},最新文件 {newest.name} 是 {_fingerprint(newest)}"
|
||||
)
|
||||
@@ -0,0 +1,246 @@
|
||||
"""用上面相机里四张角贴纸建立工装板坐标系,再画到每台相机。缺一张就不画。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from calibration.cameras.files import load_camera_files, load_camera_station
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
AXIS_LENGTH_M = 0.06
|
||||
MAX_REPROJECTION_PX = 3.0
|
||||
MAX_PLANE_RESIDUAL_M = 0.015
|
||||
POSE_FRESH_SEC = 0.5
|
||||
|
||||
|
||||
def square_object_points(tag_size_m: float) -> np.ndarray:
|
||||
"""角点顺序:左下、右下、右上、左上。与 apriltag 的 lb-rb-rt-lt 一致。"""
|
||||
half = float(tag_size_m) / 2.0
|
||||
return np.asarray(
|
||||
[
|
||||
[-half, half, 0.0],
|
||||
[half, half, 0.0],
|
||||
[half, -half, 0.0],
|
||||
[-half, -half, 0.0],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
|
||||
|
||||
def load_pinhole(path: Path) -> tuple[np.ndarray, np.ndarray]:
|
||||
document = yaml.safe_load(Path(path).read_text(encoding="utf-8"))
|
||||
matrix = np.asarray(document["camera_matrix"]["data"], dtype=np.float64).reshape(3, 3)
|
||||
distortion = np.asarray(document["distortion_coefficients"]["data"], dtype=np.float64).reshape(-1, 1)
|
||||
return matrix, distortion
|
||||
|
||||
|
||||
def _camera_sees_tag_front(rotation: np.ndarray, translation: np.ndarray) -> bool:
|
||||
"""贴纸 +Z 指向工装内部时,相机应在贴纸的 -Z 一侧。"""
|
||||
camera_in_tag = -rotation.T @ translation.reshape(3)
|
||||
return float(camera_in_tag[2]) < 0.0
|
||||
|
||||
|
||||
def tag_pose(
|
||||
corners_xy: np.ndarray,
|
||||
*,
|
||||
tag_size_m: float,
|
||||
camera_matrix: np.ndarray,
|
||||
distortion: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray] | None:
|
||||
image_points = np.asarray(corners_xy, dtype=np.float64).reshape(4, 2)
|
||||
object_points = square_object_points(tag_size_m)
|
||||
solved, rotations, translations, _ = cv2.solvePnPGeneric(
|
||||
object_points,
|
||||
image_points,
|
||||
camera_matrix,
|
||||
distortion,
|
||||
flags=cv2.SOLVEPNP_IPPE_SQUARE,
|
||||
)
|
||||
if not solved:
|
||||
return None
|
||||
best = None
|
||||
for rotation_vector, translation in zip(rotations, translations):
|
||||
rotation, _ = cv2.Rodrigues(rotation_vector)
|
||||
translation = np.asarray(translation, dtype=np.float64).reshape(3)
|
||||
if not _camera_sees_tag_front(rotation, translation):
|
||||
continue
|
||||
projected, _ = cv2.projectPoints(
|
||||
object_points, rotation_vector, translation, camera_matrix, distortion,
|
||||
)
|
||||
error = float(np.sqrt(np.mean(np.sum((projected.reshape(4, 2) - image_points) ** 2, axis=1))))
|
||||
if best is None or error < best[0]:
|
||||
best = (error, rotation, translation)
|
||||
if best is None or best[0] > MAX_REPROJECTION_PX:
|
||||
return None
|
||||
return best[1], best[2]
|
||||
|
||||
|
||||
def project_frame(
|
||||
origin: np.ndarray,
|
||||
rotation: np.ndarray,
|
||||
camera_matrix: np.ndarray,
|
||||
distortion: np.ndarray,
|
||||
*,
|
||||
axis_length_m: float = AXIS_LENGTH_M,
|
||||
) -> dict[str, tuple[tuple[int, int], tuple[int, int]]] | None:
|
||||
if float(origin[2]) <= 1e-4:
|
||||
return None
|
||||
points = np.vstack([
|
||||
origin,
|
||||
*[origin + rotation[:, column] * axis_length_m for column in range(3)],
|
||||
])
|
||||
projected, _ = cv2.projectPoints(points, np.zeros(3), np.zeros(3), camera_matrix, distortion)
|
||||
pixels = projected.reshape(4, 2)
|
||||
start = tuple(int(round(value)) for value in pixels[0])
|
||||
return {
|
||||
name: (start, tuple(int(round(value)) for value in pixels[index + 1]))
|
||||
for index, name in enumerate(("x", "y", "z"))
|
||||
}
|
||||
|
||||
|
||||
def _segment_intersection(start: np.ndarray, end: np.ndarray, other_start: np.ndarray, other_end: np.ndarray):
|
||||
direction = end - start
|
||||
other = other_end - other_start
|
||||
solution, _, _, _ = np.linalg.lstsq(
|
||||
np.column_stack([direction, -other]), other_start - start, rcond=None,
|
||||
)
|
||||
along, other_along = (float(solution[0]), float(solution[1]))
|
||||
if not (0.0 < along < 1.0 and 0.0 < other_along < 1.0):
|
||||
return None
|
||||
point = start + along * direction
|
||||
if float(np.linalg.norm(point - (other_start + other_along * other))) > 1e-4:
|
||||
return None
|
||||
return point
|
||||
|
||||
|
||||
def plate_frame(
|
||||
centers: dict[int, np.ndarray],
|
||||
*,
|
||||
x_from: int,
|
||||
x_to: int,
|
||||
) -> tuple[np.ndarray, np.ndarray] | None:
|
||||
"""原点是两条对角线的交点。X 从 x_from 指向 x_to,Z 朝向相机,Y = Z × X。"""
|
||||
ids = list(centers)
|
||||
if len(ids) != 4 or x_from not in centers or x_to not in centers:
|
||||
return None
|
||||
points = np.stack([np.asarray(centers[tag_id], dtype=np.float64).reshape(3) for tag_id in ids])
|
||||
centroid = points.mean(axis=0)
|
||||
_, _, axes = np.linalg.svd(points - centroid, full_matrices=True)
|
||||
normal = axes[-1]
|
||||
if float(np.dot(normal, centroid)) > 0.0:
|
||||
normal = -normal
|
||||
if float(np.max(np.abs((points - centroid) @ normal))) > MAX_PLANE_RESIDUAL_M:
|
||||
return None
|
||||
projected = {
|
||||
tag_id: np.asarray(centers[tag_id], dtype=np.float64).reshape(3)
|
||||
- normal * float(np.dot(np.asarray(centers[tag_id], dtype=np.float64).reshape(3) - centroid, normal))
|
||||
for tag_id in ids
|
||||
}
|
||||
others = [tag_id for tag_id in ids if tag_id != x_from]
|
||||
span = {tag_id: float(np.linalg.norm(projected[tag_id] - projected[x_from])) for tag_id in others}
|
||||
if span[x_to] >= max(span.values()) - 1e-9:
|
||||
return None
|
||||
pairings = []
|
||||
for index, first in enumerate(others):
|
||||
second = others[(index + 1) % len(others)]
|
||||
third = others[(index + 2) % len(others)]
|
||||
if len(pairings) == 3:
|
||||
break
|
||||
pairings.append(((x_from, first), (second, third)))
|
||||
origin = None
|
||||
for (first, second), (third, fourth) in pairings:
|
||||
intersection = _segment_intersection(
|
||||
projected[first], projected[second], projected[third], projected[fourth],
|
||||
)
|
||||
if intersection is not None:
|
||||
origin = intersection
|
||||
break
|
||||
if origin is None:
|
||||
return None
|
||||
forward = projected[x_to] - projected[x_from]
|
||||
forward = forward - normal * float(np.dot(forward, normal))
|
||||
if float(np.linalg.norm(forward)) < 1e-6:
|
||||
return None
|
||||
forward = forward / np.linalg.norm(forward)
|
||||
side = np.cross(normal, forward)
|
||||
side_length = float(np.linalg.norm(side))
|
||||
if side_length < 1e-6:
|
||||
return None
|
||||
side = side / side_length
|
||||
return origin, np.column_stack([forward, side, normal])
|
||||
|
||||
|
||||
class FixtureOverlay:
|
||||
def __init__(self, workspace: Path):
|
||||
config = load_camera_station(workspace)
|
||||
files = load_camera_files(workspace)
|
||||
plate = config["fixture_plate"]
|
||||
self.tag_size_m = float(config["tag_size_m"])
|
||||
self.plate_camera = str(plate["camera"])
|
||||
self.tag_ids = [int(tag_id) for tag_id in plate["tag_ids"]]
|
||||
self.x_from = int(plate["x_from"])
|
||||
self.x_to = int(plate["x_to"])
|
||||
self.cameras = {
|
||||
view: load_pinhole(path) for view, path in files.intrinsics.items()
|
||||
}
|
||||
self.front_from_view = files.front_from_view
|
||||
self._plate: tuple | None = None
|
||||
import apriltag
|
||||
self.detector = apriltag.apriltag("tag36h11")
|
||||
|
||||
def overlay(self, view: str, rgb: bytes, width: int, height: int):
|
||||
if view == self.plate_camera:
|
||||
self._detect_plate(rgb, width, height)
|
||||
return self._fixture_axes(view), self._markers(view)
|
||||
|
||||
def _detect_plate(self, rgb: bytes, width: int, height: int) -> None:
|
||||
matrix, distortion = self.cameras[self.plate_camera]
|
||||
gray = np.frombuffer(rgb, dtype=np.uint8).reshape(height, width, 3)[:, :, 0].copy()
|
||||
found = {}
|
||||
for detection in self.detector.detect(gray):
|
||||
tag_id = int(detection["id"])
|
||||
if tag_id not in self.tag_ids:
|
||||
continue
|
||||
corners = np.asarray(detection["lb-rb-rt-lt"], dtype=np.float64).reshape(4, 2)
|
||||
pose = tag_pose(
|
||||
corners, tag_size_m=self.tag_size_m, camera_matrix=matrix, distortion=distortion,
|
||||
)
|
||||
if pose is not None:
|
||||
found[tag_id] = (pose[1], corners)
|
||||
if set(found) >= set(self.tag_ids):
|
||||
self._plate = (
|
||||
time.monotonic(),
|
||||
{tag_id: found[tag_id][0] for tag_id in self.tag_ids},
|
||||
{tag_id: found[tag_id][1] for tag_id in self.tag_ids},
|
||||
)
|
||||
|
||||
def _fresh_plate(self):
|
||||
if self._plate is None or time.monotonic() - self._plate[0] > POSE_FRESH_SEC:
|
||||
return None
|
||||
return self._plate
|
||||
|
||||
def _markers(self, view: str):
|
||||
plate = self._fresh_plate()
|
||||
if plate is None or view != self.plate_camera:
|
||||
return []
|
||||
return [(corners, str(tag_id)) for tag_id, corners in plate[2].items()]
|
||||
|
||||
def _fixture_axes(self, view: str):
|
||||
plate = self._fresh_plate()
|
||||
if plate is None or view not in self.cameras:
|
||||
return None
|
||||
solved = plate_frame(plate[1], x_from=self.x_from, x_to=self.x_to)
|
||||
if solved is None:
|
||||
return None
|
||||
origin_top, rotation_top = solved
|
||||
view_from_top = np.linalg.inv(self.front_from_view[view]) @ self.front_from_view[self.plate_camera]
|
||||
origin = view_from_top[:3, :3] @ origin_top + view_from_top[:3, 3]
|
||||
rotation = view_from_top[:3, :3] @ rotation_top
|
||||
matrix, distortion = self.cameras[view]
|
||||
return project_frame(origin, rotation, matrix, distortion)
|
||||
@@ -0,0 +1,54 @@
|
||||
"""把掌心中的关节坐标系画进相机图像。工装位姿未知时不画。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def joint_frame_in_camera(
|
||||
origin_palm: np.ndarray,
|
||||
rotation_palm: np.ndarray,
|
||||
*,
|
||||
fixture_from_hand: np.ndarray,
|
||||
front_from_fixture: np.ndarray,
|
||||
camera_from_front: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
palm_from_joint = np.eye(4)
|
||||
palm_from_joint[:3, :3] = rotation_palm
|
||||
palm_from_joint[:3, 3] = origin_palm
|
||||
camera_from_palm = camera_from_front @ front_from_fixture @ fixture_from_hand
|
||||
frame = camera_from_palm @ palm_from_joint
|
||||
return frame[:3, 3].copy(), frame[:3, :3].copy()
|
||||
|
||||
|
||||
def project_axes(
|
||||
origin_camera: np.ndarray,
|
||||
rotation_camera: np.ndarray,
|
||||
camera_matrix: np.ndarray,
|
||||
*,
|
||||
axis_length_m: float = 0.015,
|
||||
) -> dict[str, tuple[tuple[int, int], tuple[int, int]]] | None:
|
||||
"""返回 x/y/z 两端的像素。原点在相机背后时不画。"""
|
||||
if origin_camera[2] <= 1e-6:
|
||||
return None
|
||||
columns = {"x": 0, "y": 1, "z": 2}
|
||||
result = {}
|
||||
start = _project(origin_camera, camera_matrix)
|
||||
if start is None:
|
||||
return None
|
||||
for name, column in columns.items():
|
||||
tip = origin_camera + rotation_camera[:, column] * axis_length_m
|
||||
if tip[2] <= 1e-6:
|
||||
return None
|
||||
end = _project(tip, camera_matrix)
|
||||
if end is None:
|
||||
return None
|
||||
result[name] = (start, end)
|
||||
return result
|
||||
|
||||
|
||||
def _project(point: np.ndarray, camera_matrix: np.ndarray) -> tuple[int, int] | None:
|
||||
if point[2] <= 1e-9:
|
||||
return None
|
||||
pixel = camera_matrix @ (point / point[2])
|
||||
return int(round(pixel[0])), int(round(pixel[1]))
|
||||
@@ -0,0 +1,674 @@
|
||||
"""ROS 2 image publisher for Hikrobot MVS USB3 Vision cameras."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from ctypes import byref, cast, memset, POINTER, sizeof, string_at
|
||||
from dataclasses import dataclass
|
||||
import importlib
|
||||
import os
|
||||
from pathlib import Path
|
||||
import sys
|
||||
from threading import Event, Lock, Thread
|
||||
from typing import Any, Sequence
|
||||
from urllib.parse import unquote, urlparse
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from sensor_msgs.srv import SetCameraInfo
|
||||
import yaml
|
||||
|
||||
|
||||
DEFAULT_MVS_PYTHON_PATH = "/opt/MVS/Samples/64/Python/MvImport"
|
||||
DEFAULT_MVS_RUNTIME_PATH = "/opt/MVS/lib"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeviceDescriptor:
|
||||
"""Stable identity fields for one enumerated USB3 Vision camera."""
|
||||
|
||||
index: int
|
||||
model: str
|
||||
serial: str
|
||||
guid: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraCalibration:
|
||||
"""Validated subset of a standard ROS camera calibration YAML file."""
|
||||
|
||||
camera_name: str
|
||||
width: int
|
||||
height: int
|
||||
distortion_model: str
|
||||
d: tuple[float, ...]
|
||||
k: tuple[float, ...]
|
||||
r: tuple[float, ...]
|
||||
p: tuple[float, ...]
|
||||
|
||||
|
||||
def decode_c_string(value: Any) -> str:
|
||||
"""Decode a null-terminated MVS SDK character array."""
|
||||
raw = bytes(value).split(b"\0", 1)[0]
|
||||
for encoding in ("utf-8", "gbk", "latin-1"):
|
||||
try:
|
||||
return raw.decode(encoding)
|
||||
except UnicodeDecodeError:
|
||||
continue
|
||||
return raw.decode("latin-1", errors="replace")
|
||||
|
||||
|
||||
def select_device(
|
||||
devices: Sequence[DeviceDescriptor],
|
||||
selector: str,
|
||||
expected_model: str,
|
||||
) -> DeviceDescriptor:
|
||||
"""Select exactly one camera by serial or GUID and check its model."""
|
||||
if not devices:
|
||||
raise RuntimeError("MVS did not enumerate any USB3 Vision cameras")
|
||||
selector = selector.strip()
|
||||
if selector:
|
||||
matches = [
|
||||
device
|
||||
for device in devices
|
||||
if selector in {device.serial, device.guid}
|
||||
]
|
||||
if not matches:
|
||||
available = ", ".join(
|
||||
f"{device.model}:{device.serial}:{device.guid}"
|
||||
for device in devices
|
||||
)
|
||||
raise RuntimeError(
|
||||
f"camera selector {selector!r} was not found; "
|
||||
f"available cameras: {available}"
|
||||
)
|
||||
elif len(devices) == 1:
|
||||
matches = [devices[0]]
|
||||
else:
|
||||
available = ", ".join(device.serial for device in devices)
|
||||
raise RuntimeError(
|
||||
"camera selector is required when multiple cameras are present; "
|
||||
f"available serials: {available}"
|
||||
)
|
||||
if len(matches) != 1:
|
||||
raise RuntimeError(f"camera selector {selector!r} is not unique")
|
||||
selected = matches[0]
|
||||
if expected_model and expected_model not in selected.model:
|
||||
raise RuntimeError(
|
||||
f"camera {selected.serial} is {selected.model!r}, expected a model "
|
||||
f"containing {expected_model!r}"
|
||||
)
|
||||
return selected
|
||||
|
||||
|
||||
def resolve_camera_info_path(url_or_path: str) -> Path | None:
|
||||
"""Resolve a plain path or file:// URL used for CameraInfo persistence."""
|
||||
value = url_or_path.strip()
|
||||
if not value:
|
||||
return None
|
||||
parsed = urlparse(value)
|
||||
if parsed.scheme not in {"", "file"}:
|
||||
raise ValueError(
|
||||
"camera_info_url must be a filesystem path or file:// URL"
|
||||
)
|
||||
if parsed.scheme == "file":
|
||||
if parsed.netloc not in {"", "localhost"}:
|
||||
raise ValueError("remote file:// camera_info_url is not supported")
|
||||
value = unquote(parsed.path)
|
||||
return Path(value).expanduser().resolve()
|
||||
|
||||
|
||||
def _matrix_data(mapping: dict[str, Any], name: str, length: int) -> tuple[float, ...]:
|
||||
section = mapping.get(name)
|
||||
if not isinstance(section, dict):
|
||||
raise ValueError(f"camera calibration is missing {name}")
|
||||
data = section.get("data")
|
||||
if not isinstance(data, list) or len(data) != length:
|
||||
raise ValueError(f"{name}.data must contain exactly {length} values")
|
||||
result = tuple(float(value) for value in data)
|
||||
if not all(value == value and abs(value) != float("inf") for value in result):
|
||||
raise ValueError(f"{name}.data contains a non-finite value")
|
||||
return result
|
||||
|
||||
|
||||
def load_camera_calibration(path: Path) -> CameraCalibration:
|
||||
"""Load and validate a standard ROS camera calibration YAML file."""
|
||||
mapping = yaml.safe_load(path.read_text(encoding="utf-8"))
|
||||
if not isinstance(mapping, dict):
|
||||
raise ValueError("camera calibration YAML root must be a mapping")
|
||||
width = int(mapping.get("image_width", 0))
|
||||
height = int(mapping.get("image_height", 0))
|
||||
if width <= 0 or height <= 0:
|
||||
raise ValueError("camera calibration image dimensions must be positive")
|
||||
distortion = mapping.get("distortion_coefficients")
|
||||
if not isinstance(distortion, dict) or not isinstance(
|
||||
distortion.get("data"), list
|
||||
):
|
||||
raise ValueError(
|
||||
"camera calibration is missing distortion_coefficients.data"
|
||||
)
|
||||
d = tuple(float(value) for value in distortion["data"])
|
||||
calibration = CameraCalibration(
|
||||
camera_name=str(mapping.get("camera_name", "hikrobot_camera")),
|
||||
width=width,
|
||||
height=height,
|
||||
distortion_model=str(mapping.get("distortion_model", "plumb_bob")),
|
||||
d=d,
|
||||
k=_matrix_data(mapping, "camera_matrix", 9),
|
||||
r=_matrix_data(mapping, "rectification_matrix", 9),
|
||||
p=_matrix_data(mapping, "projection_matrix", 12),
|
||||
)
|
||||
if calibration.k[0] <= 0.0 or calibration.k[4] <= 0.0:
|
||||
raise ValueError("camera calibration focal lengths must be positive")
|
||||
if calibration.p[0] <= 0.0 or calibration.p[5] <= 0.0:
|
||||
raise ValueError("camera calibration projection focal lengths must be positive")
|
||||
return calibration
|
||||
|
||||
|
||||
def calibration_to_mapping(
|
||||
camera_name: str, camera_info: CameraInfo
|
||||
) -> dict[str, Any]:
|
||||
"""Convert CameraInfo into the standard ROS YAML representation."""
|
||||
return {
|
||||
"image_width": int(camera_info.width),
|
||||
"image_height": int(camera_info.height),
|
||||
"camera_name": camera_name,
|
||||
"camera_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"data": [float(value) for value in camera_info.k],
|
||||
},
|
||||
"distortion_model": camera_info.distortion_model,
|
||||
"distortion_coefficients": {
|
||||
"rows": 1,
|
||||
"cols": len(camera_info.d),
|
||||
"data": [float(value) for value in camera_info.d],
|
||||
},
|
||||
"rectification_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"data": [float(value) for value in camera_info.r],
|
||||
},
|
||||
"projection_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 4,
|
||||
"data": [float(value) for value in camera_info.p],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def configure_fastdds_large_image_transport() -> Path:
|
||||
"""Select the large-image SHM profile before creating ROS nodes."""
|
||||
profile = (
|
||||
Path(__file__).resolve().parents[3]
|
||||
/ "camera_parameters"
|
||||
/ "config"
|
||||
/ "fastdds_large_images.xml"
|
||||
)
|
||||
if not profile.is_file():
|
||||
raise RuntimeError(f"Fast DDS large-image profile not found: {profile}")
|
||||
os.environ.setdefault("FASTRTPS_DEFAULT_PROFILES_FILE", str(profile))
|
||||
return profile
|
||||
|
||||
|
||||
class HikrobotCameraNode(Node):
|
||||
"""Publish synchronized Mono8 Image and CameraInfo messages from MVS."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("hikrobot_camera")
|
||||
self._declare_parameters()
|
||||
self._stop_event = Event()
|
||||
self._camera_info_lock = Lock()
|
||||
self._grab_thread: Thread | None = None
|
||||
self._camera: Any | None = None
|
||||
self._mvs: Any | None = None
|
||||
self._sdk_initialized = False
|
||||
self._handle_created = False
|
||||
self._camera_opened = False
|
||||
self._grabbing = False
|
||||
self._closed = False
|
||||
self._frame_count = 0
|
||||
self._timeout_count = 0
|
||||
self._camera_timing = None
|
||||
|
||||
self.selector = str(self.get_parameter("serial_number").value)
|
||||
self.expected_model = str(self.get_parameter("expected_model").value)
|
||||
self.frame_id = str(self.get_parameter("frame_id").value)
|
||||
self.camera_name = str(self.get_parameter("camera_name").value)
|
||||
self.image_width = int(self.get_parameter("image_width").value)
|
||||
self.image_height = int(self.get_parameter("image_height").value)
|
||||
if self.image_width <= 0 or self.image_height <= 0:
|
||||
raise ValueError("image_width and image_height must be positive")
|
||||
if float(self.get_parameter("frame_rate").value) <= 0.0:
|
||||
raise ValueError("frame_rate must be positive")
|
||||
if int(self.get_parameter("grab_timeout_ms").value) <= 0:
|
||||
raise ValueError("grab_timeout_ms must be positive")
|
||||
self.camera_info_path = resolve_camera_info_path(
|
||||
str(self.get_parameter("camera_info_url").value)
|
||||
)
|
||||
self._calibration = self._load_calibration()
|
||||
|
||||
self.image_publisher = self.create_publisher(
|
||||
Image, "image_raw", qos_profile_sensor_data
|
||||
)
|
||||
self.camera_info_publisher = self.create_publisher(
|
||||
CameraInfo, "camera_info", qos_profile_sensor_data
|
||||
)
|
||||
self.create_service(
|
||||
SetCameraInfo, "set_camera_info", self._set_camera_info_callback
|
||||
)
|
||||
|
||||
try:
|
||||
self._open_camera()
|
||||
self._grab_thread = Thread(
|
||||
target=self._grab_loop,
|
||||
name="hikrobot_mvs_grab",
|
||||
daemon=True,
|
||||
)
|
||||
self._grab_thread.start()
|
||||
except Exception:
|
||||
self.close()
|
||||
raise
|
||||
|
||||
def _declare_parameters(self) -> None:
|
||||
self.declare_parameter("serial_number", "")
|
||||
self.declare_parameter("expected_model", "MV-CS020-10UM")
|
||||
self.declare_parameter("camera_name", "hikrobot_front")
|
||||
self.declare_parameter("frame_id", "camera_color_optical_frame")
|
||||
self.declare_parameter("image_width", 1624)
|
||||
self.declare_parameter("image_height", 1240)
|
||||
self.declare_parameter("frame_rate", 30.0)
|
||||
self.declare_parameter("exposure_time_us", 5000.0)
|
||||
self.declare_parameter("gain_db", 0.0)
|
||||
self.declare_parameter("auto_exposure", False)
|
||||
self.declare_parameter("camera_info_url", "")
|
||||
self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH)
|
||||
self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH)
|
||||
self.declare_parameter("grab_timeout_ms", 1000)
|
||||
self.declare_parameter("timestamp_journal_path", "")
|
||||
|
||||
def _load_calibration(self) -> CameraCalibration | None:
|
||||
if self.camera_info_path is None or not self.camera_info_path.exists():
|
||||
target = self.camera_info_path or "an unset camera_info_url"
|
||||
self.get_logger().warning(
|
||||
f"No camera calibration found at {target}; publishing "
|
||||
"uncalibrated CameraInfo. Trajectory calibration will remain "
|
||||
"locked until valid intrinsics are installed."
|
||||
)
|
||||
return None
|
||||
calibration = load_camera_calibration(self.camera_info_path)
|
||||
self._validate_calibration_dimensions(calibration)
|
||||
self.get_logger().info(
|
||||
f"Loaded camera intrinsics from {self.camera_info_path}"
|
||||
)
|
||||
return calibration
|
||||
|
||||
def _validate_calibration_dimensions(
|
||||
self, calibration: CameraCalibration
|
||||
) -> None:
|
||||
if (
|
||||
calibration.width != self.image_width
|
||||
or calibration.height != self.image_height
|
||||
):
|
||||
raise ValueError(
|
||||
"camera calibration dimensions "
|
||||
f"{calibration.width}x{calibration.height} do not match "
|
||||
f"configured stream {self.image_width}x{self.image_height}"
|
||||
)
|
||||
|
||||
def _import_mvs(self) -> Any:
|
||||
python_path = Path(
|
||||
str(self.get_parameter("mvs_python_path").value)
|
||||
).expanduser()
|
||||
runtime_path = Path(
|
||||
str(self.get_parameter("mvs_runtime_path").value)
|
||||
).expanduser()
|
||||
if not python_path.is_dir():
|
||||
raise RuntimeError(f"MVS Python bindings not found: {python_path}")
|
||||
if not runtime_path.is_dir():
|
||||
raise RuntimeError(f"MVS runtime not found: {runtime_path}")
|
||||
os.environ.setdefault("MVCAM_COMMON_RUNENV", str(runtime_path))
|
||||
if str(python_path) not in sys.path:
|
||||
sys.path.insert(0, str(python_path))
|
||||
return importlib.import_module("MvCameraControl_class")
|
||||
|
||||
def _enumerate_usb_cameras(
|
||||
self, mvs: Any
|
||||
) -> tuple[Any, list[DeviceDescriptor]]:
|
||||
device_list = mvs.MV_CC_DEVICE_INFO_LIST()
|
||||
result = mvs.MvCamera.MV_CC_EnumDevices(
|
||||
mvs.MV_USB_DEVICE, device_list
|
||||
)
|
||||
self._require_ok("enumerate USB3 Vision devices", result)
|
||||
descriptors: list[DeviceDescriptor] = []
|
||||
for index in range(device_list.nDeviceNum):
|
||||
device_info = cast(
|
||||
device_list.pDeviceInfo[index], POINTER(mvs.MV_CC_DEVICE_INFO)
|
||||
).contents
|
||||
usb_info = device_info.SpecialInfo.stUsb3VInfo
|
||||
descriptors.append(
|
||||
DeviceDescriptor(
|
||||
index=index,
|
||||
model=decode_c_string(usb_info.chModelName),
|
||||
serial=decode_c_string(usb_info.chSerialNumber),
|
||||
guid=decode_c_string(usb_info.chDeviceGUID),
|
||||
)
|
||||
)
|
||||
return device_list, descriptors
|
||||
|
||||
def _open_camera(self) -> None:
|
||||
mvs = self._import_mvs()
|
||||
self._mvs = mvs
|
||||
self._require_ok("initialize MVS SDK", mvs.MvCamera.MV_CC_Initialize())
|
||||
self._sdk_initialized = True
|
||||
device_list, descriptors = self._enumerate_usb_cameras(mvs)
|
||||
selected = select_device(descriptors, self.selector, self.expected_model)
|
||||
device_info = cast(
|
||||
device_list.pDeviceInfo[selected.index],
|
||||
POINTER(mvs.MV_CC_DEVICE_INFO),
|
||||
).contents
|
||||
|
||||
camera = mvs.MvCamera()
|
||||
self._require_ok("create camera handle", camera.MV_CC_CreateHandle(device_info))
|
||||
self._camera = camera
|
||||
self._handle_created = True
|
||||
self._require_ok(
|
||||
"open camera",
|
||||
camera.MV_CC_OpenDevice(mvs.MV_ACCESS_Exclusive, 0),
|
||||
)
|
||||
self._camera_opened = True
|
||||
|
||||
self._set_required_enum("AcquisitionMode", "Continuous")
|
||||
self._set_required_enum("TriggerMode", "Off")
|
||||
self._set_required_enum("PixelFormat", "Mono8")
|
||||
self._set_required_integer("OffsetX", 0)
|
||||
self._set_required_integer("OffsetY", 0)
|
||||
self._set_required_integer("Width", self.image_width)
|
||||
self._set_required_integer("Height", self.image_height)
|
||||
self._set_optional_bool("AcquisitionFrameRateEnable", True)
|
||||
self._set_optional_float(
|
||||
"AcquisitionFrameRate",
|
||||
float(self.get_parameter("frame_rate").value),
|
||||
)
|
||||
if bool(self.get_parameter("auto_exposure").value):
|
||||
self._set_required_enum("ExposureAuto", "Continuous")
|
||||
else:
|
||||
self._set_required_enum("ExposureAuto", "Off")
|
||||
self._set_optional_float(
|
||||
"ExposureTime",
|
||||
float(self.get_parameter("exposure_time_us").value),
|
||||
)
|
||||
self._set_required_enum("GainAuto", "Off")
|
||||
self._set_optional_float(
|
||||
"Gain", float(self.get_parameter("gain_db").value)
|
||||
)
|
||||
|
||||
self._require_ok(
|
||||
"set latest-frame grab strategy",
|
||||
camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly),
|
||||
)
|
||||
from .camera_timing import MvsCameraTiming
|
||||
self._camera_timing = MvsCameraTiming(camera, mvs,
|
||||
lambda: int(self.get_clock().now().nanoseconds),
|
||||
exposure_us=0. if bool(self.get_parameter("auto_exposure").value)
|
||||
else float(self.get_parameter("exposure_time_us").value),
|
||||
journal_path=str(self.get_parameter("timestamp_journal_path").value))
|
||||
self._camera_timing.start()
|
||||
self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing())
|
||||
self._grabbing = True
|
||||
self.get_logger().info(
|
||||
"Opened Hikrobot camera "
|
||||
f"model={selected.model} serial={selected.serial} "
|
||||
f"guid={selected.guid} stream={self.image_width}x{self.image_height} "
|
||||
"encoding=mono8"
|
||||
)
|
||||
|
||||
def _set_required_enum(self, name: str, value: str) -> None:
|
||||
assert self._camera is not None
|
||||
self._require_ok(
|
||||
f"set {name}={value}",
|
||||
self._camera.MV_CC_SetEnumValueByString(name, value),
|
||||
)
|
||||
|
||||
def _set_required_integer(self, name: str, value: int) -> None:
|
||||
assert self._camera is not None
|
||||
self._require_ok(
|
||||
f"set {name}={value}",
|
||||
self._camera.MV_CC_SetIntValueEx(name, value),
|
||||
)
|
||||
|
||||
def _set_optional_bool(self, name: str, value: bool) -> None:
|
||||
assert self._camera is not None
|
||||
result = self._camera.MV_CC_SetBoolValue(name, value)
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS could not set {name}={value}: 0x{result:08x}"
|
||||
)
|
||||
|
||||
def _set_optional_float(self, name: str, value: float) -> None:
|
||||
assert self._camera is not None
|
||||
result = self._camera.MV_CC_SetFloatValue(name, value)
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS could not set {name}={value}: 0x{result:08x}"
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _require_ok(operation: str, result: int) -> None:
|
||||
if result != 0:
|
||||
hint = ""
|
||||
if result == 0x80000203:
|
||||
hint = (
|
||||
"; access denied: stop acquisition and close the MVS "
|
||||
"viewer or any other process using this camera"
|
||||
)
|
||||
raise RuntimeError(
|
||||
f"MVS failed to {operation}: 0x{result:08x}{hint}"
|
||||
)
|
||||
|
||||
def _grab_loop(self) -> None:
|
||||
assert self._camera is not None
|
||||
assert self._mvs is not None
|
||||
timeout_ms = int(self.get_parameter("grab_timeout_ms").value)
|
||||
recovering_clock = False
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
self._camera_timing.refresh_if_due()
|
||||
except ValueError as error:
|
||||
if not recovering_clock:
|
||||
self.get_logger().error(
|
||||
f"Camera timestamp source failed: {error}; "
|
||||
"pausing images and retrying clock synchronization"
|
||||
)
|
||||
self._camera_timing.invalidate(error)
|
||||
recovering_clock = True
|
||||
self._stop_event.wait(0.2)
|
||||
continue
|
||||
if recovering_clock:
|
||||
self.get_logger().info(
|
||||
"Camera clock synchronized again; resuming image acquisition"
|
||||
)
|
||||
recovering_clock = False
|
||||
frame = self._mvs.MV_FRAME_OUT()
|
||||
memset(byref(frame), 0, sizeof(frame))
|
||||
result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms)
|
||||
if result != 0:
|
||||
self._timeout_count += 1
|
||||
if self._timeout_count == 1 or self._timeout_count % 10 == 0:
|
||||
self.get_logger().warning(
|
||||
"MVS image acquisition timed out or failed: "
|
||||
f"0x{result:08x}; consecutive={self._timeout_count}"
|
||||
)
|
||||
continue
|
||||
try:
|
||||
self._timeout_count = 0
|
||||
self._publish_frame(frame)
|
||||
except Exception as error:
|
||||
self.get_logger().error(f"Failed to publish MVS frame: {error}")
|
||||
finally:
|
||||
free_result = self._camera.MV_CC_FreeImageBuffer(frame)
|
||||
if free_result != 0:
|
||||
self.get_logger().error(
|
||||
f"MVS failed to release frame: 0x{free_result:08x}"
|
||||
)
|
||||
|
||||
def _publish_frame(self, frame: Any) -> None:
|
||||
assert self._mvs is not None
|
||||
info = frame.stFrameInfo
|
||||
width = int(info.nWidth)
|
||||
height = int(info.nHeight)
|
||||
if width != self.image_width or height != self.image_height:
|
||||
raise RuntimeError(
|
||||
f"received {width}x{height}, expected "
|
||||
f"{self.image_width}x{self.image_height}"
|
||||
)
|
||||
if int(info.enPixelType) != int(self._mvs.PixelType_Gvsp_Mono8):
|
||||
raise RuntimeError(
|
||||
f"received pixel type 0x{int(info.enPixelType):x}, expected Mono8"
|
||||
)
|
||||
expected_size = width * height
|
||||
if int(info.nFrameLen) < expected_size:
|
||||
raise RuntimeError(
|
||||
f"frame contains {info.nFrameLen} bytes, expected {expected_size}"
|
||||
)
|
||||
from rclpy.time import Time
|
||||
received_ns = int(self.get_clock().now().nanoseconds)
|
||||
stamp = Time(nanoseconds=self._camera_timing.timestamp(info, received_ns)).to_msg()
|
||||
image = Image()
|
||||
image.header.stamp = stamp
|
||||
image.header.frame_id = self.frame_id
|
||||
image.height = height
|
||||
image.width = width
|
||||
image.encoding = "mono8"
|
||||
image.is_bigendian = 0
|
||||
image.step = width
|
||||
image.data = string_at(frame.pBufAddr, expected_size)
|
||||
|
||||
camera_info = self._make_camera_info(width, height)
|
||||
camera_info.header.stamp = stamp
|
||||
camera_info.header.frame_id = self.frame_id
|
||||
self.image_publisher.publish(image)
|
||||
self.camera_info_publisher.publish(camera_info)
|
||||
self._frame_count += 1
|
||||
|
||||
def _make_camera_info(self, width: int, height: int) -> CameraInfo:
|
||||
message = CameraInfo()
|
||||
message.width = width
|
||||
message.height = height
|
||||
with self._camera_info_lock:
|
||||
calibration = self._calibration
|
||||
if calibration is None:
|
||||
message.distortion_model = "plumb_bob"
|
||||
message.d = []
|
||||
message.k = [0.0] * 9
|
||||
message.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||
message.p = [0.0] * 12
|
||||
return message
|
||||
message.distortion_model = calibration.distortion_model
|
||||
message.d = list(calibration.d)
|
||||
message.k = list(calibration.k)
|
||||
message.r = list(calibration.r)
|
||||
message.p = list(calibration.p)
|
||||
return message
|
||||
|
||||
def _set_camera_info_callback(
|
||||
self,
|
||||
request: SetCameraInfo.Request,
|
||||
response: SetCameraInfo.Response,
|
||||
) -> SetCameraInfo.Response:
|
||||
try:
|
||||
if self.camera_info_path is None:
|
||||
raise ValueError(
|
||||
"camera_info_url is empty; set it before saving calibration"
|
||||
)
|
||||
message = request.camera_info
|
||||
if (
|
||||
int(message.width) != self.image_width
|
||||
or int(message.height) != self.image_height
|
||||
):
|
||||
raise ValueError(
|
||||
f"calibration is {message.width}x{message.height}, expected "
|
||||
f"{self.image_width}x{self.image_height}"
|
||||
)
|
||||
mapping = calibration_to_mapping(self.camera_name, message)
|
||||
self.camera_info_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary_path = self.camera_info_path.with_suffix(
|
||||
self.camera_info_path.suffix + ".tmp"
|
||||
)
|
||||
temporary_path.write_text(
|
||||
yaml.safe_dump(mapping, sort_keys=False), encoding="utf-8"
|
||||
)
|
||||
temporary_path.replace(self.camera_info_path)
|
||||
calibration = load_camera_calibration(self.camera_info_path)
|
||||
self._validate_calibration_dimensions(calibration)
|
||||
with self._camera_info_lock:
|
||||
self._calibration = calibration
|
||||
response.success = True
|
||||
response.status_message = (
|
||||
f"saved camera calibration to {self.camera_info_path}"
|
||||
)
|
||||
self.get_logger().info(response.status_message)
|
||||
except Exception as error:
|
||||
response.success = False
|
||||
response.status_message = str(error)
|
||||
self.get_logger().error(
|
||||
f"Rejected camera calibration: {response.status_message}"
|
||||
)
|
||||
return response
|
||||
|
||||
def close(self) -> None:
|
||||
if self._closed:
|
||||
return
|
||||
self._closed = True
|
||||
self._stop_event.set()
|
||||
if self._grab_thread is not None:
|
||||
self._grab_thread.join(timeout=2.0)
|
||||
if self._camera_timing is not None:
|
||||
self._camera_timing.close()
|
||||
if self._camera is not None and self._camera_opened:
|
||||
if self._grabbing:
|
||||
result = self._camera.MV_CC_StopGrabbing()
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS failed to stop grabbing: 0x{result:08x}"
|
||||
)
|
||||
self._grabbing = False
|
||||
result = self._camera.MV_CC_CloseDevice()
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS failed to close camera: 0x{result:08x}"
|
||||
)
|
||||
self._camera.MV_CC_DestroyHandle()
|
||||
elif self._camera is not None and self._handle_created:
|
||||
self._camera.MV_CC_DestroyHandle()
|
||||
self._camera = None
|
||||
self._handle_created = False
|
||||
self._camera_opened = False
|
||||
if self._sdk_initialized and self._mvs is not None:
|
||||
self._mvs.MvCamera.MV_CC_Finalize()
|
||||
self._sdk_initialized = False
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self.close()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
configure_fastdds_large_image_transport()
|
||||
rclpy.init(args=args)
|
||||
node: HikrobotCameraNode | None = None
|
||||
try:
|
||||
node = HikrobotCameraNode()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,60 @@
|
||||
"""把每个转动关节的坐标系投到相机里。旋转轴比另外两根轴更粗。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from calibration.acquisition.fixture import load_fixture
|
||||
from calibration.cameras.files import load_camera_files
|
||||
from calibration.cameras.fixture_overlay import load_pinhole, project_frame
|
||||
from calibration.cameras.frames import joint_frame_in_camera
|
||||
from calibration.urdf_model import UrdfKinematicModel
|
||||
|
||||
|
||||
AXIS_NAMES = ("x", "y", "z")
|
||||
JOINT_AXIS_LENGTH_M = 0.015
|
||||
|
||||
|
||||
def rotation_axis_name(axis_in_joint: np.ndarray) -> str:
|
||||
"""和 URDF 轴重合的那一根坐标轴。反向轴仍然是这一根。"""
|
||||
axis = np.abs(np.asarray(axis_in_joint, dtype=float))
|
||||
return AXIS_NAMES[int(np.argmax(axis))]
|
||||
|
||||
|
||||
class JointOverlay:
|
||||
def __init__(self, hand, workspace: Path):
|
||||
self.hand = hand
|
||||
self.model = UrdfKinematicModel(hand.urdf_path)
|
||||
self.joints = [spec.name for spec in hand.measurement_order()]
|
||||
self.fixture_from_hand = load_fixture(hand.product)
|
||||
front = hand.product.get("front_from_fixture")
|
||||
self.front_from_fixture = None if front is None else np.asarray(front, dtype=float)
|
||||
files = load_camera_files(workspace)
|
||||
self.front_from_view = files.front_from_view
|
||||
self.cameras = {view: load_pinhole(path) for view, path in files.intrinsics.items()}
|
||||
|
||||
def frames(self, view: str, zero_offsets: dict[str, float] | None = None) -> list[dict]:
|
||||
if self.front_from_fixture is None or view not in self.cameras or view not in self.front_from_view:
|
||||
return []
|
||||
offsets = {} if zero_offsets is None else zero_offsets
|
||||
matrix, distortion = self.cameras[view]
|
||||
camera_from_front = np.linalg.inv(self.front_from_view[view])
|
||||
drawn = []
|
||||
for name in self.joints:
|
||||
origin, rotation, axis = self.model.joint_frame(name, zero_offsets=offsets, joint_angles={})
|
||||
camera_origin, camera_rotation = joint_frame_in_camera(
|
||||
origin, rotation,
|
||||
fixture_from_hand=self.fixture_from_hand,
|
||||
front_from_fixture=self.front_from_fixture,
|
||||
camera_from_front=camera_from_front,
|
||||
)
|
||||
projected = project_frame(
|
||||
camera_origin, camera_rotation, matrix, distortion,
|
||||
axis_length_m=JOINT_AXIS_LENGTH_M,
|
||||
)
|
||||
if projected is None:
|
||||
continue
|
||||
drawn.append({"segments": projected, "rotation_axis": rotation_axis_name(axis)})
|
||||
return drawn
|
||||
@@ -0,0 +1,185 @@
|
||||
"""按行程标定同一方式拉起三台相机,并在本进程里接收图像。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import queue
|
||||
import signal
|
||||
import subprocess
|
||||
import threading
|
||||
from pathlib import Path
|
||||
|
||||
from calibration.cameras.files import load_camera_files, load_camera_station
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
def camera_commands(workspace: Path, camera_config: dict) -> list[tuple[str, str]]:
|
||||
"""返回 (视角, bash 命令)。内参和外参都来自 output_camera。"""
|
||||
files = load_camera_files(workspace)
|
||||
cameras = files.document.get("cameras") or {}
|
||||
setup = Path(str(camera_config["setup"]))
|
||||
frame_rate = float(camera_config.get("frame_rate", 30.0))
|
||||
exposure = float(camera_config.get("exposure_time_us", 5000.0))
|
||||
commands = []
|
||||
for view in VIEWS:
|
||||
camera = cameras.get(view)
|
||||
if not isinstance(camera, dict) or not camera.get("serial_number"):
|
||||
raise ValueError(f"外参缺少 {view} 相机序列号")
|
||||
serial = str(camera["serial_number"])
|
||||
intrinsics = files.intrinsics[view]
|
||||
width = int(camera.get("width", 1624))
|
||||
height = int(camera.get("height", 1240))
|
||||
parameters = " ".join([
|
||||
f"-p serial_number:={serial}",
|
||||
f"-p camera_name:=hikrobot_{serial}",
|
||||
f"-p frame_id:=calibration_{view}_optical_frame",
|
||||
f"-p image_width:={width}",
|
||||
f"-p image_height:={height}",
|
||||
f"-p camera_info_url:={intrinsics.resolve().as_uri()}",
|
||||
f"-p frame_rate:={frame_rate}",
|
||||
f"-p exposure_time_us:={exposure}",
|
||||
"-p gain_db:=0.0",
|
||||
"-p auto_exposure:=false",
|
||||
])
|
||||
script = (
|
||||
"set +u\n"
|
||||
'_ament="$AMENT_PREFIX_PATH"\n'
|
||||
'_cmake="$CMAKE_PREFIX_PATH"\n'
|
||||
'_path="$PATH"\n'
|
||||
'_ld="$LD_LIBRARY_PATH"\n'
|
||||
f'source "{setup}"\n'
|
||||
'export AMENT_PREFIX_PATH="${_ament:+$_ament:}$AMENT_PREFIX_PATH"\n'
|
||||
'export CMAKE_PREFIX_PATH="${_cmake:+$_cmake:}$CMAKE_PREFIX_PATH"\n'
|
||||
'export PATH="${_path:+$_path:}$PATH"\n'
|
||||
'export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:+$LD_LIBRARY_PATH:}$_ld"\n'
|
||||
"exec ros2 run camera_parameters hikrobot_camera_node "
|
||||
f"--ros-args -r __ns:=/calibration/{view}/camera {parameters}\n"
|
||||
)
|
||||
commands.append((view, script))
|
||||
return commands
|
||||
|
||||
|
||||
def _use_large_image_profile(workspace: Path) -> None:
|
||||
profile = workspace / "src" / "camera_parameters" / "config" / "fastdds_large_images.xml"
|
||||
if profile.is_file():
|
||||
os.environ.setdefault("FASTRTPS_DEFAULT_PROFILES_FILE", str(profile))
|
||||
|
||||
|
||||
class CameraRig:
|
||||
def __init__(self, workspace: Path):
|
||||
self.workspace = Path(workspace)
|
||||
self.processes: list[subprocess.Popen] = []
|
||||
self.frames: queue.Queue = queue.Queue(maxsize=6)
|
||||
self._latest_lock = threading.Lock()
|
||||
self._latest: dict[str, tuple] = {}
|
||||
self.executor = None
|
||||
self.node = None
|
||||
self._spin_thread: threading.Thread | None = None
|
||||
self._owns_rclpy = False
|
||||
self._preview_at: dict[str, float] = {}
|
||||
|
||||
def start(self) -> str:
|
||||
camera_config = load_camera_station(self.workspace)
|
||||
setup = Path(str(camera_config["setup"]))
|
||||
if not setup.is_file():
|
||||
raise FileNotFoundError(f"找不到相机驱动环境: {setup}")
|
||||
commands = camera_commands(self.workspace, camera_config)
|
||||
self._start_preview()
|
||||
log_dir = self.workspace / "output_urdf_calibration" / "cameras"
|
||||
log_dir.mkdir(parents=True, exist_ok=True)
|
||||
for view, script in commands:
|
||||
log_file = (log_dir / f"{view}.log").open("w", encoding="utf-8")
|
||||
self.processes.append(subprocess.Popen(
|
||||
["bash", "-lc", script], start_new_session=True,
|
||||
stdout=log_file, stderr=subprocess.STDOUT,
|
||||
))
|
||||
return "已拉起三台相机。工装位姿未知时只显示图像。"
|
||||
|
||||
def _start_preview(self) -> None:
|
||||
import rclpy
|
||||
from rclpy.executors import ExternalShutdownException, SingleThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
_use_large_image_profile(self.workspace)
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
self._owns_rclpy = True
|
||||
node = Node("calibration_camera_view")
|
||||
|
||||
def callback(view):
|
||||
def accept(message: Image):
|
||||
self._store_frame(view, message)
|
||||
return accept
|
||||
|
||||
for view in VIEWS:
|
||||
node.create_subscription(
|
||||
Image, f"/calibration/{view}/camera/image_raw", callback(view), qos_profile_sensor_data,
|
||||
)
|
||||
executor = SingleThreadedExecutor()
|
||||
executor.add_node(node)
|
||||
|
||||
def spin():
|
||||
try:
|
||||
executor.spin()
|
||||
except ExternalShutdownException:
|
||||
pass
|
||||
|
||||
self.node = node
|
||||
self.executor = executor
|
||||
self._spin_thread = threading.Thread(target=spin, daemon=True)
|
||||
self._spin_thread.start()
|
||||
|
||||
def _store_frame(self, view: str, message) -> None:
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
now = time.monotonic()
|
||||
if now - self._preview_at.get(view, float("-inf")) < 0.1:
|
||||
return
|
||||
if message.encoding != "mono8":
|
||||
return
|
||||
expected = int(message.width) * int(message.height)
|
||||
if len(message.data) < expected:
|
||||
return
|
||||
gray = np.frombuffer(message.data, dtype=np.uint8)[:expected].reshape(message.height, message.width)
|
||||
rgb = np.repeat(gray[:, :, None], 3, axis=2)
|
||||
payload = (int(message.width), int(message.height), rgb.tobytes())
|
||||
with self._latest_lock:
|
||||
self._latest[view] = payload
|
||||
self._preview_at[view] = now
|
||||
if self.frames.full():
|
||||
try:
|
||||
self.frames.get_nowait()
|
||||
except queue.Empty:
|
||||
pass
|
||||
self.frames.put((view, payload[0], payload[1], payload[2]))
|
||||
|
||||
def latest_frame(self, view: str):
|
||||
with self._latest_lock:
|
||||
return self._latest.get(view)
|
||||
|
||||
def stop(self) -> None:
|
||||
if self.executor is not None:
|
||||
self.executor.shutdown(timeout_sec=1)
|
||||
if self._spin_thread is not None:
|
||||
self._spin_thread.join(timeout=2)
|
||||
if self.node is not None:
|
||||
self.node.destroy_node()
|
||||
self.node = None
|
||||
if self._owns_rclpy:
|
||||
import rclpy
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
for process in self.processes:
|
||||
if process.poll() is not None:
|
||||
continue
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGINT)
|
||||
process.wait(timeout=5)
|
||||
except Exception:
|
||||
os.killpg(process.pid, signal.SIGKILL)
|
||||
process.wait(timeout=3)
|
||||
@@ -0,0 +1 @@
|
||||
"""用已知 URDF 轴拟合零位和行程,并写出授权字段。"""
|
||||
@@ -0,0 +1,45 @@
|
||||
"""去程和回程分开保留。零位取返回程在基准指令处的角度,行程取两侧外沿。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
MAX_AXIS_MISMATCH_RAD = math.radians(3.0)
|
||||
|
||||
|
||||
def _axis_angle(left: np.ndarray, right: np.ndarray) -> float:
|
||||
a = np.asarray(left, dtype=float)
|
||||
b = np.asarray(right, dtype=float)
|
||||
a = a / np.linalg.norm(a)
|
||||
b = b / np.linalg.norm(b)
|
||||
return float(math.acos(np.clip(np.dot(a, b), -1.0, 1.0)))
|
||||
|
||||
|
||||
def assert_sample_axis(sample_axis, model_axis) -> None:
|
||||
"""样本轴必须就是模型轴。超过约 3° 说明合成旋转没有绕着这根轴。"""
|
||||
if _axis_angle(sample_axis, model_axis) > MAX_AXIS_MISMATCH_RAD:
|
||||
raise ValueError("sample_axis_disagrees_with_urdf")
|
||||
|
||||
|
||||
def directional_envelope(samples: list[dict], *, baseline_u8: int) -> tuple[float, float, float]:
|
||||
"""返回 (zero_offset, lower, upper),单位弧度。两个方向不平均。"""
|
||||
grouped: dict[str, list[float]] = {"increasing": [], "decreasing": []}
|
||||
baseline_decreasing = []
|
||||
for sample in samples:
|
||||
direction = sample["direction"]
|
||||
theta = float(sample["theta_rad"])
|
||||
if not math.isfinite(theta) or direction not in grouped:
|
||||
raise ValueError("joint_sample_invalid")
|
||||
grouped[direction].append(theta)
|
||||
if direction == "decreasing" and int(sample["command_u8"]) == int(baseline_u8):
|
||||
baseline_decreasing.append(theta)
|
||||
if not grouped["increasing"] or not grouped["decreasing"] or not baseline_decreasing:
|
||||
raise ValueError("directional_samples_incomplete")
|
||||
zero = float(baseline_decreasing[-1])
|
||||
lower = min(min(grouped["increasing"]), min(grouped["decreasing"])) - zero
|
||||
upper = max(max(grouped["increasing"]), max(grouped["decreasing"])) - zero
|
||||
if lower > upper:
|
||||
raise ValueError("travel_envelope_empty")
|
||||
return zero, lower, upper
|
||||
@@ -0,0 +1,60 @@
|
||||
"""测量先落成 JSON。拟合只读这份文件,不读相机。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import re
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
from calibration.fitting.envelope import directional_envelope
|
||||
from calibration.fitting.urdf_out import write_authorized_urdf
|
||||
from calibration.session.errors import CalibrationStopped
|
||||
|
||||
|
||||
_TOKEN = re.compile(r"^[A-Za-z0-9_-]+$")
|
||||
|
||||
|
||||
def measurement_filename(model: str, side: str, device_uid: str, when: datetime) -> str:
|
||||
if side not in {"left", "right"}:
|
||||
raise CalibrationStopped("unknown_side")
|
||||
if not _TOKEN.fullmatch(str(model)):
|
||||
raise CalibrationStopped("model_invalid")
|
||||
if not isinstance(device_uid, str) or not _TOKEN.fullmatch(device_uid):
|
||||
raise CalibrationStopped("device_uid_invalid")
|
||||
return f"{model}_{side}_{device_uid}_{when.strftime('%Y%m%d_%H%M%S')}.json"
|
||||
|
||||
|
||||
def write_measurement_json(path: Path, document: dict) -> Path:
|
||||
destination = Path(path)
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
destination.write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
return destination
|
||||
|
||||
|
||||
def fit_urdf_from_json(path: Path, *, source_urdf: Path, authorized_fields: dict[str, list[str]]) -> Path:
|
||||
"""只根据 JSON 里的样本改 limit。origin offset 固定为 0。"""
|
||||
document = json.loads(Path(path).read_text(encoding="utf-8"))
|
||||
joints = document.get("joints")
|
||||
if not isinstance(joints, dict) or not joints:
|
||||
raise CalibrationStopped("measurement_json_invalid")
|
||||
limits = {}
|
||||
offsets = {}
|
||||
for name, block in joints.items():
|
||||
samples = block.get("samples")
|
||||
if not isinstance(samples, list):
|
||||
raise CalibrationStopped("measurement_json_invalid", name)
|
||||
try:
|
||||
_zero, lower, upper = directional_envelope(samples, baseline_u8=int(block["baseline_u8"]))
|
||||
except ValueError as error:
|
||||
raise CalibrationStopped(str(error), name) from error
|
||||
limits[name] = (lower, upper)
|
||||
offsets[name] = 0.0
|
||||
destination = Path(path).with_suffix(".urdf")
|
||||
return write_authorized_urdf(
|
||||
source_urdf=source_urdf,
|
||||
destination_urdf=destination,
|
||||
offsets_rad=offsets,
|
||||
limits_rad=limits,
|
||||
authorized_fields=authorized_fields,
|
||||
)
|
||||
@@ -0,0 +1,94 @@
|
||||
"""只改授权的 origin.rpy 和 limit。轴的 xyz 字符串保持与源文件相同。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import re
|
||||
from pathlib import Path
|
||||
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from calibration.urdf_model import UrdfKinematicModel, corrected_origin_rpy
|
||||
|
||||
|
||||
def _joint_block(text: str, name: str) -> tuple[int, int, str]:
|
||||
pattern = re.compile(
|
||||
rf'(<joint\b[^>]*\bname="{re.escape(name)}"[^>]*>.*?</joint>)',
|
||||
re.DOTALL,
|
||||
)
|
||||
match = pattern.search(text)
|
||||
if match is None:
|
||||
raise ValueError(f"source URDF is missing joint {name}")
|
||||
return match.start(), match.end(), match.group(1)
|
||||
|
||||
|
||||
def _replace_attribute(block: str, element: str, attribute: str, value: str) -> str:
|
||||
pattern = re.compile(
|
||||
rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])(?P<value>[^\"']*)\2",
|
||||
re.DOTALL,
|
||||
)
|
||||
match = pattern.search(block)
|
||||
if match is None:
|
||||
raise ValueError(f"{element} has no {attribute}")
|
||||
start, end = match.span("value")
|
||||
return block[:start] + value + block[end:]
|
||||
|
||||
|
||||
def write_authorized_urdf(
|
||||
*,
|
||||
source_urdf: Path,
|
||||
destination_urdf: Path,
|
||||
offsets_rad: dict[str, float],
|
||||
limits_rad: dict[str, tuple[float, float]],
|
||||
authorized_fields: dict[str, list[str]],
|
||||
) -> Path:
|
||||
"""按授权字段写出修正 URDF。未授权的 origin.rpy 与源文件相同。"""
|
||||
source = Path(source_urdf)
|
||||
text = source.read_text(encoding="utf-8")
|
||||
model = UrdfKinematicModel(source)
|
||||
axis_before = {
|
||||
name: re.search(r'<axis\b[^>]*\bxyz="([^"]*)"', block).group(1)
|
||||
for name, block in (
|
||||
(name, _joint_block(text, name)[2]) for name in limits_rad
|
||||
)
|
||||
}
|
||||
for joint, fields in authorized_fields.items():
|
||||
unknown = set(fields) - {"origin.rpy", "limit.lower", "limit.upper"}
|
||||
if unknown:
|
||||
raise ValueError(f"unauthorized field requested: {joint}:{sorted(unknown)}")
|
||||
pieces = text
|
||||
for joint, (lower, upper) in limits_rad.items():
|
||||
allowed = set(authorized_fields.get(joint, ()))
|
||||
if "limit.lower" not in allowed or "limit.upper" not in allowed:
|
||||
raise ValueError(f"limit is not authorized: {joint}")
|
||||
start, end, block = _joint_block(pieces, joint)
|
||||
offset = float(offsets_rad.get(joint, 0.0))
|
||||
if "origin.rpy" in allowed and abs(offset) > 1e-12:
|
||||
node = model.joints[joint]
|
||||
rpy = corrected_origin_rpy(node, offset)
|
||||
rendered = " ".join(f"{value:.15g}" for value in rpy)
|
||||
block = _replace_attribute(block, "origin", "rpy", rendered)
|
||||
elif abs(offset) > 1e-12:
|
||||
raise ValueError(f"origin.rpy is not authorized: {joint}")
|
||||
block = _replace_attribute(block, "limit", "lower", f"{lower:.15g}")
|
||||
block = _replace_attribute(block, "limit", "upper", f"{upper:.15g}")
|
||||
pieces = pieces[:start] + block + pieces[end:]
|
||||
for joint, original_axis in axis_before.items():
|
||||
_start, _end, block = _joint_block(pieces, joint)
|
||||
found = re.search(r'<axis\b[^>]*\bxyz="([^"]*)"', block)
|
||||
if found is None or found.group(1) != original_axis:
|
||||
raise ValueError(f"axis xyz changed: {joint}")
|
||||
destination = Path(destination_urdf)
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
destination.write_text(pieces, encoding="utf-8")
|
||||
return destination
|
||||
|
||||
|
||||
def sha256_file(path: Path) -> str:
|
||||
return hashlib.sha256(Path(path).read_bytes()).hexdigest()
|
||||
|
||||
|
||||
def rotation_matrix_unchanged(source_rpy: str, patched_rpy: str) -> bool:
|
||||
source = Rotation.from_euler("xyz", [float(v) for v in source_rpy.split()])
|
||||
patched = Rotation.from_euler("xyz", [float(v) for v in patched_rpy.split()])
|
||||
return float((source.inv() * patched).magnitude()) <= 1e-9
|
||||
@@ -0,0 +1 @@
|
||||
"""按所选手给出的顺序调用避让、采集和拟合。"""
|
||||
@@ -0,0 +1,8 @@
|
||||
"""标定停住时的原因。不写 JSON,也不写 URDF。"""
|
||||
|
||||
|
||||
class CalibrationStopped(Exception):
|
||||
def __init__(self, reason: str, joint: str = ""):
|
||||
self.reason = reason
|
||||
self.joint = joint
|
||||
super().__init__(reason if not joint else f"{joint}: {reason}")
|
||||
@@ -0,0 +1,200 @@
|
||||
"""按测量顺序避让、扫行程,并把活动贴纸的角点交给会话。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from calibration.acquisition.fixture import load_fixture
|
||||
from calibration.acquisition.gates import check_samples, reference_drift_px
|
||||
from calibration.cameras.files import load_camera_files
|
||||
from calibration.cameras.fixture_overlay import load_pinhole
|
||||
from calibration.session.errors import CalibrationStopped
|
||||
from calibration.session.runner import _joint_samples, run_calibration
|
||||
from calibration.urdf_model import UrdfKinematicModel
|
||||
from hands.o30.motion import fixed_tag_ids
|
||||
|
||||
|
||||
def run_live(hand, jog, cameras, *, side: str, progress=None, workspace: Path | None = None,
|
||||
offsets_callback=None) -> dict:
|
||||
if jog is None or not jog.ready():
|
||||
raise CalibrationStopped("command_not_ready")
|
||||
uid = str(getattr(hand, "device_uid", "") or "")
|
||||
from calibration.fitting.json_fit import measurement_filename
|
||||
|
||||
try:
|
||||
load_fixture(hand.product)
|
||||
except ValueError as error:
|
||||
raise CalibrationStopped(str(error)) from error
|
||||
if hand.product.get("front_from_fixture") is None:
|
||||
raise CalibrationStopped("fixture_pose_missing")
|
||||
measurement_filename(hand.model, side, uid, datetime.now())
|
||||
observations = capture_observations(
|
||||
hand, jog, cameras, progress=progress, offsets_callback=offsets_callback,
|
||||
workspace=workspace,
|
||||
)
|
||||
return run_calibration(
|
||||
hand, observations, device_uid=uid, side=side, progress=progress,
|
||||
workspace=workspace, offsets_callback=offsets_callback,
|
||||
)
|
||||
|
||||
|
||||
def capture_observations(hand, jog, cameras, *, progress=None, offsets_callback=None,
|
||||
workspace: Path | None = None) -> list[dict]:
|
||||
detector = _detector()
|
||||
acquisition = hand.profile["acquisition"]
|
||||
speed = hand.profile["motion"]["speed_parameters"]
|
||||
timeout = float(speed.get("stall_timeout_seconds", acquisition.get("stall_timeout_seconds", 4.0)))
|
||||
full_range_seconds = float(speed.get("command_trajectory_full_range_seconds", 13.0))
|
||||
minimum_frames = int(acquisition["fixed_reference_minimum_frames"])
|
||||
maximum_drift = float(acquisition["fixed_reference_maximum_drift_px"])
|
||||
observations = []
|
||||
completed: dict[str, float] = {}
|
||||
root = Path(workspace or getattr(cameras, "workspace", Path.cwd()))
|
||||
fixture_from_hand = load_fixture(hand.product)
|
||||
front_from_fixture = np.asarray(hand.product["front_from_fixture"], dtype=float)
|
||||
camera_files = load_camera_files(root)
|
||||
intrinsics = {view: load_pinhole(path) for view, path in camera_files.intrinsics.items()}
|
||||
model = UrdfKinematicModel(hand.urdf_path)
|
||||
order = hand.measurement_order()
|
||||
for index, spec in enumerate(order, start=1):
|
||||
missing = [name for name in spec.ancestors if name not in completed]
|
||||
if missing:
|
||||
raise CalibrationStopped("parent_zero_missing", spec.name)
|
||||
if progress is not None:
|
||||
progress("避让", spec.name, index, len(order))
|
||||
feedback = jog.feedback()
|
||||
current = None if feedback is None else [int(round(value)) for value in feedback]
|
||||
for waypoint in hand.avoidance_for(spec.name, current):
|
||||
_move(jog, waypoint, timeout)
|
||||
if progress is not None:
|
||||
progress("采集", spec.name, index, len(order))
|
||||
reference_frames = []
|
||||
pose = list(_move(jog, _held(hand, spec, current), timeout))
|
||||
for cycle in (0, 1):
|
||||
for direction, commands in _passes(spec):
|
||||
steady_commands = _steady_commands(commands)
|
||||
for command in commands:
|
||||
pose[spec.command_index] = int(command)
|
||||
feedback = jog.feedback()
|
||||
if feedback is None:
|
||||
raise CalibrationStopped("command_not_ready", spec.name)
|
||||
step = abs(int(command) - int(round(feedback[spec.command_index])))
|
||||
_move(jog, pose, max(timeout, full_range_seconds * step / 255.0))
|
||||
steady = command in steady_commands
|
||||
corners = _wait_corners(
|
||||
cameras, detector, spec.camera, spec.tag_id,
|
||||
timeout=1.0 if steady else 0.5,
|
||||
)
|
||||
if corners is None:
|
||||
raise CalibrationStopped("tag_not_visible", spec.name)
|
||||
observations.append({
|
||||
"joint": spec.name,
|
||||
"command_u8": int(command),
|
||||
"direction": direction,
|
||||
"cycle": cycle,
|
||||
"corners": corners,
|
||||
"steady": steady,
|
||||
"palm_pose_valid": True,
|
||||
})
|
||||
for tag_id in fixed_tag_ids(hand.profile, spec.camera):
|
||||
reference = _wait_corners(cameras, detector, spec.camera, tag_id, timeout=0.2)
|
||||
if reference is not None:
|
||||
reference_frames.append(reference)
|
||||
if len(reference_frames) < minimum_frames or reference_drift_px(reference_frames) > maximum_drift:
|
||||
raise CalibrationStopped("fixed_reference_drift", spec.name)
|
||||
joint_rows = [item for item in observations if item["joint"] == spec.name]
|
||||
try:
|
||||
samples = _joint_samples(
|
||||
spec, joint_rows, model=model, offsets=completed,
|
||||
fixture_from_hand=fixture_from_hand, front_from_fixture=front_from_fixture,
|
||||
front_from_view=camera_files.front_from_view, intrinsics=intrinsics,
|
||||
)
|
||||
check_samples(
|
||||
samples, start_u8=spec.start_u8, end_u8=spec.end_u8,
|
||||
baseline_u8=spec.baseline_u8, acquisition=acquisition,
|
||||
)
|
||||
except ValueError as error:
|
||||
raise CalibrationStopped(str(error), spec.name) from error
|
||||
completed[spec.name] = 0.0
|
||||
if offsets_callback is not None:
|
||||
offsets_callback(dict(completed))
|
||||
return observations
|
||||
|
||||
|
||||
def _held(hand, spec, current):
|
||||
waypoints = hand.avoidance_for(spec.name, current)
|
||||
return list(waypoints[-1])
|
||||
|
||||
|
||||
def _passes(spec) -> list[tuple[str, list[int]]]:
|
||||
increasing = _command_line(spec.start_u8, spec.end_u8, spec.baseline_u8)
|
||||
decreasing = _command_line(spec.end_u8, spec.start_u8, spec.baseline_u8)
|
||||
return [("increasing", increasing), ("decreasing", decreasing)]
|
||||
|
||||
|
||||
def _command_line(start: int, end: int, baseline: int) -> list[int]:
|
||||
values = []
|
||||
for value in np.linspace(start, end, 40):
|
||||
value = int(min(255, max(0, round(float(value)))))
|
||||
if not values or values[-1] != value:
|
||||
values.append(value)
|
||||
for required in (int(start), int(end), int(baseline)):
|
||||
if required not in values:
|
||||
values.append(required)
|
||||
return sorted(set(values), reverse=int(start) > int(end))
|
||||
|
||||
|
||||
def _steady_commands(commands: list[int]) -> set[int]:
|
||||
if len(commands) <= 9:
|
||||
return set(commands)
|
||||
indexes = [int(round(value)) for value in np.linspace(0, len(commands) - 1, 9)]
|
||||
return {commands[index] for index in indexes}
|
||||
|
||||
|
||||
def _move(jog, pose, timeout: float):
|
||||
pose = [float(value) for value in pose]
|
||||
if not jog.set_targets(pose):
|
||||
raise CalibrationStopped("command_not_ready")
|
||||
deadline = time.monotonic() + float(timeout)
|
||||
while time.monotonic() < deadline:
|
||||
feedback = jog.feedback()
|
||||
if feedback is not None and all(abs(float(actual) - target) <= 2.0 for actual, target in zip(feedback, pose)):
|
||||
return pose
|
||||
time.sleep(0.05)
|
||||
raise CalibrationStopped("command_not_settled")
|
||||
|
||||
|
||||
def _detector():
|
||||
try:
|
||||
import apriltag
|
||||
except ImportError as error:
|
||||
raise CalibrationStopped("apriltag_missing") from error
|
||||
return apriltag.apriltag("tag36h11")
|
||||
|
||||
|
||||
def _wait_corners(cameras, detector, view: str, tag_id: int, timeout: float):
|
||||
deadline = time.monotonic() + float(timeout)
|
||||
while time.monotonic() < deadline:
|
||||
frame = cameras.latest_frame(view)
|
||||
if frame is not None:
|
||||
corners = _detect(detector, frame, tag_id)
|
||||
if corners is not None:
|
||||
return corners
|
||||
time.sleep(0.05)
|
||||
return None
|
||||
|
||||
|
||||
def _detect(detector, frame, tag_id: int):
|
||||
width, height, data = frame
|
||||
gray = np.frombuffer(data, dtype=np.uint8).reshape(height, width, 3)[:, :, 0].copy()
|
||||
for detection in detector.detect(gray):
|
||||
if int(detection["id"]) != int(tag_id):
|
||||
continue
|
||||
corners = np.asarray(detection["lb-rb-rt-lt"], dtype=float).reshape(4, 2)
|
||||
if corners.shape == (4, 2) and np.all(np.isfinite(corners)):
|
||||
return corners
|
||||
return None
|
||||
@@ -0,0 +1,206 @@
|
||||
"""标定会话。型号差异全部来自传入的手对象。
|
||||
|
||||
样本先写成 JSON。拟合只读这份 JSON。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from calibration.acquisition.angle import axis_in_camera, edge_angle, point_in_camera, reading_from_corners, unwrap_edge_angle
|
||||
from calibration.acquisition.fixture import load_fixture
|
||||
from calibration.acquisition.gates import check_samples
|
||||
from calibration.acquisition.reprojection import mount_from_corners, reprojection_rms
|
||||
from calibration.acquisition.sample import joint_sample
|
||||
from calibration.cameras.files import load_camera_files
|
||||
from calibration.cameras.fixture_overlay import load_pinhole
|
||||
from calibration.fitting.envelope import assert_sample_axis
|
||||
from calibration.fitting.json_fit import fit_urdf_from_json, measurement_filename, write_measurement_json
|
||||
from calibration.session.errors import CalibrationStopped
|
||||
from calibration.urdf_model import UrdfKinematicModel
|
||||
|
||||
|
||||
REPROJECTION_LIMIT_PX = 1.5
|
||||
|
||||
|
||||
def run_calibration(hand, observations: list[dict], *, device_uid: str, side: str,
|
||||
progress=None, workspace: Path | None = None, offsets_callback=None,
|
||||
when: datetime | None = None) -> dict:
|
||||
"""observations 的每一项要有 joint、command_u8、direction、cycle、corners。"""
|
||||
root = Path(workspace or Path.cwd())
|
||||
finished = when or datetime.now()
|
||||
filename = measurement_filename(hand.model, side, device_uid, finished)
|
||||
try:
|
||||
fixture_from_hand = load_fixture(hand.product)
|
||||
except ValueError as error:
|
||||
raise CalibrationStopped(str(error)) from error
|
||||
front_from_fixture = hand.product.get("front_from_fixture")
|
||||
if front_from_fixture is None:
|
||||
raise CalibrationStopped("fixture_pose_missing")
|
||||
front_from_fixture = np.asarray(front_from_fixture, dtype=float)
|
||||
camera_files = load_camera_files(root)
|
||||
intrinsics = {view: load_pinhole(path) for view, path in camera_files.intrinsics.items()}
|
||||
samples = collect_samples(
|
||||
hand, observations,
|
||||
fixture_from_hand=fixture_from_hand,
|
||||
front_from_fixture=front_from_fixture,
|
||||
front_from_view=camera_files.front_from_view,
|
||||
intrinsics=intrinsics,
|
||||
progress=progress,
|
||||
offsets_callback=offsets_callback,
|
||||
)
|
||||
document = {
|
||||
"model": hand.model,
|
||||
"side": side,
|
||||
"device_uid": device_uid,
|
||||
"finished_at": finished.replace(microsecond=0).isoformat(),
|
||||
"joints": {
|
||||
spec.name: {"baseline_u8": spec.baseline_u8, "samples": samples[spec.name]}
|
||||
for spec in hand.measurement_order()
|
||||
},
|
||||
}
|
||||
json_path = write_measurement_json(root / "output_urdf_calibration" / filename, document)
|
||||
if progress is not None:
|
||||
progress("拟合", "", len(document["joints"]), len(document["joints"]))
|
||||
urdf_path = fit_urdf_from_json(
|
||||
json_path, source_urdf=hand.urdf_path, authorized_fields=hand.authorized_fields(),
|
||||
)
|
||||
return {"json": json_path, "urdf": urdf_path, "samples": samples}
|
||||
|
||||
|
||||
def collect_samples(hand, observations: list[dict], *, fixture_from_hand, front_from_fixture,
|
||||
front_from_view, intrinsics, progress=None, offsets_callback=None) -> dict[str, list[dict]]:
|
||||
model = UrdfKinematicModel(hand.urdf_path)
|
||||
offsets: dict[str, float] = {}
|
||||
collected: dict[str, list[dict]] = {}
|
||||
order = hand.measurement_order()
|
||||
for index, spec in enumerate(order, start=1):
|
||||
if progress is not None:
|
||||
progress("采集", spec.name, index, len(order))
|
||||
missing = [name for name in spec.ancestors if name not in offsets]
|
||||
if missing:
|
||||
raise CalibrationStopped("parent_zero_missing", spec.name)
|
||||
joint_observations = [item for item in observations if item.get("joint") == spec.name]
|
||||
samples = _joint_samples(
|
||||
spec, joint_observations, model=model, offsets=offsets,
|
||||
fixture_from_hand=fixture_from_hand, front_from_fixture=front_from_fixture,
|
||||
front_from_view=front_from_view, intrinsics=intrinsics,
|
||||
)
|
||||
try:
|
||||
check_samples(
|
||||
samples, start_u8=spec.start_u8, end_u8=spec.end_u8,
|
||||
baseline_u8=spec.baseline_u8, acquisition=hand.profile["acquisition"],
|
||||
)
|
||||
except ValueError as error:
|
||||
raise CalibrationStopped(str(error), spec.name) from error
|
||||
collected[spec.name] = samples
|
||||
offsets[spec.name] = 0.0
|
||||
if offsets_callback is not None:
|
||||
offsets_callback(dict(offsets))
|
||||
return collected
|
||||
|
||||
|
||||
def _joint_samples(spec, observations, *, model, offsets, fixture_from_hand, front_from_fixture,
|
||||
front_from_view, intrinsics) -> list[dict]:
|
||||
angles = _unwrap_observations(spec, observations)
|
||||
phi_zero = None
|
||||
zero_corners = None
|
||||
for observation, phi in angles:
|
||||
if phi is None:
|
||||
continue
|
||||
if (observation.get("direction") == "decreasing"
|
||||
and int(observation.get("command_u8")) == spec.baseline_u8):
|
||||
phi_zero = phi
|
||||
zero_corners = observation.get("corners")
|
||||
break
|
||||
if phi_zero is None:
|
||||
raise CalibrationStopped("zero_reference_missing", spec.name)
|
||||
mount = None
|
||||
axis_point = None
|
||||
axis_direction = None
|
||||
camera_matrix = distortion = None
|
||||
if intrinsics is not None and spec.camera in intrinsics and zero_corners is not None:
|
||||
camera_matrix, distortion = intrinsics[spec.camera]
|
||||
try:
|
||||
mount = mount_from_corners(
|
||||
np.asarray(zero_corners, dtype=float), tag_size_m=spec.tag_size_m,
|
||||
camera_matrix=camera_matrix, distortion=distortion,
|
||||
)
|
||||
except ValueError as error:
|
||||
raise CalibrationStopped(str(error), spec.name) from error
|
||||
axis_palm, point_palm = model.axis_line(spec.name, zero_offsets=offsets, joint_angles={})
|
||||
axis_direction = axis_in_camera(
|
||||
axis_palm, camera=spec.camera, fixture_from_hand=fixture_from_hand,
|
||||
front_from_fixture=front_from_fixture, front_from_view=front_from_view,
|
||||
)
|
||||
axis_point = point_in_camera(
|
||||
point_palm, camera=spec.camera, fixture_from_hand=fixture_from_hand,
|
||||
front_from_fixture=front_from_fixture, front_from_view=front_from_view,
|
||||
)
|
||||
samples = []
|
||||
for observation, phi in angles:
|
||||
if phi is None:
|
||||
continue
|
||||
reading = _read(
|
||||
observation, spec, model=model, offsets=offsets, phi_zero_rad=phi_zero,
|
||||
previous_phi=None, fixture_from_hand=fixture_from_hand,
|
||||
front_from_fixture=front_from_fixture, front_from_view=front_from_view,
|
||||
unwrapped_phi=phi,
|
||||
)
|
||||
if not reading.ok:
|
||||
raise CalibrationStopped(reading.reason, spec.name)
|
||||
if mount is not None:
|
||||
error = reprojection_rms(
|
||||
mount, theta_rad=reading.theta_rad, axis_point=axis_point,
|
||||
axis_direction=axis_direction, corners=np.asarray(observation["corners"], dtype=float),
|
||||
camera_matrix=camera_matrix, distortion=distortion,
|
||||
)
|
||||
if error > REPROJECTION_LIMIT_PX:
|
||||
raise CalibrationStopped("reprojection_exceeded", spec.name)
|
||||
sample = joint_sample(
|
||||
joint=spec.name, command_u8=observation["command_u8"],
|
||||
direction=observation["direction"], cycle=observation.get("cycle", 0),
|
||||
theta_rad=reading.theta_rad, axis_palm=reading.axis_palm, view=spec.camera,
|
||||
)
|
||||
sample["steady"] = bool(observation.get("steady"))
|
||||
model_axis, _point = model.axis_line(spec.name, zero_offsets=offsets, joint_angles={})
|
||||
assert_sample_axis(sample["axis_palm_xyz"], model_axis)
|
||||
samples.append(sample)
|
||||
return samples
|
||||
|
||||
|
||||
def _unwrap_observations(spec, observations) -> list[tuple[dict, float | None]]:
|
||||
"""按时间顺序展开图像角。跳变超过 35° 的帧记为 None,不拿来当关节角。"""
|
||||
result = []
|
||||
previous = None
|
||||
for observation in observations:
|
||||
try:
|
||||
phi = edge_angle(np.asarray(observation.get("corners"), dtype=float))
|
||||
except (TypeError, ValueError):
|
||||
raise CalibrationStopped("tag_not_visible", spec.name) from None
|
||||
if previous is not None:
|
||||
phi = unwrap_edge_angle(previous, phi)
|
||||
if phi is None:
|
||||
result.append((observation, None))
|
||||
continue
|
||||
previous = phi
|
||||
result.append((observation, phi))
|
||||
return result
|
||||
|
||||
|
||||
def _read(observation, spec, *, model, offsets, phi_zero_rad, previous_phi,
|
||||
fixture_from_hand, front_from_fixture, front_from_view, unwrapped_phi=None):
|
||||
return reading_from_corners(
|
||||
observation.get("corners"),
|
||||
model=model, joint=spec.name, camera=spec.camera,
|
||||
zero_offsets=offsets, required_ancestors=spec.ancestors,
|
||||
fixture_from_hand=fixture_from_hand,
|
||||
front_from_fixture=front_from_fixture,
|
||||
front_from_view=front_from_view, phi_zero_rad=phi_zero_rad,
|
||||
previous_phi_rad=previous_phi,
|
||||
palm_pose_valid=observation.get("palm_pose_valid", True),
|
||||
unwrapped_phi=unwrapped_phi,
|
||||
)
|
||||
@@ -0,0 +1,208 @@
|
||||
"""Standard URDF kinematics; independent of SDK, ROS and product profiles.
|
||||
|
||||
Measured passive angles are useful to estimate geometry, but release validation
|
||||
must resolve the *serialized* mimic graph, never those independent observations.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Mapping, Sequence
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def _parse_triplet(value: str) -> np.ndarray:
|
||||
result = np.asarray([float(v) for v in str(value).split()], dtype=float)
|
||||
if result.shape != (3,) or not np.all(np.isfinite(result)):
|
||||
raise ValueError(f"invalid URDF vector: {value}")
|
||||
return result
|
||||
|
||||
|
||||
def _axis_rotation(axis: Sequence[float], angle: float) -> np.ndarray:
|
||||
vector = np.asarray(axis, dtype=float)
|
||||
if vector.shape != (3,) or not np.all(np.isfinite(vector)):
|
||||
raise ValueError("joint axis must be a finite 3-vector")
|
||||
norm = float(np.linalg.norm(vector))
|
||||
if norm <= 1e-12 or not math.isfinite(float(angle)):
|
||||
raise ValueError("joint axis/angle is invalid")
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = Rotation.from_rotvec(vector / norm * float(angle)).as_matrix()
|
||||
return transform
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UrdfJoint:
|
||||
name: str
|
||||
parent: str
|
||||
child: str
|
||||
origin: np.ndarray
|
||||
axis: np.ndarray
|
||||
mimic_joint: str | None
|
||||
mimic_multiplier: float
|
||||
mimic_offset: float
|
||||
kind: str = "revolute"
|
||||
lower: float | None = None
|
||||
upper: float | None = None
|
||||
|
||||
|
||||
class UrdfKinematicModel:
|
||||
def __init__(self, source: str | Path) -> None:
|
||||
self.source = Path(source).expanduser().resolve()
|
||||
root = ET.parse(self.source).getroot()
|
||||
if root.tag != "robot":
|
||||
raise ValueError("URDF root must be robot")
|
||||
names = [element.get("name", "") for element in root.findall("link")]
|
||||
if not names or "" in names or len(set(names)) != len(names):
|
||||
raise ValueError("URDF link names must be nonempty and unique")
|
||||
self.links = frozenset(names)
|
||||
self.joints: dict[str, UrdfJoint] = {}
|
||||
self.parent_joint_by_child: dict[str, str] = {}
|
||||
for element in root.findall("joint"):
|
||||
name = element.get("name", "")
|
||||
kind = element.get("type", "")
|
||||
if not name or name in self.joints:
|
||||
raise ValueError("URDF joint names must be nonempty and unique")
|
||||
if kind not in {"fixed", "revolute", "continuous", "prismatic"}:
|
||||
raise ValueError(f"unsupported calibration joint type: {name}:{kind}")
|
||||
parent, child = element.find("parent"), element.find("child")
|
||||
if parent is None or child is None:
|
||||
raise ValueError(f"URDF joint is missing parent/child: {name}")
|
||||
parent_name, child_name = parent.get("link", ""), child.get("link", "")
|
||||
if parent_name not in self.links or child_name not in self.links:
|
||||
raise ValueError(f"URDF joint references a missing link: {name}")
|
||||
if child_name in self.parent_joint_by_child:
|
||||
raise ValueError(f"URDF link has multiple parents: {child_name}")
|
||||
origin_node, axis_node = element.find("origin"), element.find("axis")
|
||||
xyz = _parse_triplet("0 0 0" if origin_node is None else origin_node.get("xyz", "0 0 0"))
|
||||
rpy = _parse_triplet("0 0 0" if origin_node is None else origin_node.get("rpy", "0 0 0"))
|
||||
axis = _parse_triplet("1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0"))
|
||||
if float(np.linalg.norm(axis)) <= 1e-12:
|
||||
raise ValueError(f"URDF joint has a zero axis: {name}")
|
||||
axis /= np.linalg.norm(axis)
|
||||
origin = np.eye(4)
|
||||
origin[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix()
|
||||
origin[:3, 3] = xyz
|
||||
mimic, limit = element.find("mimic"), element.find("limit")
|
||||
multiplier = 1.0 if mimic is None else float(mimic.get("multiplier", "1"))
|
||||
offset = 0.0 if mimic is None else float(mimic.get("offset", "0"))
|
||||
if not math.isfinite(multiplier) or not math.isfinite(offset):
|
||||
raise ValueError(f"URDF mimic is nonfinite: {name}")
|
||||
lower = None if limit is None or kind in {"fixed", "continuous"} else float(limit.get("lower", "nan"))
|
||||
upper = None if lower is None else float(limit.get("upper", "nan"))
|
||||
if lower is not None and (not math.isfinite(lower) or not math.isfinite(upper) or lower > upper):
|
||||
raise ValueError(f"URDF joint has invalid limits: {name}")
|
||||
self.joints[name] = UrdfJoint(name, parent_name, child_name, origin, axis,
|
||||
None if mimic is None else mimic.get("joint", ""), multiplier, offset,
|
||||
kind, lower, upper)
|
||||
self.parent_joint_by_child[child_name] = name
|
||||
roots = self.links - self.parent_joint_by_child.keys()
|
||||
if len(roots) != 1:
|
||||
raise ValueError("URDF must have exactly one root link")
|
||||
self.root_link = next(iter(roots))
|
||||
for name in self.joints:
|
||||
self._chain(name)
|
||||
self.resolve_angles({}) # Validate the complete mimic graph, including siblings.
|
||||
|
||||
def _chain(self, target_joint: str) -> list[UrdfJoint]:
|
||||
if target_joint not in self.joints:
|
||||
raise ValueError(f"URDF is missing joint {target_joint}")
|
||||
chain: list[UrdfJoint] = []
|
||||
seen: set[str] = set()
|
||||
current = self.joints[target_joint]
|
||||
while True:
|
||||
if current.name in seen:
|
||||
raise ValueError(f"URDF kinematic cycle: {current.name}")
|
||||
seen.add(current.name)
|
||||
chain.append(current)
|
||||
parent = self.parent_joint_by_child.get(current.parent)
|
||||
if parent is None:
|
||||
if current.parent != self.root_link:
|
||||
raise ValueError("URDF contains a disconnected joint")
|
||||
return list(reversed(chain))
|
||||
current = self.joints[parent]
|
||||
|
||||
def resolve_angles(self, joint_angles: Mapping[str, float], *, independent_mimic_angles: bool = False) -> dict[str, float]:
|
||||
supplied = {str(k): float(v) for k, v in joint_angles.items()}
|
||||
if not all(math.isfinite(v) for v in supplied.values()):
|
||||
raise ValueError("joint angles must be finite")
|
||||
resolved: dict[str, float] = {}
|
||||
visiting: set[str] = set()
|
||||
|
||||
def resolve(name: str) -> float:
|
||||
if name in resolved:
|
||||
return resolved[name]
|
||||
if name in visiting or name not in self.joints:
|
||||
raise ValueError(f"invalid URDF mimic dependency: {name}")
|
||||
visiting.add(name)
|
||||
joint = self.joints[name]
|
||||
if joint.kind == "fixed":
|
||||
angle = 0.0
|
||||
elif joint.mimic_joint is not None and not (independent_mimic_angles and name in supplied):
|
||||
angle = joint.mimic_multiplier * resolve(joint.mimic_joint) + joint.mimic_offset
|
||||
else:
|
||||
angle = supplied.get(name, 0.0)
|
||||
if not math.isfinite(angle):
|
||||
raise ValueError(f"nonfinite resolved URDF angle: {name}")
|
||||
visiting.remove(name)
|
||||
resolved[name] = angle
|
||||
return angle
|
||||
|
||||
for name in self.joints:
|
||||
resolve(name)
|
||||
return resolved
|
||||
|
||||
@staticmethod
|
||||
def _motion(joint: UrdfJoint, angle: float) -> np.ndarray:
|
||||
if joint.kind == "fixed":
|
||||
return np.eye(4)
|
||||
if joint.kind == "prismatic":
|
||||
transform = np.eye(4)
|
||||
transform[:3, 3] = joint.axis * angle
|
||||
return transform
|
||||
return _axis_rotation(joint.axis, angle)
|
||||
|
||||
def link_transform(self, target_joint: str, *, zero_offsets: Mapping[str, float], joint_angles: Mapping[str, float], independent_mimic_angles: bool = False) -> np.ndarray:
|
||||
"""Base-to-child-link pose. Independent passive input is measurement-only."""
|
||||
resolved = self.resolve_angles(joint_angles, independent_mimic_angles=independent_mimic_angles)
|
||||
transform = np.eye(4)
|
||||
for joint in self._chain(target_joint):
|
||||
transform = transform @ joint.origin @ self._motion(joint, float(zero_offsets.get(joint.name, 0.0)) + resolved[joint.name])
|
||||
return transform
|
||||
|
||||
def joint_frame(self, target_joint: str, *, zero_offsets: Mapping[str, float], joint_angles: Mapping[str, float], independent_mimic_angles: bool = False) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""关节坐标系在手掌中的原点、旋转,以及关节自身坐标系里的轴。"""
|
||||
resolved = self.resolve_angles(joint_angles, independent_mimic_angles=independent_mimic_angles)
|
||||
transform = np.eye(4)
|
||||
for joint in self._chain(target_joint):
|
||||
frame = transform @ joint.origin @ self._motion(joint, float(zero_offsets.get(joint.name, 0.0)))
|
||||
if joint.name == target_joint:
|
||||
return frame[:3, 3].copy(), frame[:3, :3].copy(), joint.axis.copy()
|
||||
transform = frame @ self._motion(joint, resolved[joint.name])
|
||||
raise ValueError(f"cannot resolve joint axis: {target_joint}")
|
||||
|
||||
def axis_line(self, target_joint: str, *, zero_offsets: Mapping[str, float], joint_angles: Mapping[str, float], independent_mimic_angles: bool = False) -> tuple[np.ndarray, np.ndarray]:
|
||||
origin, rotation, axis = self.joint_frame(
|
||||
target_joint, zero_offsets=zero_offsets, joint_angles=joint_angles,
|
||||
independent_mimic_angles=independent_mimic_angles,
|
||||
)
|
||||
return rotation @ axis, origin
|
||||
|
||||
|
||||
def corrected_origin_rpy(joint: UrdfJoint, offset_rad: float) -> tuple[float, float, float]:
|
||||
"""q_CAD = q_output + offset; compose about the local axis, not an RPY component."""
|
||||
if joint.kind not in {"revolute", "continuous"}:
|
||||
raise ValueError("angular zero correction requires a revolute joint")
|
||||
matrix = joint.origin @ _axis_rotation(joint.axis, offset_rad)
|
||||
return tuple(float(v) for v in Rotation.from_matrix(matrix[:3, :3]).as_euler("xyz"))
|
||||
|
||||
|
||||
def corrected_mimic_offset(multiplier: float, offset: float, parent_zero: float, child_zero: float) -> float:
|
||||
values = (multiplier, offset, parent_zero, child_zero)
|
||||
if not all(math.isfinite(float(v)) for v in values):
|
||||
raise ValueError("mimic coordinate correction must be finite")
|
||||
return float(offset + multiplier * parent_zero - child_zero)
|
||||
@@ -0,0 +1 @@
|
||||
"""标定界面。关节表和避让不写在这里。"""
|
||||
@@ -0,0 +1,6 @@
|
||||
"""启动标定界面:python -m gui"""
|
||||
|
||||
from gui.window import main
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,92 @@
|
||||
"""把滑块目标发给已连接的手。拖动期间只保留最新目标。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
|
||||
class JointJog:
|
||||
def __init__(self, executor, command_topic: str, state_topic: str, names: list[str]):
|
||||
from rclpy.node import Node
|
||||
|
||||
self.names = list(names)
|
||||
self.executor = executor
|
||||
self._lock = threading.Lock()
|
||||
self._target: list[float] | None = None
|
||||
self._feedback: list[float] | None = None
|
||||
self._pending = False
|
||||
self._announce = False
|
||||
self.node = Node("calibration_joint_jog")
|
||||
self.publisher = self.node.create_publisher(JointState, command_topic, 1)
|
||||
self.node.create_subscription(JointState, state_topic, self._state, 20)
|
||||
self.node.create_timer(1 / 30, self._tick)
|
||||
self.executor.add_node(self.node)
|
||||
|
||||
def _state(self, message: JointState) -> None:
|
||||
if list(message.name) and list(message.name) != self.names:
|
||||
return
|
||||
positions = [float(value) for value in message.position]
|
||||
if len(positions) != len(self.names):
|
||||
return
|
||||
with self._lock:
|
||||
self._feedback = positions
|
||||
if self._target is None:
|
||||
self._target = list(positions)
|
||||
self._announce = True
|
||||
|
||||
def consume_sync(self) -> list[float] | None:
|
||||
with self._lock:
|
||||
if not self._announce or self._target is None:
|
||||
return None
|
||||
self._announce = False
|
||||
return list(self._target)
|
||||
|
||||
def feedback(self) -> list[float] | None:
|
||||
with self._lock:
|
||||
return None if self._feedback is None else list(self._feedback)
|
||||
|
||||
def ready(self) -> bool:
|
||||
with self._lock:
|
||||
return self._target is not None
|
||||
|
||||
def set_targets(self, values: list[float]) -> bool:
|
||||
with self._lock:
|
||||
if self._target is None or len(values) != len(self._target):
|
||||
return False
|
||||
self._target = [float(value) for value in values]
|
||||
self._pending = True
|
||||
return True
|
||||
|
||||
def set_channel(self, index: int, value: float) -> None:
|
||||
with self._lock:
|
||||
if self._target is None:
|
||||
return
|
||||
self._target[index] = float(value)
|
||||
self._pending = True
|
||||
|
||||
def sync_from_feedback(self) -> list[float] | None:
|
||||
with self._lock:
|
||||
if self._feedback is None:
|
||||
return None
|
||||
self._target = list(self._feedback)
|
||||
self._pending = False
|
||||
return list(self._target)
|
||||
|
||||
def _tick(self) -> None:
|
||||
with self._lock:
|
||||
if not self._pending or self._target is None:
|
||||
return
|
||||
values = list(self._target)
|
||||
self._pending = False
|
||||
message = JointState()
|
||||
message.header.stamp = self.node.get_clock().now().to_msg()
|
||||
message.name = list(self.names)
|
||||
message.position = values
|
||||
self.publisher.publish(message)
|
||||
|
||||
def close(self) -> None:
|
||||
if self.executor is not None:
|
||||
self.executor.remove_node(self.node)
|
||||
self.node.destroy_node()
|
||||
@@ -0,0 +1,56 @@
|
||||
"""关节滑块和精确数值框。刷新显示时不发运动指令。"""
|
||||
|
||||
from decimal import Decimal
|
||||
|
||||
from PyQt5.QtCore import QSignalBlocker, Qt, pyqtSignal
|
||||
from PyQt5.QtWidgets import QDoubleSpinBox, QHBoxLayout, QSlider, QWidget
|
||||
|
||||
|
||||
class JointControl(QWidget):
|
||||
target_changed = pyqtSignal(float)
|
||||
|
||||
def __init__(self, name: str, minimum: float, maximum: float, resolution: float, parent=None):
|
||||
super().__init__(parent)
|
||||
self.name = name
|
||||
self.minimum = float(minimum)
|
||||
self.maximum = float(maximum)
|
||||
self.resolution = float(resolution)
|
||||
self.slider = QSlider(Qt.Horizontal)
|
||||
self.slider.setRange(0, round((self.maximum - self.minimum) / self.resolution))
|
||||
self.slider.setPageStep(max(1, self.slider.maximum() // 25))
|
||||
self.slider.setMinimumWidth(160)
|
||||
self.slider.setAccessibleName(name + " 目标滑块")
|
||||
self.editor = QDoubleSpinBox()
|
||||
decimals = -Decimal(str(resolution)).as_tuple().exponent
|
||||
self.editor.setDecimals(max(0, min(8, decimals)))
|
||||
self.editor.setRange(self.minimum, self.maximum)
|
||||
self.editor.setSingleStep(self.resolution)
|
||||
self.editor.setKeyboardTracking(False)
|
||||
self.editor.setFixedWidth(80)
|
||||
self.editor.setAccessibleName(name + " 精确目标")
|
||||
layout = QHBoxLayout(self)
|
||||
layout.setContentsMargins(6, 0, 6, 0)
|
||||
layout.addWidget(self.slider, 1)
|
||||
layout.addWidget(self.editor)
|
||||
self.slider.valueChanged.connect(self._slider_changed)
|
||||
self.editor.valueChanged.connect(self._editor_changed)
|
||||
self.set_target(self.minimum)
|
||||
|
||||
def set_target(self, value: float) -> None:
|
||||
"""只更新滑块和数值,不发出运动请求。"""
|
||||
tick = round((float(value) - self.minimum) / self.resolution)
|
||||
tick = max(self.slider.minimum(), min(self.slider.maximum(), tick))
|
||||
slider_block = QSignalBlocker(self.slider)
|
||||
editor_block = QSignalBlocker(self.editor)
|
||||
self.slider.setValue(tick)
|
||||
self.editor.setValue(self.minimum + tick * self.resolution)
|
||||
del slider_block, editor_block
|
||||
|
||||
def _slider_changed(self, tick: int) -> None:
|
||||
value = self.minimum + tick * self.resolution
|
||||
self.set_target(value)
|
||||
self.target_changed.emit(value)
|
||||
|
||||
def _editor_changed(self, value: float) -> None:
|
||||
self.set_target(value)
|
||||
self.target_changed.emit(self.editor.value())
|
||||
@@ -0,0 +1,396 @@
|
||||
"""选手型、左右手,连接驱动后再标定。三路画面上可画关节坐标系。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
import threading
|
||||
from pathlib import Path
|
||||
|
||||
from PyQt5.QtCore import Qt, QTimer, pyqtSignal
|
||||
from PyQt5.QtGui import QColor, QImage, QPainter, QPen, QPixmap
|
||||
from PyQt5.QtWidgets import (
|
||||
QApplication, QComboBox, QGroupBox, QHBoxLayout, QHeaderView, QLabel,
|
||||
QMainWindow, QMessageBox, QPushButton, QTableWidget, QTableWidgetItem,
|
||||
QVBoxLayout, QWidget,
|
||||
)
|
||||
|
||||
from calibration.cameras.fixture_overlay import FixtureOverlay
|
||||
from calibration.cameras.rig import CameraRig
|
||||
from gui.jog import JointJog
|
||||
from gui.joint_control import JointControl
|
||||
from hands import available_models, load_hand
|
||||
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
HANDS = ROOT / "src" / "urdf_calibration" / "hands"
|
||||
VIEWS = ("front", "side", "top")
|
||||
VIEW_LABELS = {"front": "正面", "side": "侧面", "top": "上面"}
|
||||
AXIS_COLORS = {"x": "#d32f2f", "y": "#2e7d32", "z": "#1565c0"}
|
||||
|
||||
|
||||
class CalibrationWindow(QMainWindow):
|
||||
progress_changed = pyqtSignal(str)
|
||||
connect_finished = pyqtSignal(bool, str)
|
||||
calibration_finished = pyqtSignal(str)
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.setWindowTitle("手部 URDF 标定")
|
||||
self.resize(1280, 860)
|
||||
self.hand = None
|
||||
self.catalog_hand = None
|
||||
self._joint_overlay = None
|
||||
self._offset_lock = threading.Lock()
|
||||
self._offsets = {}
|
||||
self.sdk = None
|
||||
self.jog = None
|
||||
self.connected_side = ""
|
||||
self.calibrating = False
|
||||
self.joint_controls = []
|
||||
self.feedback_labels = []
|
||||
self.cameras = CameraRig(ROOT)
|
||||
try:
|
||||
self.fixture = FixtureOverlay(ROOT)
|
||||
except Exception:
|
||||
self.fixture = None
|
||||
self._build()
|
||||
self.progress_changed.connect(self.progress.setText)
|
||||
self.connect_finished.connect(self._on_connect_finished)
|
||||
self.calibration_finished.connect(self._on_calibration_finished)
|
||||
self._reload_models()
|
||||
self._start_cameras()
|
||||
self._image_timer = QTimer(self)
|
||||
self._image_timer.timeout.connect(self._poll_images)
|
||||
self._image_timer.timeout.connect(self._poll_motion)
|
||||
self._image_timer.start(100)
|
||||
|
||||
def _build(self):
|
||||
root = QWidget()
|
||||
self.setCentralWidget(root)
|
||||
layout = QVBoxLayout(root)
|
||||
bar = QHBoxLayout()
|
||||
self.model_box = QComboBox()
|
||||
self.side_box = QComboBox()
|
||||
self.side_box.addItems(["右手", "左手"])
|
||||
self.connect_button = QPushButton("连接")
|
||||
self.start_button = QPushButton("开始标定")
|
||||
self.start_button.setEnabled(False)
|
||||
bar.addWidget(QLabel("型号"))
|
||||
bar.addWidget(self.model_box)
|
||||
bar.addWidget(QLabel("左右"))
|
||||
bar.addWidget(self.side_box)
|
||||
bar.addWidget(self.connect_button)
|
||||
bar.addWidget(self.start_button)
|
||||
bar.addStretch(1)
|
||||
layout.addLayout(bar)
|
||||
|
||||
self.progress = QLabel("尚未连接。先选择型号和左右手,再点连接。")
|
||||
self.progress.setWordWrap(True)
|
||||
layout.addWidget(self.progress)
|
||||
|
||||
images = QHBoxLayout()
|
||||
self.views = {}
|
||||
for name in VIEWS:
|
||||
box = QGroupBox(VIEW_LABELS[name])
|
||||
inner = QVBoxLayout(box)
|
||||
label = QLabel("等待相机图像\n上面相机看全四张角贴纸后画出工装坐标系")
|
||||
label.setAlignment(Qt.AlignCenter)
|
||||
label.setMinimumSize(360, 260)
|
||||
label.setStyleSheet("background:#111;color:#ddd;")
|
||||
inner.addWidget(label)
|
||||
images.addWidget(box)
|
||||
self.views[name] = label
|
||||
layout.addLayout(images)
|
||||
|
||||
hint = QLabel("拖动滑块直接控制关节;数值框支持精调,输入后按回车确认。")
|
||||
hint.setWordWrap(True)
|
||||
layout.addWidget(hint)
|
||||
slider_bar = QHBoxLayout()
|
||||
self.sync_button = QPushButton("同步当前目标")
|
||||
self.sync_button.setEnabled(False)
|
||||
self.sync_button.clicked.connect(self._sync_targets)
|
||||
slider_bar.addWidget(self.sync_button)
|
||||
slider_bar.addStretch(1)
|
||||
layout.addLayout(slider_bar)
|
||||
self.joints_table = QTableWidget(0, 3)
|
||||
self.joints_table.setHorizontalHeaderLabels(["关节", "目标(拖动即运动)", "实际反馈"])
|
||||
self.joints_table.setEditTriggers(QTableWidget.NoEditTriggers)
|
||||
self.joints_table.setSelectionBehavior(QTableWidget.SelectRows)
|
||||
self.joints_table.verticalHeader().setVisible(False)
|
||||
header = self.joints_table.horizontalHeader()
|
||||
header.setSectionResizeMode(0, QHeaderView.ResizeToContents)
|
||||
header.setSectionResizeMode(1, QHeaderView.Stretch)
|
||||
header.setSectionResizeMode(2, QHeaderView.ResizeToContents)
|
||||
self.joints_table.setMinimumHeight(220)
|
||||
layout.addWidget(self.joints_table, 1)
|
||||
|
||||
self.side_box.currentTextChanged.connect(self._model_changed)
|
||||
self.connect_button.clicked.connect(self._connect)
|
||||
self.start_button.clicked.connect(self._start)
|
||||
self.model_box.currentTextChanged.connect(self._model_changed)
|
||||
|
||||
def _reload_models(self):
|
||||
self.model_box.clear()
|
||||
models = available_models(HANDS)
|
||||
self.model_box.addItems(models or ["(没有型号)"])
|
||||
self._model_changed()
|
||||
|
||||
def _selected_side(self) -> str:
|
||||
return "right" if self.side_box.currentText() == "右手" else "left"
|
||||
|
||||
def _release_hand(self):
|
||||
self._detach_jog()
|
||||
if self.sdk is not None:
|
||||
self.sdk.close()
|
||||
self.sdk = None
|
||||
self.hand = None
|
||||
self.start_button.setEnabled(False)
|
||||
self._set_sliders_enabled(False)
|
||||
|
||||
def _model_changed(self):
|
||||
self._release_hand()
|
||||
self.catalog_hand = None
|
||||
self._joint_overlay = None
|
||||
self._clear_sliders()
|
||||
model = self.model_box.currentText()
|
||||
if not model or model.startswith("("):
|
||||
return
|
||||
try:
|
||||
hand = load_hand(model, HANDS)
|
||||
except Exception as error:
|
||||
self.progress.setText(str(error))
|
||||
return
|
||||
self.catalog_hand = hand
|
||||
self._joint_overlay = None
|
||||
self._set_offsets({})
|
||||
names = hand.command_names()
|
||||
bounds = hand.slider_bounds()
|
||||
resolution = hand.command_resolution()
|
||||
self.joints_table.setRowCount(len(names))
|
||||
for row, (name, (low, high)) in enumerate(zip(names, bounds)):
|
||||
self.joints_table.setItem(row, 0, QTableWidgetItem(name))
|
||||
control = JointControl(name, low, high, resolution)
|
||||
control.setEnabled(False)
|
||||
control.target_changed.connect(lambda value, index=row: self._jog(index, value))
|
||||
self.joints_table.setCellWidget(row, 1, control)
|
||||
feedback = QTableWidgetItem("—")
|
||||
self.joints_table.setItem(row, 2, feedback)
|
||||
self.joints_table.setRowHeight(row, max(34, control.sizeHint().height() + 4))
|
||||
self.joint_controls.append(control)
|
||||
self.feedback_labels.append(feedback)
|
||||
|
||||
def _clear_sliders(self):
|
||||
self.joints_table.setRowCount(0)
|
||||
self.joint_controls.clear()
|
||||
self.feedback_labels.clear()
|
||||
|
||||
def _set_sliders_enabled(self, enabled: bool):
|
||||
ready = enabled and not self.calibrating and self.jog is not None and self.jog.ready()
|
||||
for control in self.joint_controls:
|
||||
control.setEnabled(ready)
|
||||
self.sync_button.setEnabled(ready)
|
||||
|
||||
def _jog(self, index: int, value: float):
|
||||
if self.calibrating or self.jog is None or not self.jog.ready():
|
||||
return
|
||||
self.jog.set_channel(index, value)
|
||||
|
||||
def _sync_targets(self):
|
||||
if self.jog is None:
|
||||
return
|
||||
values = self.jog.sync_from_feedback()
|
||||
if values is None:
|
||||
return
|
||||
for control, value in zip(self.joint_controls, values):
|
||||
control.set_target(value)
|
||||
|
||||
def _poll_motion(self):
|
||||
if self.jog is None:
|
||||
return
|
||||
feedback = self.jog.feedback()
|
||||
if feedback is not None:
|
||||
for item, value in zip(self.feedback_labels, feedback):
|
||||
item.setText(f"{value:g}")
|
||||
synced = self.jog.consume_sync()
|
||||
if synced is not None:
|
||||
for control, value in zip(self.joint_controls, synced):
|
||||
control.set_target(value)
|
||||
self._set_sliders_enabled(self.hand is not None)
|
||||
|
||||
def _attach_jog(self):
|
||||
self._detach_jog()
|
||||
if self.cameras.executor is None or self.hand is None:
|
||||
return
|
||||
topics = self.hand.ros_topics(self.connected_side)
|
||||
self.jog = JointJog(
|
||||
self.cameras.executor, topics["command"], topics["state"], self.hand.command_names(),
|
||||
)
|
||||
|
||||
def _detach_jog(self):
|
||||
if self.jog is None:
|
||||
return
|
||||
self.jog.close()
|
||||
self.jog = None
|
||||
|
||||
def _connect(self):
|
||||
model = self.model_box.currentText()
|
||||
side = self._selected_side()
|
||||
self._release_hand()
|
||||
self.connect_button.setEnabled(False)
|
||||
self.progress.setText("正在连接驱动…")
|
||||
|
||||
def work():
|
||||
try:
|
||||
hand = load_hand(model, HANDS)
|
||||
process = hand.open(side, ros_executor=self.cameras.executor)
|
||||
self.hand = hand
|
||||
self.sdk = process
|
||||
self.connected_side = side
|
||||
self.connect_finished.emit(True, "驱动已拉起。等待真机身份回报后即可开始标定。")
|
||||
except Exception as error:
|
||||
self.connect_finished.emit(False, str(error))
|
||||
|
||||
threading.Thread(target=work, daemon=True).start()
|
||||
|
||||
def _on_connect_finished(self, ok: bool, message: str):
|
||||
self.connect_button.setEnabled(True)
|
||||
self.progress.setText(message)
|
||||
if not ok:
|
||||
self.start_button.setEnabled(False)
|
||||
self._set_sliders_enabled(False)
|
||||
QMessageBox.warning(self, "连接失败", message)
|
||||
return
|
||||
self.start_button.setEnabled(True)
|
||||
self._attach_jog()
|
||||
self._set_sliders_enabled(True)
|
||||
QTimer.singleShot(200, self._refresh_frames)
|
||||
|
||||
def _start_cameras(self):
|
||||
try:
|
||||
self.progress.setText(self.cameras.start())
|
||||
except Exception as error:
|
||||
self.progress.setText(f"相机没有拉起:{error}")
|
||||
|
||||
def _poll_images(self):
|
||||
while True:
|
||||
try:
|
||||
view, width, height, data = self.cameras.frames.get_nowait()
|
||||
except Exception:
|
||||
return
|
||||
image = QImage(data, width, height, width * 3, QImage.Format_RGB888).copy()
|
||||
frames, markers = (None, [])
|
||||
if self.fixture is not None:
|
||||
try:
|
||||
frames, markers = self.fixture.overlay(view, data, width, height)
|
||||
except Exception:
|
||||
frames, markers = None, []
|
||||
joint_frames = self._joint_frames(view)
|
||||
drawn = ([frames] if frames else []) + joint_frames
|
||||
self.show_image(view, image, drawn or None, markers)
|
||||
|
||||
def _refresh_frames(self):
|
||||
"""工装位姿还没有时只保留图像区域,不画假坐标系。"""
|
||||
for label in self.views.values():
|
||||
if label.pixmap() is None:
|
||||
label.setText("等待相机图像\n上面相机看全四张角贴纸后画出工装坐标系")
|
||||
|
||||
def show_image(self, view: str, image: QImage, frames=None, markers=None):
|
||||
"""frames 是工装坐标轴。markers 是上面相机里各张角贴纸的四角和编号。"""
|
||||
canvas = image.copy()
|
||||
if frames or markers:
|
||||
painter = QPainter(canvas)
|
||||
for corners, caption in markers or []:
|
||||
painter.setPen(QPen(QColor("#f5f5f5"), 2))
|
||||
points = [tuple(int(round(value)) for value in point) for point in corners]
|
||||
for start, end in zip(points, points[1:] + points[:1]):
|
||||
painter.drawLine(*start, *end)
|
||||
painter.drawText(points[0][0], points[0][1] - 8, caption)
|
||||
if frames:
|
||||
for frame in frames:
|
||||
segments = frame.get("segments", frame)
|
||||
thick = frame.get("rotation_axis")
|
||||
for axis, segment in segments.items():
|
||||
if axis not in AXIS_COLORS:
|
||||
continue
|
||||
start, end = segment
|
||||
width = 6 if axis == thick else 3
|
||||
painter.setPen(QPen(QColor(AXIS_COLORS[axis]), width))
|
||||
painter.drawLine(*start, *end)
|
||||
painter.end()
|
||||
self.views[view].setPixmap(QPixmap.fromImage(canvas).scaled(
|
||||
self.views[view].size(), Qt.KeepAspectRatio, Qt.SmoothTransformation
|
||||
))
|
||||
|
||||
def _start(self):
|
||||
if self.hand is None:
|
||||
return
|
||||
self.calibrating = True
|
||||
self._set_sliders_enabled(False)
|
||||
self.start_button.setEnabled(False)
|
||||
self.progress_changed.emit("标定开始。")
|
||||
|
||||
def work():
|
||||
try:
|
||||
from calibration.session.live import run_live
|
||||
|
||||
def progress(stage, joint, index, total):
|
||||
name = f" {joint}" if joint else ""
|
||||
self.progress_changed.emit(f"{stage}{name} {index}/{total}")
|
||||
|
||||
result = run_live(
|
||||
self.hand, self.jog, self.cameras, side=self.connected_side,
|
||||
progress=progress, workspace=ROOT, offsets_callback=self._set_offsets,
|
||||
)
|
||||
self.calibration_finished.emit(f"拟合完成:{result['urdf']}")
|
||||
except Exception as error:
|
||||
reason = getattr(error, "reason", str(error))
|
||||
joint = getattr(error, "joint", "")
|
||||
self.calibration_finished.emit(f"标定停止:{joint + ' ' if joint else ''}{reason}")
|
||||
|
||||
threading.Thread(target=work, daemon=True).start()
|
||||
|
||||
def _set_offsets(self, offsets: dict):
|
||||
with self._offset_lock:
|
||||
self._offsets = dict(offsets)
|
||||
|
||||
def _offsets_copy(self) -> dict:
|
||||
with self._offset_lock:
|
||||
return dict(self._offsets)
|
||||
|
||||
def _joint_frames(self, view: str) -> list:
|
||||
hand = self.hand or self.catalog_hand
|
||||
if hand is None:
|
||||
return []
|
||||
if self._joint_overlay is None or getattr(self._joint_overlay, "hand", None) is not hand:
|
||||
try:
|
||||
from calibration.cameras.joint_overlay import JointOverlay
|
||||
self._joint_overlay = JointOverlay(hand, ROOT)
|
||||
except Exception:
|
||||
self._joint_overlay = False
|
||||
if not self._joint_overlay:
|
||||
return []
|
||||
try:
|
||||
return self._joint_overlay.frames(view, self._offsets_copy())
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
def _on_calibration_finished(self, message: str):
|
||||
self.calibrating = False
|
||||
self.progress.setText(message)
|
||||
self.start_button.setEnabled(self.hand is not None)
|
||||
self._set_sliders_enabled(True)
|
||||
|
||||
def closeEvent(self, event):
|
||||
self._image_timer.stop()
|
||||
self._detach_jog()
|
||||
self.cameras.stop()
|
||||
if self.sdk is not None:
|
||||
self.sdk.close()
|
||||
super().closeEvent(event)
|
||||
|
||||
|
||||
def main():
|
||||
app = QApplication(sys.argv)
|
||||
window = CalibrationWindow()
|
||||
window.show()
|
||||
sys.exit(app.exec_())
|
||||
@@ -0,0 +1,26 @@
|
||||
"""各型号手的业务。主程序只按文件夹发现型号,不写具体避让。"""
|
||||
|
||||
import importlib
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def _base(root: Path | None) -> Path:
|
||||
return Path(root) if root is not None else Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def available_models(root: Path | None = None) -> list[str]:
|
||||
names = []
|
||||
for path in sorted(_base(root).iterdir()):
|
||||
if path.is_dir() and (path / "profile.yaml").is_file() and (path / "sdk.yaml").is_file():
|
||||
names.append(path.name)
|
||||
return names
|
||||
|
||||
|
||||
def load_hand(model: str, root: Path | None = None):
|
||||
if model not in available_models(root):
|
||||
raise ValueError(f"未知型号: {model}")
|
||||
module = importlib.import_module(f"hands.{model}.hand")
|
||||
for value in vars(module).values():
|
||||
if isinstance(value, type) and value.__module__ == module.__name__ and hasattr(value, "open"):
|
||||
return value(_base(root) / model)
|
||||
raise ValueError(f"{model} 没有手型入口")
|
||||
@@ -0,0 +1 @@
|
||||
"""O30 的避让、测量顺序和驱动连接。"""
|
||||
@@ -0,0 +1,53 @@
|
||||
"""按 O30 profile 里写好的分组,先让开别的手指,再动正在测的通道。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
|
||||
def _grouped(current: list[int], target: list[int], groups, measured_channel: int | None) -> list[tuple[int, ...]]:
|
||||
cursor = list(current)
|
||||
declared = [tuple(group) for group in groups]
|
||||
handled = {index for group in declared for index in group}
|
||||
remaining = tuple(
|
||||
index for index in range(len(target))
|
||||
if index not in handled and index != measured_channel
|
||||
)
|
||||
if remaining:
|
||||
if declared and measured_channel is not None and measured_channel in declared[-1]:
|
||||
declared.insert(len(declared) - 1, remaining)
|
||||
else:
|
||||
declared.append(remaining)
|
||||
if measured_channel is not None and measured_channel not in handled:
|
||||
declared.append((measured_channel,))
|
||||
output = []
|
||||
for group in declared:
|
||||
changed = False
|
||||
for index in group:
|
||||
if cursor[index] != target[index]:
|
||||
cursor[index] = target[index]
|
||||
changed = True
|
||||
if changed:
|
||||
output.append(tuple(cursor))
|
||||
return output or [tuple(target)]
|
||||
|
||||
|
||||
def avoidance_waypoints(profile: dict, joint: str, current: list[int] | None = None) -> list[tuple[int, ...]]:
|
||||
"""进入这个关节的测量前,手要依次到达的指令。被测通道是这个关节自己的下标。"""
|
||||
from hands.o30.motion import task_for_joint
|
||||
|
||||
task = task_for_joint(profile, joint)
|
||||
command_index = int(profile["command"]["command_index_by_joint"][joint])
|
||||
baseline = [int(value) for value in profile["command"]["baseline_u8"]]
|
||||
start = list(baseline if current is None or len(current) != len(baseline) else current)
|
||||
target = list(start)
|
||||
for index, value in task.get("auxiliary_commands") or []:
|
||||
target[int(index)] = int(value)
|
||||
target[command_index] = int(task["start_u8"])
|
||||
return _grouped(start, target, task.get("preparation_groups") or [], command_index)
|
||||
|
||||
|
||||
def return_waypoints(profile: dict, current: list[int]) -> list[tuple[int, ...]]:
|
||||
baseline = [int(value) for value in profile["command"]["baseline_u8"]]
|
||||
groups = profile["motion"].get("return_groups") or []
|
||||
if not groups:
|
||||
return [tuple(baseline)]
|
||||
return _grouped(list(current), baseline, groups, measured_channel=None)
|
||||
@@ -0,0 +1,12 @@
|
||||
# 这台机器上 O30 驱动工程的 install/setup.bash。
|
||||
setup: /home/huangwen/WorkSpace/Linker_Hand_O30_ROS2_SDK/Linker_Hand_O30_ROS2_SDK/install/setup.bash
|
||||
# 手掌相对工装。xyz 单位米,rpy 单位弧度,绕固定轴 X-Y-Z。
|
||||
fixture_from_hand_base_link:
|
||||
xyz: [0.0, 0.0, 0.0]
|
||||
rpy: [0.0, 0.0, 0.0]
|
||||
# 4×4 矩阵。标定用左上角 3×3 把工装轴变到正面相机。
|
||||
# front_from_fixture:
|
||||
# - [1.0, 0.0, 0.0, 0.0]
|
||||
# - [0.0, 1.0, 0.0, 0.0]
|
||||
# - [0.0, 0.0, 1.0, 0.0]
|
||||
# - [0.0, 0.0, 0.0, 1.0]
|
||||
@@ -0,0 +1,147 @@
|
||||
"""拉起 O30 驱动并确认真机在线。自动摆位由 SDK 执行,这里只把它关掉。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
class ConnectError(Exception):
|
||||
pass
|
||||
|
||||
|
||||
def load_sdk(path: Path) -> dict:
|
||||
return yaml.safe_load(Path(path).read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def launch_command(sdk: dict, side: str) -> list[str]:
|
||||
if side not in {"left", "right"}:
|
||||
raise ConnectError(f"未知左右手: {side}")
|
||||
setup = Path(str(sdk["setup"]))
|
||||
if not setup.is_file():
|
||||
raise ConnectError(f"找不到 O30 驱动环境: {setup}")
|
||||
launch = sdk["launch"]
|
||||
parameters = []
|
||||
for name, value in launch["parameters"].items():
|
||||
rendered = side if value == "{side}" else value
|
||||
if isinstance(rendered, bool):
|
||||
rendered = "true" if rendered else "false"
|
||||
parameters.append(f"-p {name}:={rendered}")
|
||||
joined = " ".join(parameters)
|
||||
script = (
|
||||
f'source "{setup}" && exec ros2 run {launch["package"]} {launch["executable"]} '
|
||||
f"--ros-args {joined}"
|
||||
)
|
||||
return ["bash", "-lc", script]
|
||||
|
||||
|
||||
def expected_topics(sdk: dict, side: str) -> dict[str, str]:
|
||||
connect = sdk["connect"]
|
||||
return {
|
||||
"info": str(connect["info_topic"]).format(side=side),
|
||||
"state": str(connect["state_topic"]).format(side=side),
|
||||
"command": str(connect["command_topic"]).format(side=side),
|
||||
}
|
||||
|
||||
|
||||
def _side_text(side: str) -> set[str]:
|
||||
if side == "right":
|
||||
return {"right", "r", "右手"}
|
||||
return {"left", "l", "左手"}
|
||||
|
||||
|
||||
def identity_ok(payload: dict, *, side: str, joint_count: int) -> str:
|
||||
"""返回空字符串表示这只手已连上,否则返回原因。"""
|
||||
if not isinstance(payload, dict):
|
||||
return "sdk_info_invalid"
|
||||
if "O30" not in str(payload.get("model", "")).upper():
|
||||
return "sdk_model_mismatch"
|
||||
reported = str(payload.get("side", "")).lower()
|
||||
if reported not in _side_text(side):
|
||||
return "sdk_side_mismatch"
|
||||
names = payload.get("joint_names")
|
||||
if not isinstance(names, list) or len(names) != joint_count:
|
||||
return "sdk_joint_count_mismatch"
|
||||
if payload.get("online") is not True:
|
||||
return "sdk_offline"
|
||||
if not str(payload.get("uid") or "").strip():
|
||||
return "device_uid_missing"
|
||||
return ""
|
||||
|
||||
|
||||
def wait_until_online(sdk: dict, side: str, timeout: float, executor=None) -> str:
|
||||
"""订 SDK 的信息话题,直到型号、左右手和关节数都对上。"""
|
||||
try:
|
||||
import rclpy
|
||||
from std_msgs.msg import String
|
||||
except ImportError as error:
|
||||
raise ConnectError("当前环境没有 ROS,无法确认真机是否在线") from error
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
node = rclpy.create_node("calibration_hand_connect")
|
||||
holder = {"reason": "waiting_for_sdk_info", "uid": ""}
|
||||
topics = expected_topics(sdk, side)
|
||||
joint_count = int(sdk["connect"]["joint_count"])
|
||||
|
||||
def callback(message):
|
||||
try:
|
||||
payload = json.loads(message.data)
|
||||
except json.JSONDecodeError:
|
||||
holder["reason"] = "sdk_info_invalid"
|
||||
return
|
||||
holder["reason"] = identity_ok(payload, side=side, joint_count=joint_count)
|
||||
if holder["reason"] == "":
|
||||
holder["uid"] = str(payload.get("uid") or "").strip()
|
||||
|
||||
node.create_subscription(String, topics["info"], callback, 10)
|
||||
deadline = time.monotonic() + float(timeout)
|
||||
try:
|
||||
if executor is not None:
|
||||
executor.add_node(node)
|
||||
while time.monotonic() < deadline:
|
||||
if holder["reason"] == "":
|
||||
return holder["uid"]
|
||||
time.sleep(0.05)
|
||||
raise ConnectError(holder["reason"])
|
||||
while time.monotonic() < deadline:
|
||||
rclpy.spin_once(node, timeout_sec=0.2)
|
||||
if holder["reason"] == "":
|
||||
return holder["uid"]
|
||||
raise ConnectError(holder["reason"])
|
||||
finally:
|
||||
if executor is not None:
|
||||
executor.remove_node(node)
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
class SdkProcess:
|
||||
def __init__(self, process: subprocess.Popen):
|
||||
self.process = process
|
||||
|
||||
def close(self) -> None:
|
||||
if self.process.poll() is not None:
|
||||
return
|
||||
os.killpg(self.process.pid, signal.SIGINT)
|
||||
try:
|
||||
self.process.wait(timeout=8)
|
||||
except subprocess.TimeoutExpired:
|
||||
os.killpg(self.process.pid, signal.SIGKILL)
|
||||
self.process.wait(timeout=5)
|
||||
|
||||
|
||||
def start_sdk(sdk: dict, side: str) -> SdkProcess:
|
||||
command = launch_command(sdk, side)
|
||||
process = subprocess.Popen(
|
||||
command, start_new_session=True, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT
|
||||
)
|
||||
return SdkProcess(process)
|
||||
|
||||
|
||||
def parse_info(raw: str) -> dict:
|
||||
return json.loads(raw)
|
||||
@@ -0,0 +1,76 @@
|
||||
"""O30 右手的业务入口。左手资料还没有时,连接直接失败。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
from hands.o30.avoidance import avoidance_waypoints
|
||||
from hands.o30.connect import ConnectError, expected_topics, load_sdk, start_sdk, wait_until_online
|
||||
from hands.o30.motion import authorized_fields, command_names, measurement_order
|
||||
|
||||
|
||||
class O30Hand:
|
||||
def __init__(self, directory: Path):
|
||||
self.directory = Path(directory)
|
||||
self.profile = yaml.safe_load((self.directory / "profile.yaml").read_text(encoding="utf-8"))
|
||||
self.product = yaml.safe_load((self.directory / "product.yaml").read_text(encoding="utf-8"))
|
||||
self.sdk = load_sdk(self.directory / "sdk.yaml")
|
||||
hand_config = yaml.safe_load((self.directory / "config.yaml").read_text(encoding="utf-8"))
|
||||
if not isinstance(hand_config, dict) or "setup" not in hand_config:
|
||||
raise ConnectError("config.yaml 缺少 setup")
|
||||
self.device_uid = ""
|
||||
self.sdk["setup"] = hand_config["setup"]
|
||||
for key in ("fixture_from_hand_base_link", "front_from_fixture"):
|
||||
if key in hand_config:
|
||||
self.product[key] = hand_config[key]
|
||||
|
||||
@property
|
||||
def model(self) -> str:
|
||||
return "o30"
|
||||
|
||||
@property
|
||||
def urdf_path(self) -> Path:
|
||||
return self.directory / self.product["source_urdf"]
|
||||
|
||||
def supports(self, side: str) -> bool:
|
||||
return side == self.product.get("side")
|
||||
|
||||
def command_names(self) -> list[str]:
|
||||
return command_names(self.profile)
|
||||
|
||||
def measurement_order(self):
|
||||
return measurement_order(self.profile)
|
||||
|
||||
def authorized_fields(self) -> dict[str, list[str]]:
|
||||
return authorized_fields(self.profile)
|
||||
|
||||
def avoidance_for(self, joint: str, current: list[int] | None = None):
|
||||
return avoidance_waypoints(self.profile, joint, current)
|
||||
|
||||
def slider_bounds(self) -> list[tuple[int, int]]:
|
||||
return [(0, 255)] * len(self.command_names())
|
||||
|
||||
def command_resolution(self) -> float:
|
||||
return 1
|
||||
|
||||
def ros_topics(self, side: str) -> dict[str, str]:
|
||||
return expected_topics(self.sdk, side)
|
||||
|
||||
def open(self, side: str, ros_executor=None):
|
||||
if side not in {"left", "right"}:
|
||||
raise ConnectError(f"未知左右手: {side}")
|
||||
if not self.supports(side):
|
||||
label = "右手" if side == "right" else "左手"
|
||||
raise ConnectError(f"O30 目前没有{label}的避让和 URDF")
|
||||
process = start_sdk(self.sdk, side)
|
||||
try:
|
||||
self.device_uid = wait_until_online(
|
||||
self.sdk, side, float(self.sdk["connect"]["timeout_sec"]),
|
||||
executor=ros_executor,
|
||||
)
|
||||
except Exception:
|
||||
process.close()
|
||||
raise
|
||||
return process
|
||||
@@ -0,0 +1,118 @@
|
||||
"""O30 的测量顺序、指令通道和必须先测完的祖先。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointMeasurement:
|
||||
name: str
|
||||
camera: str
|
||||
command_index: int
|
||||
baseline_u8: int
|
||||
start_u8: int
|
||||
end_u8: int
|
||||
ancestors: tuple[str, ...]
|
||||
tag_role: str
|
||||
tag_id: int
|
||||
tag_size_m: float
|
||||
|
||||
|
||||
def load_profile(path) -> dict:
|
||||
return yaml.safe_load(Path_read(path))
|
||||
|
||||
|
||||
def Path_read(path):
|
||||
from pathlib import Path
|
||||
return Path(path).read_text(encoding="utf-8")
|
||||
|
||||
|
||||
def command_names(profile: dict) -> list[str]:
|
||||
return list(profile["command"]["names"])
|
||||
|
||||
|
||||
def task_for_joint(profile: dict, joint: str) -> dict:
|
||||
"""关节只要出现在任务的 joints 里就算这个任务。四指侧摆共用一个任务。"""
|
||||
matches = [task for task in profile["motion"]["tasks"] if joint in (task.get("joints") or [])]
|
||||
if len(matches) != 1:
|
||||
raise KeyError(joint)
|
||||
return matches[0]
|
||||
|
||||
|
||||
def required_ancestors(profile: dict, joint: str) -> tuple[str, ...]:
|
||||
"""会改变这根轴方向的祖先。相位父节绕同一根轴,不放进这份名单。"""
|
||||
spatial = profile["zero"]["spatial"]
|
||||
axis_parents = spatial["axis_parent_joint"]
|
||||
phase_parents = spatial["phase_parent_joint"]
|
||||
cursor = joint
|
||||
seen = set()
|
||||
while cursor not in axis_parents and cursor in phase_parents:
|
||||
if cursor in seen:
|
||||
break
|
||||
seen.add(cursor)
|
||||
cursor = phase_parents[cursor]
|
||||
chain = []
|
||||
seen_axis = set()
|
||||
while cursor in axis_parents:
|
||||
parent = axis_parents[cursor]
|
||||
if parent in seen_axis:
|
||||
break
|
||||
seen_axis.add(parent)
|
||||
chain.append(parent)
|
||||
cursor = parent
|
||||
chain.reverse()
|
||||
return tuple(chain)
|
||||
|
||||
|
||||
def tag_for_joint(profile: dict, joint: str) -> tuple[str, str, int, float]:
|
||||
"""按 measurement 的 child_role,到 vision.views 里取贴纸编号。"""
|
||||
measurement = profile["measurement"]["measurements"][joint]
|
||||
view = measurement["view"]
|
||||
role = measurement["child_role"]
|
||||
for block in profile["vision"]["views"]:
|
||||
if block["name"] != view:
|
||||
continue
|
||||
for tag in block["tags"]:
|
||||
if tag["role"] == role:
|
||||
return view, role, int(tag["id"]), float(tag["size_m"])
|
||||
raise KeyError(joint)
|
||||
|
||||
|
||||
def fixed_tag_ids(profile: dict, view: str) -> tuple[int, ...]:
|
||||
for block in profile["vision"]["views"]:
|
||||
if block["name"] != view:
|
||||
continue
|
||||
return tuple(int(tag["id"]) for tag in block["tags"] if tag.get("fixed_reference"))
|
||||
return ()
|
||||
|
||||
|
||||
def measurement_order(profile: dict) -> list[JointMeasurement]:
|
||||
"""按 axis_order 从手掌往指尖。每个关节都测,不因为共用任务被丢掉。"""
|
||||
command = profile["command"]
|
||||
index_by_joint = command["command_index_by_joint"]
|
||||
baseline = command["baseline_u8"]
|
||||
result = []
|
||||
for name in profile["zero"]["spatial"]["axis_order"]:
|
||||
task = task_for_joint(profile, name)
|
||||
view, role, tag_id, size_m = tag_for_joint(profile, name)
|
||||
index = int(index_by_joint[name])
|
||||
result.append(JointMeasurement(
|
||||
name=name,
|
||||
camera=view,
|
||||
command_index=index,
|
||||
baseline_u8=int(baseline[index]),
|
||||
start_u8=int(task["start_u8"]),
|
||||
end_u8=int(task["end_u8"]),
|
||||
ancestors=required_ancestors(profile, name),
|
||||
tag_role=role,
|
||||
tag_id=tag_id,
|
||||
tag_size_m=size_m,
|
||||
))
|
||||
return result
|
||||
|
||||
|
||||
def authorized_fields(profile: dict) -> dict[str, list[str]]:
|
||||
return {name: list(fields) for name, fields in profile["urdf"]["authorized_fields"].items()}
|
||||
@@ -0,0 +1,3 @@
|
||||
model: O30
|
||||
side: right
|
||||
source_urdf: urdf/linkerhand_O30i_right-V2_0819.urdf
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,15 @@
|
||||
launch:
|
||||
package: linker_hand_o30_ros2_sdk
|
||||
executable: linker_hand_o30_ros2_sdk
|
||||
parameters:
|
||||
hand_type: "{side}"
|
||||
hand_joint: O30
|
||||
comm_type: libcanbus
|
||||
auto_init_pose: false
|
||||
connect:
|
||||
info_topic: /cb_{side}_hand_info
|
||||
state_topic: /cb_{side}_hand_state
|
||||
command_topic: /cb_{side}_hand_control_cmd
|
||||
expect_model: O30
|
||||
joint_count: 20
|
||||
timeout_sec: 15
|
||||
File diff suppressed because it is too large
Load Diff
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user