Compare commits
28 Commits
o30
...
calibration
| Author | SHA1 | Date | |
|---|---|---|---|
 admin
|
a8eaa4c367 | ||
|
admin
|
32b6af62e2 | ||
|
admin
|
af2dc9c38f | ||
|
admin
|
7a04780b52 | ||
|
admin
|
467651fc47 | ||
|
admin
|
69c2da6808 | ||
|
admin
|
f94cf2c500 | ||
|
admin
|
c4ad2b968a | ||
|
admin
|
a8ddcc6296 | ||
|
admin
|
2356bd6247 | ||
|
admin
|
889e0ea8db | ||
|
admin
|
e1fb458eff | ||
|
admin
|
8de69c34a1 | ||
|
admin
|
d6b7bd6209 | ||
|
admin
|
8a749a3687 | ||
|
admin
|
08fe190b3a | ||
|
admin
|
f7aeef87a8 | ||
|
admin
|
2b7c1f92e7 | ||
|
admin
|
1ed36ecdd8 | ||
|
admin
|
7f84225ba8 | ||
|
admin
|
ba9f1b25e8 | ||
|
admin
|
0d606c2ba2 | ||
|
admin
|
06c050e446 | ||
|
admin
|
286581bcba | ||
|
admin
|
4dadfb954b | ||
|
admin
|
83c69b48c2 | ||
|
admin
|
4e594ddb09 | ||
|
admin
|
ef65681230 |
+19
-3
@@ -50,7 +50,8 @@ Thumbs.db
|
||||
|
||||
# Runtime and calibration scratch files
|
||||
/logs/
|
||||
/MvSdkLog/
|
||||
# The camera SDK writes logs relative to the launch working directory.
|
||||
MvSdkLog/
|
||||
*.tmp
|
||||
*.log
|
||||
*.bak
|
||||
@@ -61,14 +62,27 @@ Thumbs.db
|
||||
# Reproducible seed profiles remain under
|
||||
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
|
||||
/profiles/
|
||||
/calibration_output/
|
||||
/config/g20_three_camera_extrinsics.yaml
|
||||
# Calibration outputs may be created from the workspace root or a subdirectory.
|
||||
calibration_output/
|
||||
range_calibration_output/
|
||||
/config/*_three_camera_extrinsics.yaml
|
||||
# Superseded local O6 camera calibrations. Keep the active extrinsics and
|
||||
# intrinsics referenced by o6_right_product.yaml available for version control.
|
||||
/config/o6_three_camera_extrinsics_20260915.yaml
|
||||
/config/o6_three_camera_extrinsics_intrinsics_20260915.yaml
|
||||
/config/o6_three_camera_extrinsics_recalibrated.yaml
|
||||
*.wear_check.json
|
||||
*.checkpoint.json
|
||||
*.verification.json
|
||||
*_mapping_quality.json
|
||||
|
||||
# Device-specific robot descriptions derived from local calibration runs
|
||||
# Includes full/partial zero-calibration outputs and local copies.
|
||||
/src/linkerhand_calibration/urdf/*/*_calibrated_*.urdf
|
||||
/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf
|
||||
/src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf
|
||||
# Unreferenced local URDF experiment; never use as a protected CAD input.
|
||||
/src/linkerhand_calibration/urdf/o6_right/linkerhand_o6_right111.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
|
||||
@@ -92,3 +106,5 @@ candump-*
|
||||
# Local Codex/agent workspace metadata
|
||||
/.agents/
|
||||
/.codex/
|
||||
/.codebuddy/
|
||||
/.zcode/
|
||||
|
||||
@@ -0,0 +1,55 @@
|
||||
image_width: 1624
|
||||
image_height: 1240
|
||||
camera_name: hikrobot_top_DB2163739
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data:
|
||||
- 3608.7915725635876
|
||||
- 0.0
|
||||
- 814.529414176388
|
||||
- 0.0
|
||||
- 3602.9474866035775
|
||||
- 596.7866652774017
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data:
|
||||
- -0.05774549441026762
|
||||
- -0.5653814259405426
|
||||
- -0.0034831870836035525
|
||||
- -0.0007572304719307008
|
||||
- 0.0
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data:
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data:
|
||||
- 3590.9833984375
|
||||
- 0.0
|
||||
- 813.6314052451635
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 3591.4892578125
|
||||
- 595.0547132430074
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 0.0
|
||||
@@ -0,0 +1,55 @@
|
||||
image_width: 1624
|
||||
image_height: 1240
|
||||
camera_name: hikrobot_front_DB2163742
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data:
|
||||
- 3520.83963745858
|
||||
- 0.0
|
||||
- 971.8024440668337
|
||||
- 0.0
|
||||
- 3503.4665942352735
|
||||
- 636.2092891200001
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data:
|
||||
- -0.06456607194260039
|
||||
- -0.3209716179987898
|
||||
- 0.0016533290649511723
|
||||
- 0.014394085090833234
|
||||
- 0.0
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data:
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data:
|
||||
- 3485.41357421875
|
||||
- 0.0
|
||||
- 980.9585478080553
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 3500.062255859375
|
||||
- 636.4474700149367
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 0.0
|
||||
@@ -0,0 +1,55 @@
|
||||
image_width: 1624
|
||||
image_height: 1240
|
||||
camera_name: hikrobot_side_DB2163749
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data:
|
||||
- 3564.4557723541934
|
||||
- 0.0
|
||||
- 633.5422135270659
|
||||
- 0.0
|
||||
- 3549.8257661608695
|
||||
- 614.1194434338138
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data:
|
||||
- -0.023159717917942222
|
||||
- -0.6611401590594669
|
||||
- 0.0010272016326466154
|
||||
- -0.014523561130335804
|
||||
- 0.0
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data:
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data:
|
||||
- 3531.000244140625
|
||||
- 0.0
|
||||
- 623.4124604711214
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 3551.248291015625
|
||||
- 614.0312247691472
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 0.0
|
||||
@@ -0,0 +1,60 @@
|
||||
schema_version: 1
|
||||
reference_view: front
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
width: 1624
|
||||
height: 1240
|
||||
intrinsics_sha256: 3cdecb8fcede41561019882a48a342d7265a44855cf0282ec96ec7f69a9b1ee5
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
width: 1624
|
||||
height: 1240
|
||||
intrinsics_sha256: dbe0cdc73460dc1e51c0e3967395d14b4415a71cee952597e0717ad0babd3449
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
width: 1624
|
||||
height: 1240
|
||||
intrinsics_sha256: 315d5cd6f28964ad57ff67088222fff3d9679cce076acc3f05a22b76038e5352
|
||||
front_from_view:
|
||||
front:
|
||||
translation_xyz_m:
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
quaternion_xyzw:
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
side:
|
||||
translation_xyz_m:
|
||||
- -0.8459732881345877
|
||||
- -0.000555404488719817
|
||||
- 0.9729938257334647
|
||||
quaternion_xyzw:
|
||||
- -0.04404064417946292
|
||||
- 0.6703485002266404
|
||||
- -0.01659275028535042
|
||||
- 0.7405524900654377
|
||||
top:
|
||||
translation_xyz_m:
|
||||
- -0.10806018719194868
|
||||
- -0.5842672050520756
|
||||
- 1.0419844264616385
|
||||
quaternion_xyzw:
|
||||
- 0.7284460604085318
|
||||
- -0.016210595429770967
|
||||
- 0.019511507802845565
|
||||
- -0.6846333724953536
|
||||
quality:
|
||||
passed: true
|
||||
reprojection_rms_px: 0.46496404895142424
|
||||
maximum_rotation_repeatability_deg: 0.019287272875226844
|
||||
maximum_translation_repeatability_m: 0.0002246640979668814
|
||||
front_side_captures: 15
|
||||
front_top_captures: 15
|
||||
front_side_candidates: 15
|
||||
front_top_candidates: 15
|
||||
front_side_rejected: 0
|
||||
front_top_rejected: 0
|
||||
Submodule
+1
Submodule src/agillink_omnihand_sdk added at 026740d9fd
@@ -1,774 +0,0 @@
|
||||
# G20 左右手 AprilTag 标定
|
||||
|
||||
## 三机位三维关节轴零位标定（schema v4）
|
||||
|
||||
正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有
|
||||
`/g20_calibration` 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要
|
||||
贴正；相机和Tag在一次标定中必须固定。默认绑定为：
|
||||
|
||||
```text
|
||||
front = DB2163742，Tag 0/1/2/3/10
|
||||
side = DB2163749，Tag 4/5/6/7
|
||||
top = DB2163739，Tag 8/9
|
||||
```
|
||||
|
||||
11 张 `tag36h11` 的程序角色必须与贴纸所在刚性件一致：
|
||||
|
||||
| ID | 机位 | 固定位置/运动件 |
|
||||
|---:|---|---|
|
||||
| 0 | 正面 | 正面掌壳固定基准 |
|
||||
| 1 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 |
|
||||
| 2 | 正面 | 拇指 MCP 后连杆 |
|
||||
| 3 | 正面 | 拇指 IP 后末节 |
|
||||
| 4 | 侧面 | 掌壳侧面固定基准（最底下） |
|
||||
| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
|
||||
| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
|
||||
| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
|
||||
| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 |
|
||||
| 9 | 上面 | 拇指 CMC yaw 运动件 |
|
||||
| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
|
||||
|
||||
ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见，
|
||||
ID 9 必须在拇指横摆的完整行程中持续可见。
|
||||
贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。
|
||||
|
||||
每台相机必须有独立内参文件：
|
||||
|
||||
```text
|
||||
~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||
~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||
```
|
||||
|
||||
### 1. 一次性三相机外参
|
||||
|
||||
三相机第一次安装、任何相机移动、镜头重新聚焦或内参变化后，必须重标外参。使用
|
||||
`8x5` 内角点、实测方格边长 `27 mm`、粘在硬质平板上的棋盘：
|
||||
|
||||
```bash
|
||||
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_extrinsics.launch.py \
|
||||
output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
|
||||
```
|
||||
|
||||
启动后默认打开 `G20 Three-Camera Extrinsics` 交互窗口。可切换
|
||||
`FRONT + SIDE` 和 `FRONT + TOP`；窗口实时显示棋盘角点、单相机/组合
|
||||
RMS、时间差、联合拟合稳定性和候选/内点数量。单张只要棋盘完整、
|
||||
同步、RMS和姿态差异合格，`ADD CANDIDATE` 就会变绿；不再用单张
|
||||
PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒会自动
|
||||
采集，移到新姿态后再自动采下一组。
|
||||
|
||||
外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加
|
||||
一个候选；随后以相同方法采集正面/上面。程序使用固定内参的
|
||||
`stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和
|
||||
配对相机的单帧RMS分别不得超过1.5 px，同时组合RMS不得超过1.2 px；界面中
|
||||
单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。
|
||||
新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组，
|
||||
再在不低于15个内点的前提下有界裁剪联合误差最高的候选，最终1.2 px门限不会被
|
||||
放宽。两组均得到
|
||||
至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。
|
||||
|
||||
```bash
|
||||
ros2 service call /g20_camera_extrinsics/capture_front_side std_srvs/srv/Trigger {}
|
||||
ros2 service call /g20_camera_extrinsics/capture_front_top std_srvs/srv/Trigger {}
|
||||
ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
采集时可分别查看 `/g20_extrinsics/{front,side,top}/camera/image_rect`。界面始终
|
||||
显示当前配对的整批RMS、旋转稳定性和平移稳定性；`BATCH FAIL` 后会直接列出
|
||||
`INLIERS`、`RMS`、`ROT` 或 `TRANS` 失败项。保存门限为：联合重投影RMS不超过
|
||||
1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过1.5 mm。文件同时
|
||||
绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存通过结果，正式
|
||||
标定也不会运动。
|
||||
|
||||
### 2. 预检和正式标定
|
||||
|
||||
先使用禁止运动模式检查三个机位、外参、内参和标签：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=left \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
|
||||
commands_enabled:=false
|
||||
```
|
||||
|
||||
分别查看正式流程的三个画面：
|
||||
|
||||
```bash
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_calibration/front/camera/image_rect
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_calibration/side/camera/image_rect
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_calibration/top/camera/image_rect
|
||||
```
|
||||
|
||||
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=left \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
|
||||
can_interface:=can0
|
||||
```
|
||||
|
||||
右手使用同一入口；默认自动选择右手SDK话题和原始URDF：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=right \
|
||||
serial_number:=G20_RIGHT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
can_interface:=can0
|
||||
```
|
||||
|
||||
状态显示三个机位均“就绪”后只调用一次：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /g20_calibration/status_text
|
||||
ros2 service call /g20_calibration/start std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
收到 `start` 后，程序先下发并确认以下20通道基准姿态，稳定保持0.5秒后才开始
|
||||
第一条轨迹扫描：
|
||||
|
||||
```text
|
||||
[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
```
|
||||
|
||||
左手依次扫描电机 `0/5/15/6/1/16/10`，右手依次扫描
|
||||
`0/5/15/9/4/19/10`，每项三轮 `255→0→255`。轨迹角由父/子Tag完整相对
|
||||
四元数的旋转向量投影到三维拟合轴得到。
|
||||
轴方向使用相对姿态旋转轴和可信上游轴约束；仅对斜视、非约束关节将中心圆作为独立
|
||||
交叉检查并参与融合。轴线上一点则由整段
|
||||
相对SE(3)轨迹的 `(I-R)p=t` 方程鲁棒拟合，不再把单目Tag中心自由三维圆的圆心直接
|
||||
当成机械轴心。正面/侧面端视关节只使用图像平面内可观分量，丢弃无法由单目确定的
|
||||
光轴深度；斜视轨迹仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令
|
||||
严格归零：普通通道255，四指侧摆127。左手将食指动态轨迹、右手将小指动态轨迹继承
|
||||
给其余三指。11-Tag布局只能可靠恢复参考指的动态命令—角度曲线，不能证明四根独立
|
||||
电机的绝对装配相位相同；因此四指全部MCP侧摆、MCP屈伸和PIP静态URDF零偏都保留
|
||||
原始CAD的0，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。
|
||||
|
||||
零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定
|
||||
掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免
|
||||
稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节
|
||||
进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游
|
||||
轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差
|
||||
超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴
|
||||
Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过
|
||||
1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°；
|
||||
小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。
|
||||
|
||||
`thumb_mcp` 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过
|
||||
被动 `thumb_ip` 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴
|
||||
偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手
|
||||
`thumb_mcp` 都保留原始CAD零位0。该保护只冻结静态 `origin.rpy`，不会冻结或复制其
|
||||
`angle_rad[256]` 实测轨迹。
|
||||
|
||||
前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
|
||||
差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
|
||||
95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
|
||||
|
||||
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
|
||||
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
|
||||
不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。
|
||||
|
||||
四指参考源的MCP pitch虽有约70°大行程，但侧面机位接近沿转轴观察，单目PnP深度偏差
|
||||
仍可能把低残差的Tag中心圆平面稳定地倾斜。因此MCP pitch与其他端视关节一样，始终用
|
||||
完整相对姿态确定轴方向，Tag中心轨迹只参与轴线位置拟合；不再按10°分界在两种轴模型
|
||||
之间切换。固定Tag安装旋转会在相对旋转中抵消，不需要中心圆回退。被动PIP/DIP继承
|
||||
上游轴方向时不重复报告同一项跨轮轴失败。
|
||||
|
||||
四指MCP侧摆的动态曲线仍由参考指三轮实测并继承，但绝对静态侧摆零位固定使用原始CAD
|
||||
的0。仅凭下游pitch轴相对CAD掌坐标反推roll相位，会把稳定的跨视角/固定几何偏差写成
|
||||
约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
|
||||
|
||||
四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
|
||||
但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
|
||||
修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何
|
||||
按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
|
||||
|
||||
7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
|
||||
拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
|
||||
约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。
|
||||
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
|
||||
保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
|
||||
偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
|
||||
会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索
|
||||
不会放宽正式结果的硬门限。
|
||||
|
||||
生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
|
||||
`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
|
||||
JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。
|
||||
|
||||
坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
|
||||
重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
|
||||
端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
|
||||
连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
|
||||
暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
|
||||
完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
|
||||
拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
|
||||
只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
|
||||
失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
|
||||
仍保存在 `raw_samples.jsonl`。
|
||||
|
||||
每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
|
||||
程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
|
||||
分支；侧面Tag 4/5/6/7贴面在该端点应近似平行，初始化还会比较相邻Tag法向，避免
|
||||
错误镜像分支虽然8帧稳定且重投影很小仍被选中。每轮 `255→0` 前都会在静止端点独立
|
||||
重置并重新选择分支，使第三轮同时成为PnP初始化留出，而不是三轮共享同一错误分支。
|
||||
再开始正式轨迹采集。初始化帧不写入轨迹；最终单轴、跨轮和留出门限不变。
|
||||
|
||||
左手测食指roll时将电机7/8/9固定到0；右手测小指roll时因左右手侧摆机构镜像，
|
||||
将电机6/7/8固定到255。两者均为相机画面向右的物理避挡方向，速度分别为
|
||||
`[15,5,15,15,15]` 和 `[15,15,15,15,5]`。参考指MCP pitch/PIP扫描分别使用
|
||||
电机1/16（左）或4/19（右），参考指速度10。
|
||||
|
||||
右手扫描拇指CMC俯仰（电机0）前，程序将拇指横摆电机10和拇指侧摆电机5都固定到
|
||||
255，确认两个辅助关节到位后才允许电机0执行全行程。该关节由正面机位使用掌部
|
||||
Tag 0和运动Tag 1同帧测量。每帧都会保留两个辅助关节
|
||||
的实测条件值，轴线经外参转换到公共坐标系，零位求解按URDF上游关节链补偿；左手
|
||||
仍沿用原有正面机位和基准姿态。
|
||||
|
||||
标定 `thumb_cmc_yaw`（电机10）时，程序将
|
||||
`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag
|
||||
ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 `resume` 都保持
|
||||
电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才
|
||||
恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员
|
||||
暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的
|
||||
`baseline_command_u8` 不变。
|
||||
|
||||
右手标定小指PIP（电机19）时，命令0对应的固件反馈可能稳定饱和在5。只有电机19
|
||||
的0端使用±5反馈容差，并将该实测机械端点归入命令0端点分箱；255端和其他电机仍
|
||||
使用默认±2。轨迹仍须覆盖至少240个u8并通过完整拟合门限，所以中途卡滞不会被误判
|
||||
为端点到达。
|
||||
|
||||
右手拇指横摆电机10在命令255时多次实测稳定饱和在250，因此仅右手电机10的255端
|
||||
使用±5反馈容差；其命令0端实测反馈为4，仍使用±4，其他电机和中间位置不放宽。基准姿态、作为
|
||||
电机0辅助避挡姿态以及电机10自身扫描端点都使用同一条专用判定。
|
||||
|
||||
三机位流程默认 `validation_enabled:=false`，即不增加随机机械动作，但第三轮留出验证
|
||||
始终启用且不能关闭；最终 `quality.validation_mae_rad/p95_rad` 正是第三轮轨迹误差。
|
||||
状态中的扫描进度和总体进度分开显示：42/42只表示计划轨迹已采完，总体进度在拟合和
|
||||
验证完成、正式JSON与URDF成功写入之前不会显示100%。
|
||||
|
||||
上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag
|
||||
和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求
|
||||
当前活动机位就绪；首次调用 `start` 仍要求三个机位全部通过预检。
|
||||
|
||||
### 3. 输出
|
||||
|
||||
通过后生成精简JSON和一个新URDF：
|
||||
|
||||
```text
|
||||
calibration_output/G20_LEFT_001/<时间戳>/
|
||||
g20_left_G20_LEFT_001_calibration.json
|
||||
src/.../g20_left/
|
||||
linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
|
||||
|
||||
calibration_output/G20_RIGHT_001/<时间戳>/
|
||||
g20_right_G20_RIGHT_001_calibration.json
|
||||
src/.../g20_right/
|
||||
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
|
||||
```
|
||||
|
||||
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
|
||||
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
|
||||
指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
|
||||
覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
|
||||
改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
|
||||
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
|
||||
|
||||
完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
|
||||
命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖：
|
||||
|
||||
```bash
|
||||
python3 -m g20_thumb_apriltag_calibration.offline_replay \
|
||||
calibration_output/G20_RIGHT_001/20260811_120146 \
|
||||
--output-tag AXIS_FRAME_V3 \
|
||||
--write
|
||||
```
|
||||
|
||||
下面保留原有正面拇指独立标定说明和兼容入口。
|
||||
|
||||
### 4. 修正URDF的运行时关节映射
|
||||
|
||||
修正URDF已经把 `zero_angles.urdf_zero_offset_rad` 写入关节
|
||||
`origin.rpy`。仿真运行时只能再使用同一台、同一侧机械手JSON中的256点
|
||||
`angle_rad` 动态曲线，不能把 `urdf_zero_offset_rad` 再加一次，也不能把左手曲线
|
||||
用于右手URDF。可用桥接节点将GUI的20通道u8命令转换为完整21关节
|
||||
`JointState`（包括5个被动关节）：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
|
||||
hand_type:=right \
|
||||
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
|
||||
```
|
||||
|
||||
默认订阅 `/cb_right_hand_control_cmd`，发布
|
||||
`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
|
||||
发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
|
||||
因此不会静默退回旧标定。
|
||||
|
||||
该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`apriltag_ros`、
|
||||
Linker Hand SDK 和标定状态机，
|
||||
只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次
|
||||
终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置，
|
||||
程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。
|
||||
完成 `255→0→255` 后分别拟合正反方向并检查回差，最终运行时 JSON 将两条曲线逐点
|
||||
平均，只为每个关节保存一个 256 项 `angle_rad`。最后用 5 个随机静态命令复测精度。
|
||||
|
||||
当前默认使用 `trajectory_center_3d`。节点由四个亚像素角点和 `CameraInfo.P`
|
||||
计算每张 Tag 的三维中心，但不把小尺寸平面 Tag 的 PnP 朝向直接当作关节角：
|
||||
|
||||
- 根部扫描先减去掌心 T0 的位置，再用 T3/T4/T5 三条圆轨迹共同拟合 CMC 旋转轴；
|
||||
每帧三个角度取中位数。
|
||||
- 尖部扫描用 T4 相对 T3 的圆轨迹直接拟合 MCP。G20 只有电机 15 这一个尖部输入，
|
||||
URDF 将被动 IP 定义为 `thumb_ip = 1.02 × thumb_mcp`，因此运行时 IP 曲线严格按
|
||||
这个机械耦合生成。这样不会把不同相机角度下 T5 的平面 PnP 深度偏差误认为 IP
|
||||
真实运动。
|
||||
- 程序仍会按 MCP 角将 T5 反向旋转并拟合剩余小圆，但该结果只用于
|
||||
`trajectory_center_quality.tip` 中的观测一致性诊断，不参与最终 IP 数组。
|
||||
- 每条曲线都减去命令 255 的测量角，所以最终文件严格满足
|
||||
`angle_rad[255] == 0.0`；`angle_rad[0]` 是该关节相对零位的最大角度。
|
||||
|
||||
这种方法对固定的相机摆放角度、Tag 在同一刚性连杆上的固定位置和贴纸朝向更不敏感。
|
||||
但相机或贴纸在一次扫描过程中移动、Tag 翘起、角点严重抖动仍会破坏圆轨迹。程序会
|
||||
检查平面残差、圆残差、轨迹半径、实际弧长和根部三个轨迹点的角度一致性。
|
||||
|
||||
PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维中心及辅助质量检查，不再
|
||||
直接生成运行时角度。根部扫描用固定的 T3–T4、T4–T5 中心间距共同选择分支；
|
||||
尖部扫描用固定的 T0–T3 中心间距约束非目标部分。中心间距漂移超过阈值仍会暂停，
|
||||
避免错误中心进入圆拟合，但 Tag 的 PnP 朝向抖动不会触发该门限。
|
||||
|
||||
## 1. 标记和安全检查
|
||||
|
||||
- `T0` 必须保留并固定在掌壳，作为整体平移参考；`T3` 固定在拇指根部运动连杆，`T4` 固定在 MCP 后的连杆，
|
||||
`T5` 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。
|
||||
- 当前实物使用 `tag36h11` 的 ID `0/1/2/3`，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改
|
||||
`config/front_tags.yaml` 里检测节点和标定节点的两组数组。
|
||||
- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前实物黑色正方形实测为
|
||||
`16 mm`，因此配置为 `0.016`。
|
||||
测量检测角点所围成的正方形边长，不包含外围白色留边。
|
||||
- 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由
|
||||
静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时，
|
||||
启动参数增加 `publish_debug_image:=true`，再订阅
|
||||
`/g20_thumb_calibration/debug_image`；正式长时间扫描建议保持默认关闭。
|
||||
- 执行全行程前清空拇指周围空间并准备断开电机电源。确认这只手的下标 0 和 15
|
||||
均可安全走完整 `255→0→255`。标定节点发现命令话题上另有发布者时不会解锁扫描。
|
||||
|
||||
## 2. 安装与构建
|
||||
|
||||
```bash
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y \
|
||||
ros-jazzy-image-pipeline \
|
||||
ros-jazzy-apriltag-ros \
|
||||
ros-jazzy-apriltag-msgs \
|
||||
ros-jazzy-camera-calibration \
|
||||
python3-yaml
|
||||
|
||||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
colcon build --symlink-install \
|
||||
--packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
相机节点直接使用海康 MVS SDK。当前机器的默认安装位置是 `/opt/MVS`，需要存在：
|
||||
|
||||
```text
|
||||
/opt/MVS/lib/64/libMvCameraControl.so
|
||||
/opt/MVS/Samples/64/Python/MvImport/MvCameraControl_class.py
|
||||
```
|
||||
|
||||
正面相机默认按序列号 `DB2163742` 绑定（MVS 显示的 GUID 是
|
||||
`2BDFB2163742`），型号校验为 `MV-CS020-10UM`。三台相机同时连接时程序不会按枚举
|
||||
顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。
|
||||
|
||||
`1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。
|
||||
三相机标定 launch 会固定使用 `rmw_fastrtps_cpp`，并通过新旧两个 Fast DDS 环境变量
|
||||
加载 `config/fastdds_large_images.xml`，使用 64 MB 共享内存段；否则相机内部虽为 30 Hz，
|
||||
大图订阅端通常只能收到约 1～4 Hz。修改配置后必须重启相关 ROS 进程才能生效。
|
||||
|
||||
首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从
|
||||
`~/.ros/camera_info/hikrobot_DB2163742.yaml` 加载标准 ROS CameraInfo YAML；文件缺失时
|
||||
仍可预览 `mono8` 原图，但发布的内参无效，轨迹标定预检不会解锁运动。
|
||||
|
||||
先单独启动相机（不会连接机械手，也不会发送关节命令）：
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
|
||||
--remap __ns:=/camera/camera/color \
|
||||
-p serial_number:=DB2163742 \
|
||||
-p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
```
|
||||
|
||||
测速时优先检查同帧发布的小消息和原图；两者正常值都应接近 30 Hz：
|
||||
|
||||
```bash
|
||||
ros2 topic hz /camera/camera/color/camera_info
|
||||
ros2 topic hz /camera/camera/color/image_raw
|
||||
```
|
||||
|
||||
使用标定板采集内参。下面的 `8x6` 是内角点数量、`0.020` 是单格边长 20 mm，必须按
|
||||
实际标定板修改：
|
||||
|
||||
```bash
|
||||
ros2 run camera_calibration cameracalibrator \
|
||||
--size 8x6 --square 0.020 \
|
||||
--camera_name hikrobot_front_DB2163742 \
|
||||
--ros-args \
|
||||
--remap image:=/camera/camera/color/image_raw \
|
||||
--remap camera/set_camera_info:=/camera/camera/color/set_camera_info
|
||||
```
|
||||
|
||||
在标定界面完成采样后点击 `CALIBRATE`，确认重投影误差，再点击 `COMMIT`。相机节点会
|
||||
原子写入上述 YAML，并立即开始发布有效内参。内参只适用于标定时的镜头焦距、对焦、
|
||||
分辨率和 ROI；改变任何一项都要重新标定。
|
||||
|
||||
连接 CAN 后先确认 `can0` 已启动。不要同时运行其他会发布
|
||||
`/g20/cb_left_hand_control_cmd` 的程序。
|
||||
|
||||
## 3. 启动和操作
|
||||
|
||||
首次使用时可先用 `commands_enabled:=false` 做预检；SDK 仍会设置速度/扭矩并读取状态，
|
||||
但标定节点不会发送位置运动命令，也不会允许解锁全行程扫描：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_serial_number:=DB2163742 \
|
||||
commands_enabled:=false
|
||||
```
|
||||
|
||||
确认 T0、T3、T4、T5 在根部和尖部全行程中不会被遮挡，且拇指运动不会碰撞后，
|
||||
停止预检并启动一个新的正式会话。默认使用 AprilTag 内部 `decimate=1.5` 提升检测
|
||||
速度，并使用单终点连续运动：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_serial_number:=DB2163742 \
|
||||
can_interface:=can0 \
|
||||
calibration_speed:=15 \
|
||||
continuous_motion_mode:=endpoint \
|
||||
angle_estimation_mode:=trajectory_center_3d \
|
||||
apriltag_decimate:=1.5 \
|
||||
use_roi:=false
|
||||
```
|
||||
|
||||
默认关闭 ROI，AprilTag 使用完整的 1624×1240 校正画面。查看实际送入 AprilTag
|
||||
的完整画面：
|
||||
|
||||
```bash
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/camera/camera/color/image_rect
|
||||
```
|
||||
|
||||
图像检测链路使用 `sensor_data`（BEST_EFFORT）QoS，只保留最新帧，避免完整分辨率
|
||||
下可靠队列积压反压相机；这不会裁剪图像，也不会降低相机分辨率。
|
||||
|
||||
若以后需要以帧率优先，可传入 `use_roi:=true`；默认 ROI 是原图中的
|
||||
`x=128, y=192, width=1024, height=528`，也可用 `roi_x`、`roi_y`、
|
||||
`roi_width`、`roi_height` 覆盖。
|
||||
|
||||
监控状态：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /g20_thumb_calibration/status
|
||||
```
|
||||
|
||||
预检通过后状态为 `WAIT_ROOT_CONFIRM`，`reason` 为 `call_start`。只需调用一次：
|
||||
|
||||
```bash
|
||||
ros2 service call /g20_thumb_calibration/start std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
节点随后自动完成下标 0 的 `255→0→255`、下标 15 的 `255→0→255` 和 5 点随机复测，
|
||||
正常结束状态为 `COMPLETE`，无需在根部和尖部之间再次确认。为安全起见，调用 `start`
|
||||
前必须一次性确认两个关节的完整行程都已清空。原来的
|
||||
`confirm_root_full_range`、`confirm_tip_full_range` 服务仍保留用于兼容。
|
||||
|
||||
暂停、恢复和终止：
|
||||
|
||||
```bash
|
||||
ros2 service call /g20_thumb_calibration/pause std_srvs/srv/Trigger {}
|
||||
ros2 service call /g20_thumb_calibration/resume std_srvs/srv/Trigger {}
|
||||
ros2 service call /g20_thumb_calibration/abort std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
预检要求四 Tag 有效帧率至少 95%，且检测消息频率至少 15 Hz。PnP 有效率也必须
|
||||
至少 95%，每个候选解的重投影 RMS 不超过 1.5 px。中心轨迹模式以三组相对中心
|
||||
的静止 RMS 不超过 2 mm、5 mm 范围内位置内点不少于 90% 为硬判据；PnP 朝向抖动
|
||||
只作为诊断，不会阻止静态捕获。
|
||||
状态中的
|
||||
`pnp_rejections` 会指出当前是哪张 Tag 因丢失、重投影/倾角超限或姿态跳变而被拒绝，
|
||||
`pnp_reprojection_error_px` 显示四张 Tag 最近一次有效解的误差。连续扫描要求
|
||||
图像与状态的时间差不超过 150 ms、全行程至少得到 40 个有效帧、
|
||||
至少覆盖 32 个整数位置且相邻实测位置间隔不超过 16。Tag 或同步状态持续丢失 3 秒、
|
||||
90 秒内未到达终点，或覆盖不足时，节点保持当前命令并进入 `PAUSED`。恢复时会先回到
|
||||
该方向的起点，再完整重扫这个方向，避免把半程数据混入结果。`abort` 也只停止队列，
|
||||
不会主动移动机械手。正常扫描和随机复测最后一项均为命令 255。
|
||||
|
||||
PnP 跟踪在整个会话中对四张 Tag 都优先保持同一个 IPPE 平面分支；最多 5 秒的短暂检测
|
||||
间隔不会重新初始化分支。随机复测只有在同步电机反馈与目标相差不超过 2、且稳定
|
||||
窗口与捕获窗口内三个相对中心的最大偏差都不超过 3 mm 时才会写入，否则继续等待并最终暂停，
|
||||
不会再生成明知不可靠但字段完整的结果。
|
||||
|
||||
单终点连续模式共有 4 个端到端命令：根部和尖部各一个往返。每个方向运动前会先用
|
||||
实际电机反馈确认已经到达起点，再做一次短暂静态确认；随机验证的“接近位置”只等待
|
||||
电机反馈到位，不再重复采图。若实际 AprilTag 检测仍低于 15 Hz，先优化检测链路，
|
||||
不要降低到固件低速区。必须临时回退时可启动
|
||||
`continuous_motion_mode:=paced`，该模式按步长 8 到位即发下一段。
|
||||
|
||||
连续扫描中的主要状态字段：
|
||||
|
||||
- `state_zh`/`reason_zh`/`action_zh`：当前阶段、失败原因和下一步操作的中文说明；
|
||||
原有 `state`/`reason` 英文机器码继续保留。
|
||||
- `tag_quality`：逐张显示 T0/T3/T4/T5 的边长、hamming、识别置信度、重投影误差、
|
||||
是否有效和具体中文问题，不再需要手工解析 `/apriltag/detections`。
|
||||
- `/g20_thumb_calibration/status_text`：适合终端直接查看的多行中文状态。使用
|
||||
`ros2 topic echo --once /g20_thumb_calibration/status_text --field data`
|
||||
即可看到原因、建议及四张标签的质量。
|
||||
- `scan_progress`：4 个方向的完成比例，依次约为 0、0.25、0.5、0.75、1.0。
|
||||
- `sweep_valid_frames_seen`：当前连续方向已收到的同步有效帧数。
|
||||
- `sweep_state_span_u8`：当前方向实际覆盖的电机范围，接近 255 才算完整。
|
||||
- `active_phase`/`active_direction`：当前是根部或尖部、下降或上升方向。
|
||||
- `pnp_branch_corrections`：四张 Tag 联合跟踪为维持相邻关节姿态连续，而没有选择
|
||||
单张 Tag 最小重投影分支的累计次数。
|
||||
- `pnp_trajectory_quality`：最近一个完整方向的整段分支修正帧数，以及相对整段稳健
|
||||
参考的旋转、相对平移和中心间距漂移。中心轨迹模式只按欧氏中心间距判断：
|
||||
P95 超过 3 mm 或单帧最大值超过 6 mm 时暂停；旋转及随 Tag 坐标轴表达的相对平移
|
||||
只保留为诊断。
|
||||
- `trajectory_center_quality`：四个方向完成并拟合后，显示三维平面/圆残差、拟合半径、
|
||||
实际弧长、T0/T3 锚点漂移和根部三个轨迹点的角度一致性。其中
|
||||
`tip.ip_observed_vs_constrained_*` 显示T5残余小圆与URDF被动耦合之间的差异；
|
||||
它用于发现T5识别误差、标签松动或机构异常，但不会改变最终IP曲线。
|
||||
|
||||
根部扫描中 T3/T4/T5 作为完整刚性组共同选择 IPPE 分支，不再把 T3 固定为在线解；
|
||||
尖部扫描仍固定 T3，只用静止的 T0/T3 约束修正非目标根部姿态。
|
||||
|
||||
## 4. 中断恢复和输出
|
||||
|
||||
默认会话目录是启动命令当前目录下：
|
||||
|
||||
```text
|
||||
calibration_output/<序列号>/<时间戳>/
|
||||
```
|
||||
|
||||
恢复时必须显式复用原目录，否则会创建新会话：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
|
||||
```
|
||||
|
||||
恢复会校验序列号、Tag 配置、基准命令、扫描模式、采集参数和代码哈希；
|
||||
任一项变化都会拒绝混用旧样本，
|
||||
此时应新建会话。
|
||||
|
||||
目录内文件：
|
||||
|
||||
- `raw_samples.jsonl`：连续帧按实际整数电机位置分箱后的 Tag 三维中心、姿态辅助统计及复测点；每完成一个
|
||||
扫描方向后落盘。
|
||||
- `checkpoint.json`：当前状态和进度。
|
||||
- `session_manifest.json`：Tag、相机内参、SDK、代码哈希和会话信息。
|
||||
- `validation.json`：随机复测及全部质量判据。
|
||||
- `rosbag/`：仅在 `record_bag:=true` 时生成，用于保存相机、检测、命令和状态等诊断数据。
|
||||
- `g20_left_<序列号>_thumb_angle.json`：精简后的运行时标定文件。
|
||||
|
||||
最终文件使用 `schema_version: 2`。每个关节只包含：
|
||||
|
||||
```json
|
||||
{
|
||||
"motor_index": 0,
|
||||
"angle_rad": ["按命令0～255索引的256个弧度值"]
|
||||
}
|
||||
```
|
||||
|
||||
`thumb_ip.angle_rad` 由 `thumb_mcp.angle_rad` 乘
|
||||
`ip_coupling.multiplier`（默认 `1.02`）得到，二者在命令255处都严格为零。
|
||||
|
||||
`thumb_ip` 另外包含 `"passive": true`。正反方向原始曲线不进入最终 JSON，但仍保留
|
||||
在 `raw_samples.jsonl` 中，并用于最大回差和质量判定。
|
||||
|
||||
零位和最大角度可直接读取：
|
||||
|
||||
```python
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
data = json.loads(Path("g20_left_G20_LEFT_001_thumb_angle.json").read_text())
|
||||
for name, joint in data["joints"].items():
|
||||
print(name, "zero(rad)=", joint["angle_rad"][255],
|
||||
"max(rad)=", joint["angle_rad"][0])
|
||||
```
|
||||
|
||||
如果相机或 SDK 已由外部进程启动，可传
|
||||
`start_camera:=false` 或 `start_sdk:=false`。`camera_serial_number` 同时接受 MVS
|
||||
序列号和 GUID，但推荐使用稳定且简短的序列号 `DB2163742`。
|
||||
|
||||
海康相机默认输出 `1624x1240@30Hz mono8`，全局快门，曝光时间 `5000us`、增益
|
||||
`0dB`，并使用“只取最新帧”策略避免视觉延迟。现场亮度不足时优先增加照明；必要时可用
|
||||
`exposure_time_us`、`gain_db` 调整，或临时传 `auto_exposure:=true`。正式轨迹采集建议固定
|
||||
曝光，避免自动曝光在运动过程中改变角点质量。rosbag 默认关闭；需要诊断留档时增加
|
||||
`record_bag:=true`。
|
||||
|
||||
默认对完整 1624×1240 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
|
||||
`image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。
|
||||
校正和 AprilTag 组件运行
|
||||
在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。
|
||||
可选 ROI 模式会额外在同一容器内加入裁剪组件并同步修正 `CameraInfo`。标定节点默认
|
||||
不订阅整幅图像，只订阅检测结果和 TF。
|
||||
若启用调试图，预览会缩放到 50%、限速 10 Hz 并使用最新帧优先的传输方式，
|
||||
不影响 AprilTag 的 ROI 输入。
|
||||
|
||||
静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
|
||||
当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
|
||||
相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
|
||||
|
||||
启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
|
||||
订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制；
|
||||
若用于正式可追溯验收，再启用并检查磁盘空间。
|
||||
|
||||
## 5. CMC Pitch 零位角测量
|
||||
|
||||
只测量命令 255 时 `thumb_cmc_pitch` 的画面水平投影零位角时，使用独立启动文件。
|
||||
它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射，也不会生成或修改
|
||||
URDF：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
front_cmc_pitch_zero.launch.py \
|
||||
serial_number:=G20_LEFT_001
|
||||
```
|
||||
|
||||
该流程只要求 T0（ID 0）和 T3（ID 1）有效。T4/T5 可以留在手上，但丢失不会阻塞。
|
||||
预检完成后查看中文状态：
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once --full-length \
|
||||
/g20_thumb_cmc_pitch_zero/status_text \
|
||||
--field data
|
||||
```
|
||||
|
||||
状态显示“等待开始”后启动三轮测量：
|
||||
|
||||
```bash
|
||||
ros2 service call \
|
||||
/g20_thumb_cmc_pitch_zero/start \
|
||||
std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
查看带红色画面水平线、T0/T3标签中心、青色轨迹点、紫色拟合圆心和径向零位线
|
||||
的调试画面：
|
||||
|
||||
```bash
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_thumb_cmc_pitch_zero/debug_image
|
||||
```
|
||||
|
||||
画面底部红线是固定的相机水平与构图目标。程序会在画面下部自动寻找一条足够长、
|
||||
接近水平的物理桌边或高对比参考直线，并画成蓝线。开始前调整相机，使蓝线与红线
|
||||
重合；画面和 `status_text` 会实时显示红蓝线夹角及垂直偏差。`±0.5°` 和
|
||||
`±12 px` 只用于显示 `ALIGNED/ADJUST`，完全不参与预检或 `start` 服务判断。
|
||||
由操作人员确认相机位置后手动开始标定。参考直线应清晰、连续并尽量横跨画面；
|
||||
该检测不使用 T0/T3 标签朝向。
|
||||
|
||||
一条二维直线只能确认相机滚转角和上下构图位置，不能单独证明相机的距离、俯仰、
|
||||
偏航或完整三维位置。若需要严格复现这些量，还应使用固定相机支架或专用标定板。
|
||||
|
||||
每轮只控制电机 0 执行一次 `255→64` 端点运动和一次 `64→255` 返回运动。运动期间
|
||||
连续采集 `T3中心−T0中心`，按机械手状态分箱后拟合图像平面圆；返回 255 后使用
|
||||
“T3零位中心→拟合圆心”的固定内向径向矢量计算角度。运动前和返回后各采集30帧静态零位，
|
||||
三轮轨迹合并后得到最终圆心。其他 19 个命令保持固定基准。任何其他节点同时发布
|
||||
`/g20/cb_left_hand_control_cmd` 时，`start` 服务会拒绝启动。
|
||||
|
||||
T0中心用于消除相机或整只手的平移抖动。T0和T3标签自身的朝向与角点 `+x`
|
||||
都不参与零位或行程计算；标签可以任意平面内旋转或反贴180°，只需标签平整、
|
||||
固定且中心始终可见。若轨迹跨度、圆弧、半径、径向RMS/P95或回零误差不合格，
|
||||
节点暂停或写出 `quality.passed=false`。
|
||||
|
||||
完成后只生成：
|
||||
|
||||
```text
|
||||
calibration_output/G20_LEFT_001/<时间戳>/
|
||||
g20_left_G20_LEFT_001_thumb_cmc_pitch_zero.json
|
||||
```
|
||||
|
||||
核心字段是：
|
||||
|
||||
```text
|
||||
zero_angles.table_projected_zero_rad
|
||||
```
|
||||
|
||||
该值是内向径向零位矢量相对相机画面水平向右方向的角度。它不使用 T0 的方向，
|
||||
但会用 T0 中心抵消平移；它仍不是真实三维桌面检测，因此会随相机滚转和机械手
|
||||
摆放改变。
|
||||
|
||||
## 6. CMC Roll 零位与行程标定
|
||||
|
||||
`thumb_cmc_roll` 复用上节的 T0 平移补偿、T3 中心轨迹分箱和稳健圆拟合，
|
||||
但控制的是电机 5。每轮执行 `255→0→255`：在 255 零位、0 行程端点和返回
|
||||
255 后各静态采集 30 帧，因此可以同时测量零位角和完整 `0～255` 实际角行程。
|
||||
命令 0 是完整行程端点，开始前必须确认拇指没有机械碰撞或硬限位顶死风险。
|
||||
Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量，不读取T3标签朝向。
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
front_cmc_roll_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001
|
||||
```
|
||||
|
||||
预检通过后启动三轮标定：
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once --full-length \
|
||||
/g20_thumb_cmc_roll_calibration/status_text \
|
||||
--field data
|
||||
|
||||
ros2 service call \
|
||||
/g20_thumb_cmc_roll_calibration/start \
|
||||
std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
调试画面：
|
||||
|
||||
```bash
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_thumb_cmc_roll_calibration/debug_image
|
||||
```
|
||||
|
||||
Roll 使用与 Pitch 相同的红蓝参考线显示，但是否对齐由操作人员确认，程序不会用
|
||||
蓝线状态阻止 `start` 进入电机运动。
|
||||
|
||||
完成后生成：
|
||||
|
||||
```text
|
||||
calibration_output/G20_LEFT_001/<时间戳>/
|
||||
g20_left_G20_LEFT_001_thumb_cmc_roll_zero_travel.json
|
||||
```
|
||||
|
||||
核心输出字段：
|
||||
|
||||
```text
|
||||
zero_angles.table_projected_zero_rad
|
||||
travel.signed_rad
|
||||
travel.range_rad
|
||||
```
|
||||
|
||||
`travel.signed_rad` 是从命令 255 到 0 的有符号转角，`travel.range_rad` 是三轮
|
||||
行程大小的中值。只有轨迹圆质量、T0/T3 检出率、三轮零位/行程一致性、端点径向
|
||||
误差和回零误差全部通过时，`quality.passed` 才为 `true`。
|
||||
@@ -1,120 +0,0 @@
|
||||
g20_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /g20/cb_left_hand_control_cmd
|
||||
state_topic: /g20/cb_left_hand_state
|
||||
info_topic: /g20/cb_left_hand_info
|
||||
setting_topic: /g20/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /g20_calibration/front/camera/camera_info
|
||||
front_detections_topic: /g20_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /g20_calibration/side/camera/camera_info
|
||||
side_detections_topic: /g20_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /g20_calibration/top/camera/camera_info
|
||||
top_detections_topic: /g20_calibration/top/apriltag/detections
|
||||
|
||||
# /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
normal_calibration_speed: 15
|
||||
index_roll_calibration_speed: 5
|
||||
index_flex_calibration_speed: 10
|
||||
speed_setting_settle_seconds: 0.25
|
||||
|
||||
# tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。
|
||||
tag_size_m: 0.016
|
||||
repetitions: 3
|
||||
preflight_frames: 60
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_detection_hz: 15.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
pnp_reprojection_tie_px: 1.5
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
pnp_maximum_translation_jump_m: 0.04
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
pnp_tracker_reset_seconds: 5.0
|
||||
# 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
|
||||
pnp_group_initialization_frames: 8
|
||||
# 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
|
||||
pnp_group_normal_alignment_scale_deg: 5.0
|
||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||
top_pnp_invalid_reset_seconds: 1.0
|
||||
# 三维位姿必须与实测20通道状态严格按时间戳配对。
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
|
||||
axis_maximum_plane_rms_m: 0.003
|
||||
# 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
|
||||
# 轴线一致性仍沿用严格检查。
|
||||
passive_axis_maximum_plane_rms_m: 0.004
|
||||
axis_maximum_radial_rms_m: 0.003
|
||||
# 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
|
||||
axis_maximum_pose_line_rms_m: 0.001
|
||||
# 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
|
||||
# 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
|
||||
axis_maximum_rotation_circle_difference_deg: 1.0
|
||||
# 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
|
||||
# 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及第三轮留出。
|
||||
active_maximum_rotation_orthogonal_rms_deg: 2.5
|
||||
passive_maximum_rotation_orthogonal_rms_deg: 7.5
|
||||
zero_maximum_axis_cycle_difference_deg: 0.75
|
||||
# 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
|
||||
zero_maximum_axis_cone_mismatch_deg: 5.0
|
||||
zero_maximum_offset_deg: 20.0
|
||||
# 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。
|
||||
zero_finger_maximum_offset_deg: 3.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
# 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
|
||||
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
|
||||
# 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
|
||||
right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
|
||||
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
|
||||
pinky_pip_zero_endpoint_tolerance_u8: 5.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
baseline_hold_seconds: 0.5
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
# 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停；
|
||||
# 这类故障不进入遮挡/超时的三次自动重扫。
|
||||
motor_stall_timeout_seconds: 8.0
|
||||
motor_stall_minimum_progress_u8: 1.0
|
||||
invalid_timeout_seconds: 3.0
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
# 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
|
||||
automatic_sweep_retry_limit: 3
|
||||
# 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
|
||||
automatic_fit_retry_limit: 2
|
||||
automatic_motion_retry_limit: 2
|
||||
# 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
|
||||
provisional_warning_ratio: 1.25
|
||||
retry_minimum_speed: 3
|
||||
retry_speed_scales: [0.8, 0.6, 0.5]
|
||||
retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
|
||||
|
||||
trajectory_maximum_plane_rms_m: 0.004
|
||||
trajectory_maximum_radial_rms_m: 0.004
|
||||
trajectory_minimum_radius_m: 0.003
|
||||
trajectory_minimum_arc_deg: 15.0
|
||||
# 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
|
||||
image_trajectory_maximum_radial_rms_px: 2.0
|
||||
image_trajectory_maximum_radial_p95_px: 3.5
|
||||
image_trajectory_minimum_radius_px: 20.0
|
||||
trajectory_maximum_cycle_travel_difference_deg: 3.0
|
||||
passive_maximum_cycle_travel_difference_deg: 10.0
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
maximum_hysteresis_deg: 5.0
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
passive_maximum_hysteresis_deg: 7.5
|
||||
|
||||
# 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。
|
||||
validation_enabled: false
|
||||
validation_command_count: 3
|
||||
validation_frames: 10
|
||||
validation_seed: 20260804
|
||||
validation_timeout_seconds: 20.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
@@ -1,5 +0,0 @@
|
||||
"""Front-camera AprilTag calibration for the left LinkerHand G20 thumb."""
|
||||
|
||||
from .core import BASELINE_COMMAND, COMMAND_NAMES
|
||||
|
||||
__all__ = ["BASELINE_COMMAND", "COMMAND_NAMES"]
|
||||
-218
@@ -1,218 +0,0 @@
|
||||
"""Map G20 u8 commands to URDF joint angles using one calibration JSON.
|
||||
|
||||
The static encoder-zero corrections in ``zero_angles`` are already baked into
|
||||
the corrected URDF joint origins. This bridge therefore publishes only the
|
||||
dynamic ``angle_rad`` values and never adds the static offsets a second time.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .full_hand import get_hand_calibration_profile, validate_compact_payload
|
||||
|
||||
|
||||
G20_COMMAND_NAMES: tuple[str, ...] = (
|
||||
"thumb_cmc_pitch",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"reserved_11",
|
||||
"reserved_12",
|
||||
"reserved_13",
|
||||
"reserved_14",
|
||||
"thumb_mcp",
|
||||
"index_pip",
|
||||
"middle_pip",
|
||||
"ring_pip",
|
||||
"pinky_pip",
|
||||
)
|
||||
|
||||
# Match the stable ordering used by the existing MuJoCo bridge. JointState
|
||||
# consumers must use names, but retaining the ordering also keeps logs and
|
||||
# direct comparisons deterministic.
|
||||
G20_URDF_JOINT_NAMES: tuple[str, ...] = (
|
||||
"index_dip",
|
||||
"index_mcp_pitch",
|
||||
"index_mcp_roll",
|
||||
"index_pip",
|
||||
"middle_dip",
|
||||
"middle_mcp_pitch",
|
||||
"middle_mcp_roll",
|
||||
"middle_pip",
|
||||
"pinky_dip",
|
||||
"pinky_mcp_pitch",
|
||||
"pinky_mcp_roll",
|
||||
"pinky_pip",
|
||||
"ring_dip",
|
||||
"ring_mcp_pitch",
|
||||
"ring_mcp_roll",
|
||||
"ring_pip",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_ip",
|
||||
"thumb_mcp",
|
||||
)
|
||||
|
||||
|
||||
class CalibratedCommandMapper:
|
||||
"""Validated, side-specific lookup from a G20 command to URDF radians."""
|
||||
|
||||
def __init__(
|
||||
self, payload: Mapping[str, Any], *, expected_side: str | None = None
|
||||
) -> None:
|
||||
validate_compact_payload(payload)
|
||||
side = str(payload["side"]).lower()
|
||||
if expected_side is not None and side != str(expected_side).lower():
|
||||
raise ValueError(
|
||||
f"calibration side {side!r} does not match requested side "
|
||||
f"{str(expected_side).lower()!r}"
|
||||
)
|
||||
quality = payload["quality"]
|
||||
if quality.get("passed") is not True:
|
||||
raise ValueError("calibration quality.passed must be true")
|
||||
profile = get_hand_calibration_profile(side)
|
||||
self.side = side
|
||||
self.serial_number = str(payload["serial_number"])
|
||||
self._motor_by_joint = {
|
||||
name: int(profile.joint_specs[name].motor_index)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def _command_index(value: float) -> int:
|
||||
command = float(value)
|
||||
if not math.isfinite(command):
|
||||
raise ValueError("G20 command positions must be finite")
|
||||
return max(0, min(255, int(math.floor(command + 0.5))))
|
||||
|
||||
def map_positions(
|
||||
self, positions: Sequence[float], names: Sequence[str] = ()
|
||||
) -> tuple[float, ...]:
|
||||
values = tuple(float(value) for value in positions)
|
||||
if names:
|
||||
if len(names) != len(values):
|
||||
raise ValueError(
|
||||
"JointState names and positions must have equal length"
|
||||
)
|
||||
if len(set(names)) != len(names):
|
||||
raise ValueError("JointState names must be unique")
|
||||
by_name = dict(zip((str(name) for name in names), values))
|
||||
missing = [name for name in G20_COMMAND_NAMES if name not in by_name]
|
||||
if missing:
|
||||
raise ValueError(
|
||||
"G20 command is missing named channels: " + ",".join(missing)
|
||||
)
|
||||
command = tuple(by_name[name] for name in G20_COMMAND_NAMES)
|
||||
else:
|
||||
if len(values) != len(G20_COMMAND_NAMES):
|
||||
raise ValueError(
|
||||
"unnamed G20 command must contain exactly 20 positions"
|
||||
)
|
||||
command = values
|
||||
indices = tuple(self._command_index(value) for value in command)
|
||||
return tuple(
|
||||
self._curves[name][indices[self._motor_by_joint[name]]]
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
)
|
||||
|
||||
|
||||
def load_calibrated_command_mapper(
|
||||
calibration_file: str | Path, *, expected_side: str | None = None
|
||||
) -> CalibratedCommandMapper:
|
||||
path = Path(calibration_file).expanduser().resolve()
|
||||
if not path.is_file():
|
||||
raise ValueError(f"calibration JSON does not exist: {path}")
|
||||
payload = json.loads(path.read_text(encoding="utf-8"))
|
||||
return CalibratedCommandMapper(payload, expected_side=expected_side)
|
||||
|
||||
|
||||
class CalibratedJointStateBridge(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("g20_calibrated_joint_state_bridge")
|
||||
self.declare_parameter("hand_type", "right")
|
||||
self.declare_parameter("calibration_file", "")
|
||||
self.declare_parameter("input_topic", "")
|
||||
self.declare_parameter("output_topic", "")
|
||||
|
||||
hand_type = str(self.get_parameter("hand_type").value).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise ValueError("hand_type must be left or right")
|
||||
calibration_file = str(self.get_parameter("calibration_file").value)
|
||||
if not calibration_file:
|
||||
raise ValueError("calibration_file is required")
|
||||
self.mapper = load_calibrated_command_mapper(
|
||||
calibration_file, expected_side=hand_type
|
||||
)
|
||||
input_topic = str(self.get_parameter("input_topic").value).strip()
|
||||
output_topic = str(self.get_parameter("output_topic").value).strip()
|
||||
self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd"
|
||||
self.output_topic = (
|
||||
output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
|
||||
)
|
||||
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
|
||||
self.subscription = self.create_subscription(
|
||||
JointState, self.input_topic, self._command_callback, 10
|
||||
)
|
||||
self._last_error = ""
|
||||
self.get_logger().info(
|
||||
f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
|
||||
f"{self.input_topic} -> {self.output_topic}"
|
||||
)
|
||||
|
||||
def _command_callback(self, command: JointState) -> None:
|
||||
try:
|
||||
positions = self.mapper.map_positions(command.position, command.name)
|
||||
except ValueError as error:
|
||||
message = str(error)
|
||||
if message != self._last_error:
|
||||
self.get_logger().error(message)
|
||||
self._last_error = message
|
||||
return
|
||||
self._last_error = ""
|
||||
result = JointState()
|
||||
result.header = command.header
|
||||
result.name = list(G20_URDF_JOINT_NAMES)
|
||||
result.position = list(positions)
|
||||
self.publisher.publish(result)
|
||||
|
||||
|
||||
def main(args: Sequence[str] | None = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node: CalibratedJointStateBridge | None = None
|
||||
try:
|
||||
node = CalibratedJointStateBridge()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,878 +0,0 @@
|
||||
"""Pure three-camera G20 calibration model and compact runtime schema.
|
||||
|
||||
The hardware node records one parent/child AprilTag trajectory for each
|
||||
directly observable joint. This module deliberately contains no ROS imports:
|
||||
curve fitting, baseline centring, inheritance, and schema validation
|
||||
remain deterministic and unit-testable without connected cameras or a hand.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, replace
|
||||
import math
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .trajectory import (
|
||||
_angle_for_circle,
|
||||
_fit_circle_with_axis,
|
||||
_fit_joint_curve,
|
||||
_fit_plane_axis,
|
||||
_orient_circle_positive,
|
||||
)
|
||||
from .zero_calibration import (
|
||||
_fit_circle,
|
||||
_trajectory_arc_rad,
|
||||
circular_median_rad,
|
||||
maximum_pairwise_angle_difference_rad,
|
||||
wrap_angle_rad,
|
||||
)
|
||||
|
||||
|
||||
THREE_CAMERA_BASELINE_COMMAND: tuple[int, ...] = (
|
||||
255, 255, 255, 255, 255, 255,
|
||||
127, 127, 127, 127,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointSpec:
|
||||
name: str
|
||||
motor_index: int
|
||||
active: bool
|
||||
view: str | None
|
||||
parent_role: str | None
|
||||
child_role: str | None
|
||||
source_joint: str | None = None
|
||||
zero_kind: str | None = None
|
||||
|
||||
@property
|
||||
def measured(self) -> bool:
|
||||
return self.source_joint is None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SweepSpec:
|
||||
view: str
|
||||
motor_index: int
|
||||
joints: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointCurveFit:
|
||||
angle_rad: tuple[float, ...]
|
||||
decreasing_rad: tuple[float, ...]
|
||||
increasing_rad: tuple[float, ...]
|
||||
circle: Mapping[str, Any]
|
||||
maximum_monotonic_correction_rad: float
|
||||
maximum_hysteresis_rad: float
|
||||
quality: Mapping[str, float]
|
||||
zero_offset_rad: float = 0.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HandCalibrationProfile:
|
||||
side: str
|
||||
reference_finger: str
|
||||
view_tags: Mapping[str, Mapping[str, int]]
|
||||
preflight_view_roles: Mapping[str, tuple[str, ...]]
|
||||
joint_specs: Mapping[str, JointSpec]
|
||||
sweep_specs: tuple[SweepSpec, ...]
|
||||
image_trajectory_joints: frozenset[str]
|
||||
roll_clearance_commands: Mapping[int, int]
|
||||
thumb_pitch_clearance_commands: Mapping[int, int]
|
||||
|
||||
@property
|
||||
def measured_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if spec.measured
|
||||
)
|
||||
|
||||
@property
|
||||
def active_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if spec.active
|
||||
)
|
||||
|
||||
@property
|
||||
def passive_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if not spec.active
|
||||
)
|
||||
|
||||
@property
|
||||
def reference_roll_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_mcp_roll"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_pitch_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_mcp_pitch"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_pip_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_pip"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_speed_slot(self) -> int:
|
||||
return {"index": 1, "middle": 2, "ring": 3, "pinky": 4}[
|
||||
self.reference_finger
|
||||
]
|
||||
|
||||
|
||||
_FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky")
|
||||
_MOTOR_BY_JOINT: dict[str, int] = {
|
||||
**{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)},
|
||||
**{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)},
|
||||
**{f"{finger}_pip": index + 16 for index, finger in enumerate(_FINGERS)},
|
||||
}
|
||||
|
||||
|
||||
def _build_hand_profile(side: str) -> HandCalibrationProfile:
|
||||
hand_side = str(side).lower()
|
||||
if hand_side not in {"left", "right"}:
|
||||
raise ValueError("hand side must be left or right")
|
||||
reference = "index" if hand_side == "left" else "pinky"
|
||||
roll_role = f"{reference}_roll"
|
||||
mcp_role = f"{reference}_mcp"
|
||||
pip_role = f"{reference}_pip"
|
||||
dip_role = f"{reference}_dip"
|
||||
view_tags: dict[str, dict[str, int]] = {
|
||||
"front": {
|
||||
"front_base": 0,
|
||||
"thumb_cmc": 1,
|
||||
"thumb_mcp": 2,
|
||||
"thumb_ip": 3,
|
||||
roll_role: 10,
|
||||
},
|
||||
"side": {
|
||||
"side_base": 4,
|
||||
mcp_role: 5,
|
||||
pip_role: 6,
|
||||
dip_role: 7,
|
||||
},
|
||||
"top": {"top_base": 8, "thumb_yaw": 9},
|
||||
}
|
||||
preflight_view_roles = {
|
||||
view: tuple(tags) for view, tags in view_tags.items()
|
||||
}
|
||||
joint_specs: dict[str, JointSpec] = {}
|
||||
# Both hands use the front base/Tag 1 pair for motor 0. Keeping the
|
||||
# complete thumb root chain in one calibrated view avoids introducing a
|
||||
# cross-camera axis-line phase into the URDF zero solve.
|
||||
thumb_pitch_view = "front"
|
||||
thumb_pitch_parent = "front_base"
|
||||
joint_specs["thumb_cmc_pitch"] = JointSpec(
|
||||
"thumb_cmc_pitch",
|
||||
0,
|
||||
True,
|
||||
thumb_pitch_view,
|
||||
thumb_pitch_parent,
|
||||
"thumb_cmc",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_mcp_pitch"
|
||||
source = None if finger == reference else f"{reference}_mcp_pitch"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"side" if source is None else None,
|
||||
"side_base" if source is None else None,
|
||||
mcp_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_cmc_roll"] = JointSpec(
|
||||
"thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_mcp_roll"
|
||||
source = None if finger == reference else f"{reference}_mcp_roll"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"front" if source is None else None,
|
||||
"front_base" if source is None else None,
|
||||
roll_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_cmc_yaw"] = JointSpec(
|
||||
"thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
joint_specs["thumb_mcp"] = JointSpec(
|
||||
"thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_pip"
|
||||
source = None if finger == reference else f"{reference}_pip"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"side" if source is None else None,
|
||||
mcp_role if source is None else None,
|
||||
pip_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_ip"] = JointSpec(
|
||||
"thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_dip"
|
||||
source = None if finger == reference else f"{reference}_dip"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[f"{finger}_pip"],
|
||||
False,
|
||||
"side" if source is None else None,
|
||||
pip_role if source is None else None,
|
||||
dip_role if source is None else None,
|
||||
source_joint=source,
|
||||
)
|
||||
sweep_specs = (
|
||||
SweepSpec(thumb_pitch_view, 0, ("thumb_cmc_pitch",)),
|
||||
SweepSpec("front", 5, ("thumb_cmc_roll",)),
|
||||
SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")),
|
||||
SweepSpec("front", _MOTOR_BY_JOINT[f"{reference}_mcp_roll"], (
|
||||
f"{reference}_mcp_roll",
|
||||
)),
|
||||
SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_mcp_pitch"], (
|
||||
f"{reference}_mcp_pitch",
|
||||
)),
|
||||
SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_pip"], (
|
||||
f"{reference}_pip", f"{reference}_dip",
|
||||
)),
|
||||
SweepSpec("top", 10, ("thumb_cmc_yaw",)),
|
||||
)
|
||||
image_joints = frozenset(
|
||||
{
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
f"{reference}_mcp_roll",
|
||||
f"{reference}_mcp_pitch",
|
||||
f"{reference}_pip",
|
||||
}
|
||||
)
|
||||
measured_roll = _MOTOR_BY_JOINT[f"{reference}_mcp_roll"]
|
||||
# The roll channels are mirrored mechanically between hands. Command 0
|
||||
# moves the three non-reference fingers to the camera-right clearance pose
|
||||
# on the left hand, while the same physical pose is command 255 on the
|
||||
# right hand. Using 0 for both sides made the right index/middle/ring
|
||||
# fingers lean left and could obstruct or load the pinky roll sweep.
|
||||
roll_clearance_command = 0 if hand_side == "left" else 255
|
||||
clearance = {
|
||||
_MOTOR_BY_JOINT[f"{finger}_mcp_roll"]: roll_clearance_command
|
||||
for finger in _FINGERS
|
||||
if finger != reference
|
||||
}
|
||||
assert measured_roll not in clearance
|
||||
# Motor 0 is observed from the front with both other thumb-root channels
|
||||
# fully open. Store the commands explicitly for the right hand so a
|
||||
# caller-provided baseline cannot silently restore the obsolete 50/120
|
||||
# pose and hide Tag 1 from the front camera.
|
||||
thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {}
|
||||
return HandCalibrationProfile(
|
||||
side=hand_side,
|
||||
reference_finger=reference,
|
||||
view_tags=view_tags,
|
||||
preflight_view_roles=preflight_view_roles,
|
||||
joint_specs=joint_specs,
|
||||
sweep_specs=sweep_specs,
|
||||
image_trajectory_joints=image_joints,
|
||||
roll_clearance_commands=clearance,
|
||||
thumb_pitch_clearance_commands=thumb_pitch_clearance,
|
||||
)
|
||||
|
||||
|
||||
LEFT_HAND_PROFILE = _build_hand_profile("left")
|
||||
RIGHT_HAND_PROFILE = _build_hand_profile("right")
|
||||
|
||||
|
||||
def get_hand_calibration_profile(side: str) -> HandCalibrationProfile:
|
||||
value = str(side).lower()
|
||||
if value == "left":
|
||||
return LEFT_HAND_PROFILE
|
||||
if value == "right":
|
||||
return RIGHT_HAND_PROFILE
|
||||
raise ValueError("hand side must be left or right")
|
||||
|
||||
|
||||
# Backwards-compatible aliases keep the existing left-hand API stable.
|
||||
VIEW_TAGS = LEFT_HAND_PROFILE.view_tags
|
||||
JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs
|
||||
SWEEP_SPECS = LEFT_HAND_PROFILE.sweep_specs
|
||||
MEASURED_JOINTS = LEFT_HAND_PROFILE.measured_joints
|
||||
ACTIVE_JOINTS = LEFT_HAND_PROFILE.active_joints
|
||||
PASSIVE_JOINTS = LEFT_HAND_PROFILE.passive_joints
|
||||
SPLAY_JOINTS: tuple[str, ...] = (
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
)
|
||||
|
||||
# These directly actuated motions have a fixed parent link during their sweep
|
||||
# and a motion plane that is close to the active camera's image plane. Their
|
||||
# projected circles are substantially more repeatable than the difference of
|
||||
# two independently estimated planar-Tag PnP depths. thumb_ip remains projected
|
||||
# because its front-view PnP depth is not repeatable enough for a 3-D fit. The
|
||||
# side-view index_dip and oblique top-view yaw remain parent-relative full
|
||||
# SE(3) measurements; their circle directions are constrained by the more
|
||||
# repeatable relative-orientation screw axis in urdf_zero.py.
|
||||
IMAGE_TRAJECTORY_JOINTS = LEFT_HAND_PROFILE.image_trajectory_joints
|
||||
|
||||
# Keep the three unmeasured finger-roll motors away from the front camera's
|
||||
# line of sight while index_mcp_roll is measured.
|
||||
INDEX_ROLL_CLEARANCE_COMMANDS = LEFT_HAND_PROFILE.roll_clearance_commands
|
||||
|
||||
# Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is
|
||||
# measured. This keeps the moving top-view tag sufficiently front-facing.
|
||||
THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = {
|
||||
5: 145,
|
||||
}
|
||||
|
||||
|
||||
def calibration_auxiliary_commands(
|
||||
spec: SweepSpec,
|
||||
*,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> dict[int, int]:
|
||||
"""Return motors that must remain fixed throughout one calibration task."""
|
||||
if spec.motor_index == 0:
|
||||
return dict(profile.thumb_pitch_clearance_commands)
|
||||
if spec.motor_index == profile.reference_roll_motor:
|
||||
return dict(profile.roll_clearance_commands)
|
||||
if spec.motor_index == 10:
|
||||
return dict(THUMB_YAW_CLEARANCE_COMMANDS)
|
||||
return {}
|
||||
|
||||
|
||||
def build_full_hand_command(
|
||||
motor_index: int,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 values")
|
||||
motor = int(motor_index)
|
||||
if motor not in {spec.motor_index for spec in profile.joint_specs.values()}:
|
||||
raise ValueError("motor_index is not a controlled G20 calibration channel")
|
||||
command = int(command_u8)
|
||||
if not 0 <= command <= 255:
|
||||
raise ValueError("command_u8 must be in [0, 255]")
|
||||
result = [int(value) for value in baseline]
|
||||
if any(not 0 <= value <= 255 for value in result):
|
||||
raise ValueError("baseline values must be in [0, 255]")
|
||||
result[motor] = command
|
||||
return result
|
||||
|
||||
|
||||
def build_calibration_motion_command(
|
||||
spec: SweepSpec,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
"""Build a sweep command, including any required clearance pose."""
|
||||
result = build_full_hand_command(
|
||||
spec.motor_index,
|
||||
command_u8,
|
||||
baseline=baseline,
|
||||
profile=profile,
|
||||
)
|
||||
for motor_index, auxiliary_command in calibration_auxiliary_commands(
|
||||
spec, profile=profile
|
||||
).items():
|
||||
result[motor_index] = auxiliary_command
|
||||
return result
|
||||
|
||||
|
||||
def build_calibration_speed_profile(
|
||||
spec: SweepSpec,
|
||||
*,
|
||||
normal_speed: int,
|
||||
index_roll_speed: int,
|
||||
index_flex_speed: int,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
"""Return G20's five per-finger speeds for one calibration task."""
|
||||
normal = int(normal_speed)
|
||||
index_roll = int(index_roll_speed)
|
||||
index_flex = int(index_flex_speed)
|
||||
if not all(
|
||||
0 <= speed <= 255 for speed in (normal, index_roll, index_flex)
|
||||
):
|
||||
raise ValueError("calibration speeds must be in [0, 255]")
|
||||
speeds = [normal] * 5
|
||||
if spec.motor_index == profile.reference_roll_motor:
|
||||
speeds[profile.reference_speed_slot] = index_roll
|
||||
elif spec.motor_index in {
|
||||
profile.reference_pitch_motor,
|
||||
profile.reference_pip_motor,
|
||||
}:
|
||||
speeds[profile.reference_speed_slot] = index_flex
|
||||
return speeds
|
||||
|
||||
|
||||
def _record_vector(record: Mapping[str, Any]) -> np.ndarray:
|
||||
value = np.asarray(record.get("relative_translation_xyz_m"), dtype=float)
|
||||
if value.shape != (3,) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("record relative_translation_xyz_m must contain 3 values")
|
||||
return value
|
||||
|
||||
|
||||
def _record_image_vector(record: Mapping[str, Any]) -> np.ndarray:
|
||||
value = np.asarray(record.get("image_relative_xy_px"), dtype=float)
|
||||
if value.shape != (2,) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("record image_relative_xy_px must contain 2 values")
|
||||
return value
|
||||
|
||||
|
||||
def fit_joint_center_curve(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_plane_rms_m: float = 0.004,
|
||||
maximum_radial_rms_m: float = 0.004,
|
||||
minimum_radius_m: float = 0.003,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
) -> JointCurveFit:
|
||||
"""Fit one command-indexed curve from a parent-frame centre trajectory."""
|
||||
samples = [dict(record) for record in records]
|
||||
if len(samples) < 12:
|
||||
raise ValueError("joint trajectory requires at least 12 samples")
|
||||
points = np.asarray([_record_vector(record) for record in samples])
|
||||
axis, common_plane_rms = _fit_plane_axis([points])
|
||||
circle = _fit_circle_with_axis(points, axis)
|
||||
circle = _orient_circle_positive(circle, samples, list(points))
|
||||
values = [_angle_for_circle(point, circle) for point in points]
|
||||
curves, correction, hysteresis = _fit_joint_curve(samples, values)
|
||||
quality = {
|
||||
"plane_rms_m": max(
|
||||
float(common_plane_rms), float(circle["plane_rms_m"])
|
||||
),
|
||||
"radial_rms_m": float(circle["radial_rms_m"]),
|
||||
"radius_m": float(circle["radius_m"]),
|
||||
"arc_rad": float(circle["observed_arc_rad"]),
|
||||
}
|
||||
failures: list[str] = []
|
||||
if quality["plane_rms_m"] > float(maximum_plane_rms_m):
|
||||
failures.append("plane_rms")
|
||||
if quality["radial_rms_m"] > float(maximum_radial_rms_m):
|
||||
failures.append("radial_rms")
|
||||
if quality["radius_m"] < float(minimum_radius_m):
|
||||
failures.append("radius")
|
||||
if quality["arc_rad"] < float(minimum_arc_rad):
|
||||
failures.append("arc")
|
||||
if failures:
|
||||
raise ValueError("joint_trajectory_quality_failed:" + ",".join(failures))
|
||||
return JointCurveFit(
|
||||
angle_rad=tuple(float(value) for value in curves["angle_rad"]),
|
||||
decreasing_rad=tuple(
|
||||
float(value) for value in curves["decreasing_rad"]
|
||||
),
|
||||
increasing_rad=tuple(
|
||||
float(value) for value in curves["increasing_rad"]
|
||||
),
|
||||
circle=dict(circle),
|
||||
maximum_monotonic_correction_rad=float(correction),
|
||||
maximum_hysteresis_rad=float(hysteresis),
|
||||
quality=quality,
|
||||
)
|
||||
|
||||
|
||||
def _signed_image_circle_angle(
|
||||
point_xy_px: Sequence[float], circle: Mapping[str, Any]
|
||||
) -> float:
|
||||
point = np.asarray(point_xy_px, dtype=float)
|
||||
centre = np.asarray(circle["center_xy_px"], dtype=float)
|
||||
reference = np.asarray(circle["reference_xy_px"], dtype=float)
|
||||
vector = point - centre
|
||||
if point.shape != (2,) or not np.all(np.isfinite(point)):
|
||||
raise ValueError("image point must contain 2 finite values")
|
||||
if float(np.linalg.norm(vector)) < 1.0e-9:
|
||||
raise ValueError("image point lies at the fitted circle centre")
|
||||
return float(circle["orientation_sign"]) * math.atan2(
|
||||
float(reference[0] * vector[1] - reference[1] * vector[0]),
|
||||
float(np.dot(reference, vector)),
|
||||
)
|
||||
|
||||
|
||||
def fit_joint_image_curve(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_radial_rms_px: float = 2.0,
|
||||
maximum_radial_p95_px: float = 3.5,
|
||||
minimum_radius_px: float = 20.0,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
) -> JointCurveFit:
|
||||
"""Fit angular phase from a stable projected parent/child circle."""
|
||||
samples = [dict(record) for record in records]
|
||||
if len(samples) < 12:
|
||||
raise ValueError("joint image trajectory requires at least 12 samples")
|
||||
points = np.asarray([_record_image_vector(record) for record in samples])
|
||||
centre, radius = _fit_circle(points)
|
||||
radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius)
|
||||
quality = {
|
||||
"radial_rms_px": float(np.sqrt(np.mean(np.square(radial_error)))),
|
||||
"radial_p95_px": float(np.percentile(radial_error, 95.0)),
|
||||
"radius_px": float(radius),
|
||||
"arc_rad": float(_trajectory_arc_rad(points, centre)),
|
||||
}
|
||||
failures: list[str] = []
|
||||
if quality["radial_rms_px"] > float(maximum_radial_rms_px):
|
||||
failures.append("radial_rms")
|
||||
if quality["radial_p95_px"] > float(maximum_radial_p95_px):
|
||||
failures.append("radial_p95")
|
||||
if quality["radius_px"] < float(minimum_radius_px):
|
||||
failures.append("radius")
|
||||
if quality["arc_rad"] < float(minimum_arc_rad):
|
||||
failures.append("arc")
|
||||
if failures:
|
||||
raise ValueError(
|
||||
"joint_image_trajectory_quality_failed:" + ",".join(failures)
|
||||
)
|
||||
|
||||
endpoint_points = np.asarray(
|
||||
[
|
||||
point
|
||||
for record, point in zip(samples, points)
|
||||
if int(record["command_u8"]) == 255
|
||||
]
|
||||
)
|
||||
if endpoint_points.size == 0:
|
||||
raise ValueError("joint image trajectory is missing command 255")
|
||||
reference = np.median(endpoint_points, axis=0) - centre
|
||||
reference_norm = float(np.linalg.norm(reference))
|
||||
if reference_norm < 1.0e-9:
|
||||
raise ValueError("joint image trajectory endpoint is degenerate")
|
||||
reference /= reference_norm
|
||||
circle: dict[str, Any] = {
|
||||
"space": "image_2d",
|
||||
"center_xy_px": [float(value) for value in centre],
|
||||
"reference_xy_px": [float(value) for value in reference],
|
||||
"orientation_sign": 1.0,
|
||||
"radius_px": float(radius),
|
||||
}
|
||||
values = [_signed_image_circle_angle(point, circle) for point in points]
|
||||
command_zero_values = [
|
||||
value
|
||||
for record, value in zip(samples, values)
|
||||
if int(record["command_u8"]) == 0
|
||||
]
|
||||
if not command_zero_values:
|
||||
raise ValueError("joint image trajectory is missing command 0")
|
||||
if float(np.median(command_zero_values)) < 0.0:
|
||||
circle["orientation_sign"] = -1.0
|
||||
values = [-value for value in values]
|
||||
curves, correction, hysteresis = _fit_joint_curve(samples, values)
|
||||
return JointCurveFit(
|
||||
angle_rad=tuple(float(value) for value in curves["angle_rad"]),
|
||||
decreasing_rad=tuple(
|
||||
float(value) for value in curves["decreasing_rad"]
|
||||
),
|
||||
increasing_rad=tuple(
|
||||
float(value) for value in curves["increasing_rad"]
|
||||
),
|
||||
circle=circle,
|
||||
maximum_monotonic_correction_rad=float(correction),
|
||||
maximum_hysteresis_rad=float(hysteresis),
|
||||
quality=quality,
|
||||
)
|
||||
|
||||
|
||||
def fit_measured_joint_curve(
|
||||
joint_name: str,
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_plane_rms_m: float = 0.004,
|
||||
maximum_radial_rms_m: float = 0.004,
|
||||
minimum_radius_m: float = 0.003,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
image_maximum_radial_rms_px: float = 2.0,
|
||||
image_maximum_radial_p95_px: float = 3.5,
|
||||
image_minimum_radius_px: float = 20.0,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> JointCurveFit:
|
||||
"""Select the stable trajectory representation for one measured joint."""
|
||||
if str(joint_name) in profile.image_trajectory_joints:
|
||||
return fit_joint_image_curve(
|
||||
records,
|
||||
maximum_radial_rms_px=image_maximum_radial_rms_px,
|
||||
maximum_radial_p95_px=image_maximum_radial_p95_px,
|
||||
minimum_radius_px=image_minimum_radius_px,
|
||||
minimum_arc_rad=minimum_arc_rad,
|
||||
)
|
||||
return fit_joint_center_curve(
|
||||
records,
|
||||
maximum_plane_rms_m=maximum_plane_rms_m,
|
||||
maximum_radial_rms_m=maximum_radial_rms_m,
|
||||
minimum_radius_m=minimum_radius_m,
|
||||
minimum_arc_rad=minimum_arc_rad,
|
||||
)
|
||||
|
||||
|
||||
def center_splay_curve(fit: JointCurveFit) -> tuple[JointCurveFit, int, float]:
|
||||
"""Centre a measured 255->0 splay travel on its angular midpoint."""
|
||||
raw = np.asarray(fit.angle_rad, dtype=float)
|
||||
if raw.shape != (256,) or not np.all(np.isfinite(raw)):
|
||||
raise ValueError("splay curve must contain 256 finite values")
|
||||
if np.any(np.diff(raw) > 1.0e-7):
|
||||
raise ValueError("raw splay curve must be non-increasing")
|
||||
travel_midpoint = 0.5 * float(raw[0] + raw[255])
|
||||
if travel_midpoint <= 0.0:
|
||||
raise ValueError("splay travel must be positive")
|
||||
centred = raw - travel_midpoint
|
||||
# Make the public endpoint symmetry exact after decimal rounding.
|
||||
endpoint = round(0.5 * float(raw[0] - raw[255]), 8)
|
||||
centred[0] = endpoint
|
||||
centred[255] = -endpoint
|
||||
centred = np.asarray([round(float(value), 8) for value in centred])
|
||||
zero_command = int(np.argmin(np.abs(centred)))
|
||||
return (
|
||||
replace(
|
||||
fit,
|
||||
angle_rad=tuple(float(value) for value in centred),
|
||||
zero_offset_rad=round(float(travel_midpoint), 8),
|
||||
),
|
||||
zero_command,
|
||||
round(float(travel_midpoint), 8),
|
||||
)
|
||||
|
||||
|
||||
def measure_joint_vector(fit: JointCurveFit, vector_xyz_m: Sequence[float]) -> float:
|
||||
"""Measure one vector with a fitted model, including splay centring."""
|
||||
return float(
|
||||
_angle_for_circle(vector_xyz_m, fit.circle) - fit.zero_offset_rad
|
||||
)
|
||||
|
||||
|
||||
def measure_joint_observation(
|
||||
fit: JointCurveFit,
|
||||
*,
|
||||
vector_xyz_m: Sequence[float],
|
||||
image_vector_xy_px: Sequence[float],
|
||||
) -> float:
|
||||
"""Measure an observation in the same space used to fit its curve."""
|
||||
if fit.circle.get("space") == "image_2d":
|
||||
return float(
|
||||
_signed_image_circle_angle(image_vector_xy_px, fit.circle)
|
||||
- fit.zero_offset_rad
|
||||
)
|
||||
return measure_joint_vector(fit, vector_xyz_m)
|
||||
|
||||
|
||||
def fit_projected_zero(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
minimum_radius_px: float = 20.0,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
maximum_radial_rms_px: float = 2.0,
|
||||
maximum_radial_p95_px: float = 3.5,
|
||||
maximum_round_difference_rad: float = math.radians(1.0),
|
||||
) -> float:
|
||||
"""Return the image/table-projected zero from three sweep rounds."""
|
||||
samples = [dict(record) for record in records]
|
||||
points = np.asarray(
|
||||
[record.get("image_relative_xy_px") for record in samples], dtype=float
|
||||
)
|
||||
if points.ndim != 2 or points.shape[1] != 2 or len(points) < 12:
|
||||
raise ValueError("projected zero requires 2-D trajectory samples")
|
||||
if not np.all(np.isfinite(points)):
|
||||
raise ValueError("projected zero samples must be finite")
|
||||
centre, radius = _fit_circle(points)
|
||||
radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius)
|
||||
radial_rms = float(np.sqrt(np.mean(np.square(radial_error))))
|
||||
radial_p95 = float(np.percentile(radial_error, 95.0))
|
||||
arc = _trajectory_arc_rad(points, centre)
|
||||
if radius < minimum_radius_px:
|
||||
raise ValueError("projected_zero_radius_too_small")
|
||||
if arc < minimum_arc_rad:
|
||||
raise ValueError("projected_zero_arc_too_small")
|
||||
if radial_rms > maximum_radial_rms_px:
|
||||
raise ValueError("projected_zero_radial_rms_too_large")
|
||||
if radial_p95 > maximum_radial_p95_px:
|
||||
raise ValueError("projected_zero_radial_p95_too_large")
|
||||
|
||||
cycles = sorted({int(record["cycle"]) for record in samples})
|
||||
if len(cycles) < 3:
|
||||
raise ValueError("projected zero requires three sweep rounds")
|
||||
angles: list[float] = []
|
||||
for cycle in cycles:
|
||||
zero_points = np.asarray(
|
||||
[
|
||||
record["image_relative_xy_px"]
|
||||
for record in samples
|
||||
if int(record["cycle"]) == cycle
|
||||
and int(record["command_u8"]) == 255
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
if zero_points.size == 0:
|
||||
raise ValueError("projected zero is missing command 255")
|
||||
point = np.median(zero_points, axis=0)
|
||||
inward = centre - point
|
||||
if float(np.linalg.norm(inward)) < 1.0e-9:
|
||||
raise ValueError("projected zero point is degenerate")
|
||||
angles.append(
|
||||
wrap_angle_rad(math.atan2(-float(inward[1]), float(inward[0])))
|
||||
)
|
||||
if maximum_pairwise_angle_difference_rad(angles) > maximum_round_difference_rad:
|
||||
raise ValueError("projected_zero_round_difference_too_large")
|
||||
return round(float(circular_median_rad(angles)), 8)
|
||||
|
||||
|
||||
def build_compact_payload(
|
||||
*,
|
||||
serial_number: str,
|
||||
measured_fits: Mapping[str, JointCurveFit],
|
||||
urdf_zero_offsets_rad: Mapping[str, float],
|
||||
validation_errors_rad: Sequence[float],
|
||||
passed: bool,
|
||||
baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
side: str = "left",
|
||||
) -> dict[str, Any]:
|
||||
profile = get_hand_calibration_profile(side)
|
||||
if set(measured_fits) != set(profile.measured_joints):
|
||||
raise ValueError("measured_fits must contain all directly measured joints")
|
||||
expected_active = {
|
||||
name for name, spec in profile.joint_specs.items() if spec.active
|
||||
}
|
||||
if set(urdf_zero_offsets_rad) != expected_active:
|
||||
raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set")
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 commands")
|
||||
|
||||
joints: dict[str, dict[str, Any]] = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
source_name = spec.source_joint or name
|
||||
fit = measured_fits[source_name]
|
||||
joint: dict[str, Any] = {
|
||||
"motor_index": int(spec.motor_index),
|
||||
"angle_rad": [round(float(value), 8) for value in fit.angle_rad],
|
||||
}
|
||||
if spec.active:
|
||||
joint["zero_command_u8"] = int(baseline[spec.motor_index])
|
||||
joint["zero_angles"] = {
|
||||
"urdf_zero_offset_rad": round(
|
||||
float(urdf_zero_offsets_rad[name]), 8
|
||||
)
|
||||
}
|
||||
else:
|
||||
joint["passive"] = True
|
||||
if spec.source_joint is not None:
|
||||
joint["source_joint"] = spec.source_joint
|
||||
joints[name] = joint
|
||||
|
||||
errors = np.abs(np.asarray(validation_errors_rad, dtype=float))
|
||||
mae = float(np.mean(errors)) if errors.size else float("nan")
|
||||
p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan")
|
||||
payload = {
|
||||
"schema_version": 4,
|
||||
"model": "G20",
|
||||
"side": profile.side,
|
||||
"serial_number": str(serial_number),
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": [int(value) for value in baseline],
|
||||
"joints": joints,
|
||||
"quality": {
|
||||
"passed": bool(passed),
|
||||
"validation_mae_rad": (
|
||||
None if not math.isfinite(mae) else round(mae, 8)
|
||||
),
|
||||
"validation_p95_rad": (
|
||||
None if not math.isfinite(p95) else round(p95, 8)
|
||||
),
|
||||
},
|
||||
}
|
||||
validate_compact_payload(payload)
|
||||
return payload
|
||||
|
||||
|
||||
def validate_compact_payload(payload: Mapping[str, Any]) -> None:
|
||||
expected_top = {
|
||||
"schema_version", "model", "side", "serial_number", "angle_unit",
|
||||
"command_range", "baseline_command_u8", "joints", "quality",
|
||||
}
|
||||
if set(payload) != expected_top:
|
||||
raise ValueError("compact calibration has unexpected top-level fields")
|
||||
if payload["schema_version"] != 4:
|
||||
raise ValueError("schema_version must be 4")
|
||||
if payload["model"] != "G20" or payload["side"] not in {"left", "right"}:
|
||||
raise ValueError("payload must describe a left or right G20")
|
||||
profile = get_hand_calibration_profile(str(payload["side"]))
|
||||
if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
|
||||
raise ValueError("payload angle or command units are invalid")
|
||||
baseline = payload["baseline_command_u8"]
|
||||
if not isinstance(baseline, list) or len(baseline) != 20:
|
||||
raise ValueError("baseline_command_u8 must contain 20 values")
|
||||
joints = payload["joints"]
|
||||
if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs):
|
||||
raise ValueError("payload must contain exactly 21 G20 joints")
|
||||
|
||||
for name, spec in profile.joint_specs.items():
|
||||
joint = joints[name]
|
||||
allowed = {"motor_index", "angle_rad"}
|
||||
allowed.add("zero_command_u8" if spec.active else "passive")
|
||||
if spec.active:
|
||||
allowed.add("zero_angles")
|
||||
if spec.source_joint is not None:
|
||||
allowed.add("source_joint")
|
||||
if set(joint) != allowed:
|
||||
raise ValueError(f"{name} has unexpected fields")
|
||||
if int(joint["motor_index"]) != spec.motor_index:
|
||||
raise ValueError(f"{name} has the wrong motor_index")
|
||||
curve = np.asarray(joint["angle_rad"], dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError(f"{name}.angle_rad must contain 256 finite values")
|
||||
if np.any(np.diff(curve) > 1.0e-7):
|
||||
raise ValueError(f"{name}.angle_rad must be non-increasing")
|
||||
if spec.active:
|
||||
zero = joint["zero_command_u8"]
|
||||
if not isinstance(zero, int) or not 0 <= zero <= 255:
|
||||
raise ValueError(f"{name}.zero_command_u8 is invalid")
|
||||
zero_angles = joint.get("zero_angles")
|
||||
if not isinstance(zero_angles, Mapping) or set(zero_angles) != {
|
||||
"urdf_zero_offset_rad"
|
||||
}:
|
||||
raise ValueError(
|
||||
f"{name}.zero_angles must contain urdf_zero_offset_rad"
|
||||
)
|
||||
offset = float(zero_angles["urdf_zero_offset_rad"])
|
||||
if not math.isfinite(offset):
|
||||
raise ValueError(f"{name}.urdf_zero_offset_rad is invalid")
|
||||
if abs(float(curve[zero])) > 1.0e-6:
|
||||
raise ValueError(
|
||||
f"{name}.angle_rad must be zero at zero_command_u8"
|
||||
)
|
||||
elif joint.get("passive") is not True:
|
||||
raise ValueError(f"{name} must be marked passive")
|
||||
if spec.source_joint is not None:
|
||||
if joint.get("source_joint") != spec.source_joint:
|
||||
raise ValueError(f"{name} has the wrong source_joint")
|
||||
source = joints[spec.source_joint]
|
||||
if joint["angle_rad"] != source["angle_rad"]:
|
||||
raise ValueError(f"{name} must copy its source curve exactly")
|
||||
if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]:
|
||||
raise ValueError(f"{name} must copy its source zero command")
|
||||
# source_joint denotes a dynamic command-angle curve source. The
|
||||
# absolute URDF zero belongs to an independent motor/assembly and
|
||||
# must remain per-joint unless it was independently observed.
|
||||
|
||||
quality = payload["quality"]
|
||||
if set(quality) != {
|
||||
"passed", "validation_mae_rad", "validation_p95_rad"
|
||||
}:
|
||||
raise ValueError("quality must contain only compact summary fields")
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,846 +0,0 @@
|
||||
"""Safely replay a complete three-camera session without moving the hand."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from collections import defaultdict
|
||||
from dataclasses import replace
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import tempfile
|
||||
from typing import Any, Mapping, Sequence
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
import yaml
|
||||
|
||||
from .extrinsics import load_three_camera_extrinsics
|
||||
from .full_hand import (
|
||||
HandCalibrationProfile,
|
||||
JointCurveFit,
|
||||
build_calibration_motion_command,
|
||||
build_compact_payload,
|
||||
get_hand_calibration_profile,
|
||||
validate_compact_payload,
|
||||
)
|
||||
from .storage import atomic_write_json
|
||||
from .urdf_zero import (
|
||||
JointAxisMeasurement,
|
||||
UrdfKinematicModel,
|
||||
_angles_from_state,
|
||||
fit_joint_axis_measurement,
|
||||
fit_rotation_joint_curve,
|
||||
get_zero_calibration_profile,
|
||||
rotation_curve_holdout_errors,
|
||||
solve_urdf_zero_offsets,
|
||||
write_zero_corrected_urdf,
|
||||
)
|
||||
|
||||
|
||||
def _sha256(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _output_suffix(output_tag: str | None) -> str:
|
||||
"""Return a filename-safe suffix for a non-destructive replay variant."""
|
||||
if output_tag is None:
|
||||
return ""
|
||||
tag = str(output_tag)
|
||||
if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag):
|
||||
raise ValueError(
|
||||
"output tag must be 1-64 filename-safe characters, beginning "
|
||||
"with a letter or digit"
|
||||
)
|
||||
return f"_{tag}"
|
||||
|
||||
|
||||
def _load_parameters(path: Path) -> dict[str, Any]:
|
||||
payload = yaml.safe_load(path.read_text(encoding="utf-8"))
|
||||
return dict(payload["g20_calibration"]["ros__parameters"])
|
||||
|
||||
|
||||
def _latest_attempt_records(
|
||||
rows: Sequence[Mapping[str, Any]],
|
||||
) -> dict[str, list[dict[str, Any]]]:
|
||||
"""Reproduce the online retry buffer from append-only raw samples.
|
||||
|
||||
Online retry clears only the failed joint/cycle/direction from memory,
|
||||
while JSONL deliberately retains every attempt for audit. Offline replay
|
||||
must therefore select the greatest attempt independently for each logical
|
||||
trajectory rather than mixing rejected attempts into the final fit.
|
||||
"""
|
||||
samples = [dict(row) for row in rows if row.get("kind") == "sample"]
|
||||
latest_attempt: dict[tuple[str, int, str], int] = {}
|
||||
for row in samples:
|
||||
key = (
|
||||
str(row["joint"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
latest_attempt[key] = max(
|
||||
latest_attempt.get(key, 0), int(row.get("attempt", 1))
|
||||
)
|
||||
records: dict[str, list[dict[str, Any]]] = defaultdict(list)
|
||||
for row in samples:
|
||||
key = (
|
||||
str(row["joint"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
if int(row.get("attempt", 1)) == latest_attempt[key]:
|
||||
records[key[0]].append(row)
|
||||
return dict(records)
|
||||
|
||||
|
||||
def _load_raw_session(
|
||||
session_dir: Path,
|
||||
) -> tuple[dict[str, Any], dict[str, list[dict[str, Any]]], Path]:
|
||||
raw_path = session_dir / "raw_samples.jsonl"
|
||||
if not raw_path.is_file():
|
||||
raise ValueError(f"raw session does not exist: {raw_path}")
|
||||
rows = [
|
||||
json.loads(line)
|
||||
for line in raw_path.read_text(encoding="utf-8").splitlines()
|
||||
if line.strip()
|
||||
]
|
||||
starts = [row for row in rows if row.get("kind") == "session_start"]
|
||||
if len(starts) != 1:
|
||||
raise ValueError("raw session must contain exactly one session_start")
|
||||
return starts[0], _latest_attempt_records(rows), raw_path
|
||||
|
||||
|
||||
def _fit_curve(
|
||||
name: str,
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
profile: HandCalibrationProfile,
|
||||
baseline: Sequence[int],
|
||||
) -> JointCurveFit:
|
||||
motor = profile.joint_specs[name].motor_index
|
||||
return fit_rotation_joint_curve(
|
||||
records,
|
||||
zero_command_u8=int(baseline[motor]),
|
||||
)
|
||||
|
||||
|
||||
def _fit_axes(
|
||||
*,
|
||||
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
|
||||
profile: HandCalibrationProfile,
|
||||
baseline: Sequence[int],
|
||||
extrinsics_file: Path,
|
||||
repetitions: int,
|
||||
) -> list[JointAxisMeasurement]:
|
||||
zero_profile = get_zero_calibration_profile(profile.side)
|
||||
extrinsics = load_three_camera_extrinsics(extrinsics_file)
|
||||
cache: dict[tuple[str, int], JointAxisMeasurement] = {}
|
||||
reference = profile.reference_finger
|
||||
upstream_by_joint = {
|
||||
"thumb_mcp": "thumb_cmc_pitch",
|
||||
"thumb_ip": "thumb_mcp",
|
||||
f"{reference}_pip": f"{reference}_mcp_pitch",
|
||||
f"{reference}_dip": f"{reference}_pip",
|
||||
}
|
||||
|
||||
def fit_one(name: str, cycle: int) -> JointAxisMeasurement:
|
||||
key = (name, cycle)
|
||||
if key in cache:
|
||||
return cache[key]
|
||||
upstream = upstream_by_joint.get(name)
|
||||
constraint = None if upstream is None else fit_one(upstream, cycle).axis_common_xyz
|
||||
spec = profile.joint_specs[name]
|
||||
view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray(
|
||||
[0.0, 0.0, 1.0], dtype=float
|
||||
)
|
||||
result = fit_joint_axis_measurement(
|
||||
name,
|
||||
records_by_joint[name],
|
||||
cycle=cycle,
|
||||
zero_command_u8=int(baseline[spec.motor_index]),
|
||||
axis_common_constraint=constraint,
|
||||
constrained_circle_joints=zero_profile.constrained_circle_joints,
|
||||
view_normal_common_xyz=view_normal,
|
||||
)
|
||||
condition = build_calibration_motion_command(
|
||||
spec,
|
||||
int(baseline[spec.motor_index]),
|
||||
baseline=baseline,
|
||||
profile=profile,
|
||||
)
|
||||
result = replace(
|
||||
result,
|
||||
condition_command_u8=tuple(float(value) for value in condition),
|
||||
view_normal_common_xyz=tuple(float(value) for value in view_normal),
|
||||
)
|
||||
cache[key] = result
|
||||
return result
|
||||
|
||||
return [
|
||||
fit_one(name, cycle)
|
||||
for name in zero_profile.axis_joints
|
||||
for cycle in range(repetitions)
|
||||
]
|
||||
|
||||
|
||||
def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float:
|
||||
maximum = 0.0
|
||||
for left in axes:
|
||||
for right in axes:
|
||||
maximum = max(
|
||||
maximum,
|
||||
math.acos(
|
||||
abs(
|
||||
float(
|
||||
np.clip(
|
||||
np.asarray(left, dtype=float)
|
||||
@ np.asarray(right, dtype=float),
|
||||
-1.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
)
|
||||
),
|
||||
)
|
||||
return maximum
|
||||
|
||||
|
||||
def _quality_failures(
|
||||
*,
|
||||
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
|
||||
profile: HandCalibrationProfile,
|
||||
baseline: Sequence[int],
|
||||
fits: Mapping[str, JointCurveFit],
|
||||
axes: Sequence[JointAxisMeasurement],
|
||||
parameters: Mapping[str, Any],
|
||||
) -> list[str]:
|
||||
failures: list[str] = []
|
||||
repetitions = int(parameters["repetitions"])
|
||||
zero_profile = get_zero_calibration_profile(profile.side)
|
||||
axis_by_key = {(item.joint, item.cycle): item for item in axes}
|
||||
expected_directions = {"decreasing", "increasing"}
|
||||
for name in profile.measured_joints:
|
||||
records = list(records_by_joint.get(name, ()))
|
||||
if not records:
|
||||
failures.append(f"{name}: no samples")
|
||||
continue
|
||||
attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set)
|
||||
for record in records:
|
||||
attempts_by_direction[
|
||||
(int(record["cycle"]), str(record["direction"]))
|
||||
].add(int(record.get("attempt", 1)))
|
||||
for cycle in range(repetitions):
|
||||
for direction in expected_directions:
|
||||
selected = [
|
||||
record
|
||||
for record in records
|
||||
if int(record["cycle"]) == cycle
|
||||
and str(record["direction"]) == direction
|
||||
]
|
||||
attempts = attempts_by_direction.get((cycle, direction), set())
|
||||
if len(attempts) != 1:
|
||||
failures.append(
|
||||
f"{name} cycle {cycle + 1} {direction}: ambiguous attempts {sorted(attempts)}"
|
||||
)
|
||||
continue
|
||||
commands = sorted({int(record["command_u8"]) for record in selected})
|
||||
if len(selected) < int(parameters["minimum_sweep_frames"]):
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames")
|
||||
if not commands or max(commands) - min(commands) < float(
|
||||
parameters["minimum_state_span_u8"]
|
||||
):
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span")
|
||||
if len(commands) < int(parameters["minimum_sweep_bins"]):
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins")
|
||||
if 0 not in commands or 255 not in commands:
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing")
|
||||
if commands and max(np.diff(commands), default=0) > int(
|
||||
parameters["maximum_bin_gap"]
|
||||
):
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap")
|
||||
|
||||
sync_p95 = float(
|
||||
np.percentile(
|
||||
[float(record.get("state_image_sync_error_ms", 0.0)) for record in records],
|
||||
95.0,
|
||||
)
|
||||
)
|
||||
if sync_p95 > float(parameters["maximum_state_image_skew_ms"]):
|
||||
failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms")
|
||||
spec = profile.joint_specs[name]
|
||||
fit = fits[name]
|
||||
orthogonal_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"active_maximum_rotation_orthogonal_rms_deg"
|
||||
if spec.active
|
||||
else "passive_maximum_rotation_orthogonal_rms_deg"
|
||||
]
|
||||
)
|
||||
)
|
||||
if float(fit.quality["rotation_orthogonal_rms_rad"]) > orthogonal_limit:
|
||||
failures.append(f"{name}: rotation orthogonal RMS")
|
||||
if float(fit.quality["arc_rad"]) < math.radians(
|
||||
float(parameters["trajectory_minimum_arc_deg"])
|
||||
):
|
||||
failures.append(f"{name}: trajectory arc")
|
||||
monotonic_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"maximum_monotonic_correction_deg"
|
||||
if spec.active
|
||||
else "passive_maximum_monotonic_correction_deg"
|
||||
]
|
||||
)
|
||||
)
|
||||
hysteresis_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"maximum_hysteresis_deg"
|
||||
if spec.active
|
||||
else "passive_maximum_hysteresis_deg"
|
||||
]
|
||||
)
|
||||
)
|
||||
if fit.maximum_monotonic_correction_rad > monotonic_limit:
|
||||
failures.append(f"{name}: monotonic correction")
|
||||
if fit.maximum_hysteresis_rad > hysteresis_limit:
|
||||
failures.append(f"{name}: hysteresis")
|
||||
|
||||
cycle_travels: list[float] = []
|
||||
cycle_axes: list[Sequence[float]] = []
|
||||
cycle_axis_sources: list[str] = []
|
||||
for cycle in range(repetitions):
|
||||
cycle_fit = _fit_curve(
|
||||
name,
|
||||
[record for record in records if int(record["cycle"]) == cycle],
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
)
|
||||
cycle_travels.append(
|
||||
abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255]))
|
||||
)
|
||||
axis = axis_by_key[(name, cycle)]
|
||||
cycle_axes.append(axis.axis_common_xyz)
|
||||
cycle_axis_sources.append(axis.axis_direction_source)
|
||||
if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]):
|
||||
failures.append(f"{name} cycle {cycle + 1}: radial RMS")
|
||||
if axis.pose_axis_line_rms_m > float(
|
||||
parameters["axis_maximum_pose_line_rms_m"]
|
||||
):
|
||||
failures.append(
|
||||
f"{name} cycle {cycle + 1}: pose axis-line RMS"
|
||||
)
|
||||
if name not in zero_profile.constrained_circle_joints:
|
||||
plane_limit = float(
|
||||
parameters[
|
||||
"axis_maximum_plane_rms_m"
|
||||
if spec.active
|
||||
else "passive_axis_maximum_plane_rms_m"
|
||||
]
|
||||
)
|
||||
if axis.plane_rms_m > plane_limit:
|
||||
failures.append(f"{name} cycle {cycle + 1}: plane RMS")
|
||||
if (
|
||||
name not in zero_profile.constrained_circle_joints
|
||||
and axis.rotation_circle_axis_difference_rad > math.radians(
|
||||
float(parameters["axis_maximum_rotation_circle_difference_deg"])
|
||||
)
|
||||
):
|
||||
failures.append(f"{name} cycle {cycle + 1}: axis disagreement")
|
||||
travel_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"trajectory_maximum_cycle_travel_difference_deg"
|
||||
if spec.active
|
||||
else "passive_maximum_cycle_travel_difference_deg"
|
||||
]
|
||||
)
|
||||
)
|
||||
if max(cycle_travels) - min(cycle_travels) > travel_limit:
|
||||
failures.append(f"{name}: cycle travel difference")
|
||||
if (
|
||||
not all(
|
||||
source == "upstream_constraint"
|
||||
for source in cycle_axis_sources
|
||||
)
|
||||
and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
|
||||
float(parameters["zero_maximum_axis_cycle_difference_deg"])
|
||||
)
|
||||
):
|
||||
failures.append(f"{name}: cycle axis difference")
|
||||
return failures
|
||||
|
||||
|
||||
def _joint_xml(path: Path) -> dict[str, ET.Element]:
|
||||
return {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(path).getroot().findall("joint")
|
||||
}
|
||||
|
||||
|
||||
def _triplet(value: str) -> np.ndarray:
|
||||
return np.asarray([float(item) for item in value.split()], dtype=float)
|
||||
|
||||
|
||||
def _validate_corrected_urdf(
|
||||
*,
|
||||
source: Path,
|
||||
corrected: Path,
|
||||
offsets: Mapping[str, float],
|
||||
axes: Sequence[JointAxisMeasurement],
|
||||
curves: Mapping[str, JointCurveFit],
|
||||
motor_by_joint: Mapping[str, int],
|
||||
inherited_zero_joints: Mapping[str, str],
|
||||
) -> dict[str, float]:
|
||||
source_joints = _joint_xml(source)
|
||||
corrected_joints = _joint_xml(corrected)
|
||||
if set(source_joints) != set(corrected_joints):
|
||||
raise ValueError("corrected URDF changed the joint set")
|
||||
maximum_origin_rotation_error = 0.0
|
||||
maximum_origin_translation_error = 0.0
|
||||
for name, original_joint in source_joints.items():
|
||||
corrected_joint = corrected_joints[name]
|
||||
original_origin = original_joint.find("origin")
|
||||
corrected_origin = corrected_joint.find("origin")
|
||||
if original_origin is None or corrected_origin is None:
|
||||
continue
|
||||
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
|
||||
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
|
||||
maximum_origin_translation_error = max(
|
||||
maximum_origin_translation_error,
|
||||
float(np.linalg.norm(corrected_xyz - original_xyz)),
|
||||
)
|
||||
original_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
corrected_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
expected = original_rotation
|
||||
if name in offsets:
|
||||
axis_node = original_joint.find("axis")
|
||||
axis = _triplet(
|
||||
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
|
||||
)
|
||||
axis /= np.linalg.norm(axis)
|
||||
expected = original_rotation * Rotation.from_rotvec(
|
||||
axis * float(offsets[name])
|
||||
)
|
||||
error = float((expected.inv() * corrected_rotation).magnitude())
|
||||
maximum_origin_rotation_error = max(maximum_origin_rotation_error, error)
|
||||
original_limit = original_joint.find("limit")
|
||||
corrected_limit = corrected_joint.find("limit")
|
||||
if original_limit is not None and corrected_limit is not None:
|
||||
if (
|
||||
original_limit.get("lower") != corrected_limit.get("lower")
|
||||
or original_limit.get("upper") != corrected_limit.get("upper")
|
||||
):
|
||||
raise ValueError(f"corrected URDF unexpectedly changed {name} limits")
|
||||
if maximum_origin_translation_error > 1.0e-12:
|
||||
raise ValueError("corrected URDF changed a joint origin translation")
|
||||
if maximum_origin_rotation_error > 1.0e-10:
|
||||
raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)")
|
||||
original_model = UrdfKinematicModel(source)
|
||||
corrected_model = UrdfKinematicModel(corrected)
|
||||
maximum_axis_error = 0.0
|
||||
maximum_point_error = 0.0
|
||||
for measurement in axes:
|
||||
state = (
|
||||
measurement.condition_state_u8
|
||||
if measurement.condition_command_u8 is None
|
||||
else measurement.condition_command_u8
|
||||
)
|
||||
angles = _angles_from_state(
|
||||
state,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=inherited_zero_joints,
|
||||
)
|
||||
expected_axis, expected_point = original_model.axis_line(
|
||||
measurement.joint,
|
||||
zero_offsets=offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
actual_axis, actual_point = corrected_model.axis_line(
|
||||
measurement.joint,
|
||||
zero_offsets={},
|
||||
joint_angles=angles,
|
||||
)
|
||||
maximum_axis_error = max(
|
||||
maximum_axis_error,
|
||||
math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))),
|
||||
)
|
||||
maximum_point_error = max(
|
||||
maximum_point_error,
|
||||
float(np.linalg.norm(expected_point - actual_point)),
|
||||
)
|
||||
if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10:
|
||||
raise ValueError("written URDF kinematics differ from the solved correction")
|
||||
return {
|
||||
"maximum_origin_rotation_error_rad": maximum_origin_rotation_error,
|
||||
"maximum_origin_translation_error_m": maximum_origin_translation_error,
|
||||
"maximum_axis_equivalence_error_rad": maximum_axis_error,
|
||||
"maximum_axis_point_equivalence_error_m": maximum_point_error,
|
||||
}
|
||||
|
||||
|
||||
def replay_session(
|
||||
session_dir: str | Path,
|
||||
*,
|
||||
serial_number: str | None = None,
|
||||
config_file: str | Path | None = None,
|
||||
write_outputs: bool = False,
|
||||
output_tag: str | None = None,
|
||||
) -> dict[str, Any]:
|
||||
session = Path(session_dir).expanduser().resolve()
|
||||
package_root = Path(__file__).resolve().parents[1]
|
||||
config = (
|
||||
package_root / "config" / "three_camera_calibration.yaml"
|
||||
if config_file is None
|
||||
else Path(config_file).expanduser().resolve()
|
||||
)
|
||||
parameters = _load_parameters(config)
|
||||
start, records_by_joint, raw_path = _load_raw_session(session)
|
||||
side = str(start["hand_type"]).lower()
|
||||
profile = get_hand_calibration_profile(side)
|
||||
zero_profile = get_zero_calibration_profile(side)
|
||||
baseline = tuple(int(value) for value in start["baseline_command_u8"])
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("session baseline must contain exactly 20 commands")
|
||||
if set(records_by_joint) != set(profile.measured_joints):
|
||||
raise ValueError("raw session does not contain exactly the measured joint set")
|
||||
source_urdf = Path(start["source_urdf_path"]).expanduser().resolve()
|
||||
extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve()
|
||||
if not source_urdf.is_file() or not extrinsics_file.is_file():
|
||||
raise ValueError("session source URDF or camera extrinsics is missing")
|
||||
if "zero_calibrated" in source_urdf.stem.lower():
|
||||
raise ValueError("offline replay requires the original CAD URDF")
|
||||
hand_serial = str(serial_number or session.parent.name)
|
||||
output_suffix = _output_suffix(output_tag)
|
||||
repetitions = int(parameters["repetitions"])
|
||||
|
||||
measured_fits = {
|
||||
name: _fit_curve(
|
||||
name,
|
||||
records_by_joint[name],
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
)
|
||||
for name in profile.measured_joints
|
||||
}
|
||||
training_fits = {
|
||||
name: _fit_curve(
|
||||
name,
|
||||
[
|
||||
record
|
||||
for record in records_by_joint[name]
|
||||
if int(record["cycle"]) in {0, 1}
|
||||
],
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
)
|
||||
for name in profile.measured_joints
|
||||
}
|
||||
axes = _fit_axes(
|
||||
records_by_joint=records_by_joint,
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
extrinsics_file=extrinsics_file,
|
||||
repetitions=repetitions,
|
||||
)
|
||||
failures = _quality_failures(
|
||||
records_by_joint=records_by_joint,
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
fits=measured_fits,
|
||||
axes=axes,
|
||||
parameters=parameters,
|
||||
)
|
||||
if failures:
|
||||
raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures))
|
||||
|
||||
holdout_by_joint = {
|
||||
name: rotation_curve_holdout_errors(
|
||||
training_fits[name],
|
||||
[
|
||||
record
|
||||
for record in records_by_joint[name]
|
||||
if int(record["cycle"]) == 2
|
||||
],
|
||||
zero_command_u8=int(
|
||||
baseline[profile.joint_specs[name].motor_index]
|
||||
),
|
||||
)
|
||||
for name in profile.measured_joints
|
||||
}
|
||||
trajectory_errors = np.abs(
|
||||
np.asarray(
|
||||
[value for values in holdout_by_joint.values() for value in values],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
maximum_validation_mae = math.radians(
|
||||
float(parameters["maximum_validation_mae_deg"])
|
||||
)
|
||||
maximum_validation_p95 = math.radians(
|
||||
float(parameters["maximum_validation_p95_deg"])
|
||||
)
|
||||
trajectory_mae = float(np.mean(trajectory_errors))
|
||||
trajectory_p95 = float(np.percentile(trajectory_errors, 95.0))
|
||||
if (
|
||||
trajectory_mae > maximum_validation_mae
|
||||
or trajectory_p95 > maximum_validation_p95
|
||||
):
|
||||
raise ValueError("third-cycle trajectory holdout failed")
|
||||
|
||||
motor_by_joint = {
|
||||
name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
|
||||
}
|
||||
joint_limits: dict[str, float] = {}
|
||||
solve_arguments = {
|
||||
"source_urdf": source_urdf,
|
||||
"measurements": axes,
|
||||
"motor_by_joint": motor_by_joint,
|
||||
"maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
|
||||
"finger_maximum_offset_rad": math.radians(
|
||||
float(parameters.get("zero_finger_maximum_offset_deg", 3.0))
|
||||
),
|
||||
"joint_maximum_offset_rad": joint_limits,
|
||||
"maximum_cycle_difference_rad": math.radians(
|
||||
float(parameters["zero_maximum_axis_cycle_difference_deg"])
|
||||
),
|
||||
"maximum_axis_cone_mismatch_rad": math.radians(
|
||||
float(parameters["zero_maximum_axis_cone_mismatch_deg"])
|
||||
),
|
||||
"maximum_pose_axis_line_rms_m": float(
|
||||
parameters["axis_maximum_pose_line_rms_m"]
|
||||
),
|
||||
"maximum_validation_mae_rad": maximum_validation_mae,
|
||||
"maximum_validation_p95_rad": maximum_validation_p95,
|
||||
"hand_type": side,
|
||||
}
|
||||
holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
|
||||
if not holdout_zero.passed:
|
||||
failure = {
|
||||
"reasons": dict(holdout_zero.failure_reasons),
|
||||
"fitted_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.direct_offsets_rad.items()
|
||||
},
|
||||
"cycle_offsets_deg": {
|
||||
name: [math.degrees(value) for value in values]
|
||||
for name, values in holdout_zero.cycle_offsets_rad.items()
|
||||
},
|
||||
}
|
||||
raise ValueError(
|
||||
"third-cycle zero/URDF holdout failed: "
|
||||
+ json.dumps(failure, ensure_ascii=False, sort_keys=True)
|
||||
)
|
||||
final_zero = solve_urdf_zero_offsets(curves=measured_fits, **solve_arguments)
|
||||
if not final_zero.passed:
|
||||
raise ValueError(
|
||||
"all-cycle zero refit failed: "
|
||||
+ json.dumps(
|
||||
{
|
||||
"reasons": dict(final_zero.failure_reasons),
|
||||
"fitted_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in final_zero.direct_offsets_rad.items()
|
||||
},
|
||||
},
|
||||
ensure_ascii=False,
|
||||
sort_keys=True,
|
||||
)
|
||||
)
|
||||
for target, source_name in zero_profile.inherited_static_zero_joints.items():
|
||||
if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]:
|
||||
raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}")
|
||||
for target in (
|
||||
set(zero_profile.inherited_zero_joints)
|
||||
- set(zero_profile.inherited_static_zero_joints)
|
||||
):
|
||||
if final_zero.all_active_offsets_rad[target] != 0.0:
|
||||
raise ValueError(f"unobserved static zero must retain source CAD: {target}")
|
||||
|
||||
validation_errors = [
|
||||
float(value) for values in holdout_by_joint.values() for value in values
|
||||
]
|
||||
validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad)
|
||||
payload = build_compact_payload(
|
||||
serial_number=hand_serial,
|
||||
measured_fits=measured_fits,
|
||||
urdf_zero_offsets_rad=final_zero.all_active_offsets_rad,
|
||||
validation_errors_rad=validation_errors,
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
side=side,
|
||||
)
|
||||
validate_compact_payload(payload)
|
||||
|
||||
stamp = session.name
|
||||
final_json = session / (
|
||||
f"g20_{side}_{hand_serial}_calibration{output_suffix}.json"
|
||||
)
|
||||
expected_urdf_name = (
|
||||
f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}"
|
||||
f"{output_suffix}.urdf"
|
||||
)
|
||||
final_urdf = source_urdf.parent / expected_urdf_name
|
||||
report_path = session / (
|
||||
f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json"
|
||||
)
|
||||
if write_outputs:
|
||||
existing = [path for path in (final_json, final_urdf, report_path) if path.exists()]
|
||||
if existing:
|
||||
raise ValueError(
|
||||
"refusing to overwrite replay outputs: "
|
||||
+ ", ".join(str(path) for path in existing)
|
||||
)
|
||||
|
||||
source_hash_before = _sha256(source_urdf)
|
||||
with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary:
|
||||
candidate = write_zero_corrected_urdf(
|
||||
source_urdf=source_urdf,
|
||||
output_directory=temporary,
|
||||
serial_number=hand_serial,
|
||||
offsets_rad=final_zero.all_active_offsets_rad,
|
||||
timestamp=stamp,
|
||||
)
|
||||
urdf_checks = _validate_corrected_urdf(
|
||||
source=source_urdf,
|
||||
corrected=candidate,
|
||||
offsets=final_zero.all_active_offsets_rad,
|
||||
axes=axes,
|
||||
curves=measured_fits,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero_profile.inherited_zero_joints,
|
||||
)
|
||||
residual_zero = solve_urdf_zero_offsets(
|
||||
curves=measured_fits,
|
||||
fixed_direct_zero_offsets_rad={
|
||||
name: 0.0
|
||||
for name in zero_profile.fixed_direct_zero_offsets_rad
|
||||
},
|
||||
static_output_zero_offsets_rad={
|
||||
name: 0.0
|
||||
for name in zero_profile.static_output_zero_offsets_rad
|
||||
},
|
||||
**{**solve_arguments, "source_urdf": candidate},
|
||||
)
|
||||
maximum_residual_offset = max(
|
||||
abs(float(value)) for value in residual_zero.direct_offsets_rad.values()
|
||||
)
|
||||
if not residual_zero.passed or maximum_residual_offset > math.radians(0.3):
|
||||
raise ValueError("written URDF retains a significant zero correction")
|
||||
candidate_hash = _sha256(candidate)
|
||||
if write_outputs:
|
||||
os.replace(candidate, final_urdf)
|
||||
|
||||
if _sha256(source_urdf) != source_hash_before:
|
||||
raise ValueError("source URDF changed during offline replay")
|
||||
|
||||
report: dict[str, Any] = {
|
||||
"passed": True,
|
||||
"session_dir": str(session),
|
||||
"side": side,
|
||||
"serial_number": hand_serial,
|
||||
"output_tag": output_tag,
|
||||
"raw_samples_sha256": _sha256(raw_path),
|
||||
"source_urdf": str(source_urdf),
|
||||
"source_urdf_sha256": source_hash_before,
|
||||
"joint_limits_deg": {
|
||||
"finger_default": float(
|
||||
parameters.get("zero_finger_maximum_offset_deg", 3.0)
|
||||
),
|
||||
"thumb_default": float(parameters["zero_maximum_offset_deg"]),
|
||||
},
|
||||
"static_zero_policy": "direct_measurements_only",
|
||||
"fixed_zero_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in zero_profile.fixed_direct_zero_offsets_rad.items()
|
||||
},
|
||||
"static_output_zero_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in zero_profile.static_output_zero_offsets_rad.items()
|
||||
},
|
||||
"direct_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in final_zero.direct_offsets_rad.items()
|
||||
},
|
||||
"cycle_offsets_deg": {
|
||||
name: [math.degrees(value) for value in values]
|
||||
for name, values in holdout_zero.cycle_offsets_rad.items()
|
||||
},
|
||||
"offset_uncertainty_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.offset_uncertainty_rad.items()
|
||||
},
|
||||
"trajectory_holdout_mae_deg": math.degrees(trajectory_mae),
|
||||
"trajectory_holdout_p95_deg": math.degrees(trajectory_p95),
|
||||
"zero_holdout_error_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.validation_error_by_joint_rad.items()
|
||||
},
|
||||
"zero_original_error_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.validation_original_error_by_joint_rad.items()
|
||||
},
|
||||
"zero_improvement_95pct_lower_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in (
|
||||
holdout_zero.validation_improvement_confidence_lower_rad.items()
|
||||
)
|
||||
},
|
||||
"corrected_urdf_checks": {
|
||||
**urdf_checks,
|
||||
"maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
|
||||
},
|
||||
"corrected_urdf_sha256": candidate_hash,
|
||||
"final_json": str(final_json) if write_outputs else None,
|
||||
"corrected_urdf": str(final_urdf) if write_outputs else None,
|
||||
}
|
||||
if write_outputs:
|
||||
atomic_write_json(final_json, payload)
|
||||
report["final_json_sha256"] = _sha256(final_json)
|
||||
if _sha256(final_urdf) != candidate_hash:
|
||||
raise ValueError("formal corrected URDF differs from validated candidate")
|
||||
atomic_write_json(report_path, report)
|
||||
report["validation_report"] = str(report_path)
|
||||
return report
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Replay and independently validate a complete G20 calibration session."
|
||||
)
|
||||
parser.add_argument("session_dir")
|
||||
parser.add_argument("--serial-number", default=None)
|
||||
parser.add_argument("--config-file", default=None)
|
||||
parser.add_argument("--write", action="store_true")
|
||||
parser.add_argument(
|
||||
"--output-tag",
|
||||
default=None,
|
||||
help="safe suffix for a replay variant; existing outputs are never overwritten",
|
||||
)
|
||||
arguments = parser.parse_args()
|
||||
result = replay_session(
|
||||
arguments.session_dir,
|
||||
serial_number=arguments.serial_number,
|
||||
config_file=arguments.config_file,
|
||||
write_outputs=arguments.write,
|
||||
output_tag=arguments.output_tag,
|
||||
)
|
||||
print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
File diff suppressed because it is too large
Load Diff
-481
@@ -1,481 +0,0 @@
|
||||
"""Chinese, operator-facing diagnostics for three-camera calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
STATE_NAMES_ZH = {
|
||||
"PREFLIGHT": "设备和标签预检",
|
||||
"WAIT_START": "等待开始标定",
|
||||
"RETURN_BASELINE": "正在恢复目标姿态",
|
||||
"PREPARE_SWEEP": "正在到达扫描起点",
|
||||
"SWEEP": "正在采集轨迹",
|
||||
"FITTING": "正在拟合轨迹和零位",
|
||||
"VALIDATION_MOVE": "正在移动到随机复测位置",
|
||||
"VALIDATION_CAPTURE": "正在采集随机复测数据",
|
||||
"PAUSED": "标定已暂停",
|
||||
"ABORTED": "标定已终止",
|
||||
"COMPLETE": "标定已完成",
|
||||
}
|
||||
|
||||
VIEW_NAMES_ZH = {
|
||||
"front": "正面",
|
||||
"side": "侧面",
|
||||
"top": "上面",
|
||||
}
|
||||
|
||||
JOINT_NAMES_ZH = {
|
||||
"thumb_cmc_pitch": "拇指CMC俯仰",
|
||||
"thumb_cmc_roll": "拇指CMC滚转",
|
||||
"thumb_mcp": "拇指MCP",
|
||||
"thumb_ip": "拇指IP（被动）",
|
||||
"index_mcp_roll": "食指MCP侧摆",
|
||||
"index_mcp_pitch": "食指MCP屈伸",
|
||||
"index_pip": "食指PIP",
|
||||
"index_dip": "食指DIP（被动）",
|
||||
"middle_mcp_roll": "中指MCP侧摆",
|
||||
"middle_mcp_pitch": "中指MCP屈伸",
|
||||
"middle_pip": "中指PIP",
|
||||
"middle_dip": "中指DIP（被动）",
|
||||
"ring_mcp_roll": "无名指MCP侧摆",
|
||||
"ring_mcp_pitch": "无名指MCP屈伸",
|
||||
"ring_pip": "无名指PIP",
|
||||
"ring_dip": "无名指DIP（被动）",
|
||||
"pinky_mcp_roll": "小指MCP侧摆",
|
||||
"pinky_mcp_pitch": "小指MCP屈伸",
|
||||
"pinky_pip": "小指PIP",
|
||||
"pinky_dip": "小指DIP（被动）",
|
||||
"thumb_cmc_yaw": "拇指CMC侧摆",
|
||||
}
|
||||
|
||||
|
||||
def _format_u8(value: Any) -> str:
|
||||
if value is None:
|
||||
return "尚无反馈"
|
||||
return f"{float(value):.1f}"
|
||||
|
||||
|
||||
def _task_text(active: Mapping[str, Any]) -> str:
|
||||
if not active:
|
||||
return "尚无活动任务"
|
||||
view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
|
||||
if active.get("kind") == "fit_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位，{joint_text}拟合检查失败，"
|
||||
f"电机{active.get('motor_index')}，"
|
||||
f"第{active.get('attempt', 1)}次尝试"
|
||||
)
|
||||
if active.get("kind") == "zero_model_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位，{joint_text}零位/URDF验证失败，"
|
||||
f"电机{active.get('motor_index')}，不会自动重扫"
|
||||
)
|
||||
if active.get("kind") == "motion_stall":
|
||||
return (
|
||||
f"电机{active.get('motor_index', '?')}运动停滞，目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}"
|
||||
)
|
||||
if active.get("kind") == "validation":
|
||||
return (
|
||||
f"{view}机位，随机复测，电机{active.get('motor_index')}，"
|
||||
f"目标命令{active.get('command_u8')}"
|
||||
)
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
start = active.get("start_u8")
|
||||
target = active.get("target_u8")
|
||||
cycle = active.get("cycle", "?")
|
||||
repetitions = active.get("repetitions", "?")
|
||||
task = (
|
||||
f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
|
||||
f"第{cycle}/{repetitions}轮，{start}→{target}"
|
||||
)
|
||||
fit_attempt = int(active.get("fit_attempt", 1))
|
||||
if fit_attempt > 1:
|
||||
task += (
|
||||
f"（整关节自动重采第{fit_attempt}/"
|
||||
f"{active.get('fit_attempt_limit', '?')}次）"
|
||||
)
|
||||
return task
|
||||
|
||||
|
||||
def three_camera_reason_zh(
|
||||
state: str,
|
||||
reason: str,
|
||||
active: Mapping[str, Any],
|
||||
) -> tuple[str, str]:
|
||||
"""Translate a reason code and provide one concrete operator action."""
|
||||
reason = str(reason)
|
||||
sample = active.get("sample", {}) if active else {}
|
||||
missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
|
||||
sample_range = (
|
||||
f"{_format_u8(sample.get('minimum_u8'))}～"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
tolerance = sample.get("endpoint_tolerance_u8", "?")
|
||||
|
||||
if reason.startswith("motor_state_stalled:"):
|
||||
fields = reason.split(":")
|
||||
context = fields[1] if len(fields) > 1 else "unknown"
|
||||
error_match = re.search(r"error_u8=([0-9.]+)", reason)
|
||||
error = error_match.group(1) if error_match else "未知"
|
||||
motor = active.get("motor_index")
|
||||
if motor is not None:
|
||||
return (
|
||||
f"电机{motor}反馈连续8秒没有向目标推进；目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，"
|
||||
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
|
||||
f"（阶段={context}）。程序已保持当前位置。",
|
||||
"若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后"
|
||||
"重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。",
|
||||
)
|
||||
return (
|
||||
f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8"
|
||||
f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。",
|
||||
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
|
||||
"正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。",
|
||||
)
|
||||
|
||||
if "URDF zero offset reached the configured" in reason:
|
||||
bound_match = re.search(
|
||||
r"configured\s+([0-9.]+)\s+degree bound", reason
|
||||
)
|
||||
bound = bound_match.group(1) if bound_match else "配置的"
|
||||
hit_text = ""
|
||||
if "bound:" in reason:
|
||||
hit_text = reason.split("bound:", 1)[1].split(
|
||||
"; all_offsets:", 1
|
||||
)[0]
|
||||
for name, label in JOINT_NAMES_ZH.items():
|
||||
hit_text = hit_text.replace(name, label)
|
||||
hit_suffix = f"；触边关节：{hit_text}" if hit_text else ""
|
||||
return (
|
||||
f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
|
||||
"零位结果，而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
|
||||
"不要调用resume，也不要增大零位边界。先确认Tag有效黑框边长、三相机"
|
||||
"内外参和原始CAD URDF；Tag尺寸修正后必须调用start重新采集，旧尺度"
|
||||
"产生的轨迹不能直接生成修正URDF。",
|
||||
)
|
||||
|
||||
if reason == "sweep_missing_endpoint_bin":
|
||||
missing_text = "、".join(str(value) for value in missing) or "0或255"
|
||||
return (
|
||||
f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少"
|
||||
f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为"
|
||||
f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到"
|
||||
"端点，但该时刻没有同时取得有效Tag图像和电机状态。",
|
||||
"确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用"
|
||||
"/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。",
|
||||
)
|
||||
if reason == "sweep_bins_too_few":
|
||||
return (
|
||||
f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
|
||||
f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
|
||||
"检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
|
||||
)
|
||||
if reason == "sweep_bin_gap_too_large":
|
||||
return (
|
||||
f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
|
||||
f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
|
||||
"检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
|
||||
)
|
||||
if reason == "synchronised_tag_state_timeout":
|
||||
return (
|
||||
"运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
"按时间戳配对”的图像帧。",
|
||||
"查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
|
||||
"程序会重扫当前方向。",
|
||||
)
|
||||
if reason == "sweep_start_position_timeout":
|
||||
return (
|
||||
f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
|
||||
f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。",
|
||||
"检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。",
|
||||
)
|
||||
if reason == "sweep_timeout":
|
||||
return (
|
||||
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
|
||||
"检查电机实际值、Tag连续识别和标定速度，修正后调用resume。",
|
||||
)
|
||||
if reason == "return_baseline_timeout":
|
||||
return (
|
||||
"一个或多个标定电机未在规定时间返回基准命令。",
|
||||
"检查机械手状态、CAN和机械卡阻，确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_move_timeout":
|
||||
return (
|
||||
"随机复测时电机未在规定时间到达目标命令。",
|
||||
"检查机械手状态和机械卡阻，确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_capture_timeout":
|
||||
return (
|
||||
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
||||
"检查当前机位Tag可见性后调用resume。",
|
||||
)
|
||||
if reason == "joint_fit_check_failed":
|
||||
metric_names = {
|
||||
"plane_rms_mm": "平面拟合RMS",
|
||||
"radial_rms_mm": "圆半径拟合RMS",
|
||||
"radius_mm": "拟合半径",
|
||||
"image_radial_rms_px": "二维圆半径拟合RMS",
|
||||
"image_radial_p95_px": "二维圆半径误差P95",
|
||||
"image_radius_px": "二维拟合半径",
|
||||
"arc_deg": "实测圆弧",
|
||||
"monotonic_correction_deg": "最大单调修正",
|
||||
"hysteresis_deg": "最大正反程差",
|
||||
"cycle_travel_range_deg": "三轮行程差",
|
||||
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
|
||||
"axis_plane_rms_mm": "三维圆轴向RMS",
|
||||
"axis_radial_rms_mm": "三维圆半径RMS",
|
||||
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
|
||||
"axis_cycle_difference_deg": "三轮转轴方向极差",
|
||||
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
||||
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
||||
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
||||
"state_image_sync_p95_ms": "图像与电机状态同步误差P95",
|
||||
"tag_valid_rate_percent": "所需Tag同时有效率",
|
||||
}
|
||||
metric_units = {
|
||||
"plane_rms_mm": "mm",
|
||||
"radial_rms_mm": "mm",
|
||||
"radius_mm": "mm",
|
||||
"image_radial_rms_px": "px",
|
||||
"image_radial_p95_px": "px",
|
||||
"image_radius_px": "px",
|
||||
"arc_deg": "°",
|
||||
"monotonic_correction_deg": "°",
|
||||
"hysteresis_deg": "°",
|
||||
"cycle_travel_range_deg": "°",
|
||||
"rotation_orthogonal_rms_deg": "°",
|
||||
"axis_plane_rms_mm": "mm",
|
||||
"axis_radial_rms_mm": "mm",
|
||||
"rotation_circle_axis_difference_deg": "°",
|
||||
"axis_cycle_difference_deg": "°",
|
||||
"third_cycle_axis_holdout_deg": "°",
|
||||
"third_cycle_axis_line_rms_mm": "mm",
|
||||
"third_cycle_trajectory_p95_deg": "°",
|
||||
"state_image_sync_p95_ms": "ms",
|
||||
"tag_valid_rate_percent": "%",
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
metric = str(failure.get("metric", ""))
|
||||
if metric in metric_names:
|
||||
comparison = str(failure.get("comparison", ""))
|
||||
requirement = "不超过" if comparison == "maximum" else "至少"
|
||||
unit = metric_units[metric]
|
||||
detail = (
|
||||
f"{joint}的{metric_names[metric]}为"
|
||||
f"{float(failure.get('actual', 0.0)):.2f}{unit}，"
|
||||
f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
|
||||
)
|
||||
cycle_travel = failure.get("cycle_travel_deg", [])
|
||||
if cycle_travel:
|
||||
detail += "（三轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_travel
|
||||
) + "）"
|
||||
details.append(detail)
|
||||
else:
|
||||
cycle = failure.get("cycle")
|
||||
cycle_text = "" if cycle is None else f"第{cycle}轮"
|
||||
details.append(
|
||||
f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败："
|
||||
f"{failure.get('reason', '未知原因')}"
|
||||
)
|
||||
detail_text = "；".join(details) or "当前关节的轨迹拟合未通过"
|
||||
return (
|
||||
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
|
||||
"修正Tag位置、遮挡或机械行程后调用"
|
||||
"/g20_calibration/resume；程序只清除当前失败关节的数据"
|
||||
f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。",
|
||||
)
|
||||
if reason == "zero_model_validation_failed":
|
||||
reason_names = {
|
||||
"zero_offset_reached_configured_bound": "零位解触及安全边界",
|
||||
"zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
|
||||
"zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
|
||||
"zero_offset_cycle_difference_too_large": "三轮零位离散过大",
|
||||
"zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
|
||||
"zero_axis_cone_mismatch_too_large": (
|
||||
"父子轴夹角与原始URDF不一致，零位旋转无法解释"
|
||||
),
|
||||
"zero_phase_axis_line_residual_too_large": (
|
||||
"整段SE(3)运动无法稳定确定平行轴线相位"
|
||||
),
|
||||
"zero_offset_did_not_improve_with_95pct_confidence": (
|
||||
"第三轮留出验证未以95%置信度改善"
|
||||
),
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
if failure.get("metric") == "zero_guard":
|
||||
reason_text = reason_names.get(
|
||||
str(failure.get("reason")), str(failure.get("reason"))
|
||||
)
|
||||
if "actual_deg" in failure and "limit_deg" in failure:
|
||||
reason_text += (
|
||||
f"（估计{float(failure['actual_deg']):+.2f}°，"
|
||||
f"允许±{float(failure['limit_deg']):.2f}°）"
|
||||
)
|
||||
details.append(f"{joint}：{reason_text}")
|
||||
return (
|
||||
"轨迹采集已完成，但零位/URDF几何验证失败"
|
||||
+ ("（" + "；".join(details) + "）" if details else "")
|
||||
+ "。程序没有生成正式JSON或修正URDF。",
|
||||
"该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；"
|
||||
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
|
||||
)
|
||||
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "call_start":
|
||||
return (
|
||||
"三机位预检已经通过，等待操作员确认开始。",
|
||||
"清空机械手运动范围后调用/g20_calibration/start。",
|
||||
)
|
||||
if reason == "operator_pause":
|
||||
return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
|
||||
if reason == "operator_abort":
|
||||
return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
|
||||
if reason == "collecting_timestamp_synchronised_tag_centres":
|
||||
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。"
|
||||
if reason == "capturing_random_validation_pose":
|
||||
return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。"
|
||||
if reason in {"calibration_passed", "calibration_complete"}:
|
||||
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
||||
if reason == "quality_failed":
|
||||
return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
||||
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
|
||||
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
|
||||
if state == "RETURN_BASELINE":
|
||||
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
|
||||
return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
|
||||
|
||||
|
||||
def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
"""Render the complete operator status; the JSON topic remains unchanged."""
|
||||
state = str(payload.get("state", ""))
|
||||
active = payload.get("active", {})
|
||||
reason_zh, action_zh = three_camera_reason_zh(
|
||||
state, str(payload.get("reason", "")), active
|
||||
)
|
||||
progress = float(payload.get("progress", 0.0))
|
||||
completed = payload.get("completed_sweeps", 0)
|
||||
total = payload.get("total_sweeps", 0)
|
||||
scan_progress = float(
|
||||
payload.get(
|
||||
"scan_progress",
|
||||
0.0 if not total else float(completed) / float(total),
|
||||
)
|
||||
)
|
||||
lines = [
|
||||
f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）",
|
||||
f"原因：{reason_zh}",
|
||||
f"建议：{action_zh}",
|
||||
f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，"
|
||||
f"扫描进度{scan_progress:.1%}）",
|
||||
f"当前任务：{_task_text(active)}",
|
||||
]
|
||||
if state == "RETURN_BASELINE":
|
||||
baseline_command = payload.get("baseline_command_u8", [])
|
||||
return_command = payload.get("return_command_u8", baseline_command)
|
||||
label = "恢复姿态" if return_command != baseline_command else "基准姿态"
|
||||
lines.append(f"正在确认{label}：{return_command}")
|
||||
if active and active.get("kind") not in {
|
||||
"fit_failure",
|
||||
"zero_model_failure",
|
||||
"motion_stall",
|
||||
}:
|
||||
retry_count = int(active.get("automatic_retry_count", 0))
|
||||
if retry_count:
|
||||
lines.append(
|
||||
"自动重试：当前方向已自动重扫"
|
||||
f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，"
|
||||
f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，"
|
||||
f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
|
||||
)
|
||||
sample = active.get("sample", {})
|
||||
motion_progress = active.get("motion_progress")
|
||||
motion_text = (
|
||||
"未知" if motion_progress is None else f"{float(motion_progress):.1%}"
|
||||
)
|
||||
lines.append(
|
||||
"运动采样："
|
||||
f"目标{active.get('target_u8', active.get('command_u8', '?'))}，"
|
||||
f"实际{_format_u8(active.get('actual_u8'))}，"
|
||||
f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，"
|
||||
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
auxiliary = active.get("auxiliary_motors", [])
|
||||
if auxiliary:
|
||||
lines.append(
|
||||
"避挡姿态："
|
||||
+ "，".join(
|
||||
f"电机{item.get('motor_index')}目标"
|
||||
f"{item.get('command_u8')}、实际"
|
||||
f"{_format_u8(item.get('actual_u8'))}"
|
||||
for item in auxiliary
|
||||
)
|
||||
)
|
||||
speed = active.get("speed", {})
|
||||
if speed:
|
||||
lines.append(
|
||||
"阶段速度：五指目标"
|
||||
f"{speed.get('commanded_finger_speed')}，SDK报告"
|
||||
f"{speed.get('reported_finger_speed')}"
|
||||
)
|
||||
if active.get("sweep_timeout_seconds") is not None:
|
||||
lines.append(
|
||||
"运动保护：扫描超时"
|
||||
f"{float(active['sweep_timeout_seconds']):.1f}s，"
|
||||
"连续"
|
||||
f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
|
||||
"进展不足"
|
||||
f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
|
||||
"则立即暂停"
|
||||
)
|
||||
lines.append("机位：")
|
||||
for name, view in payload.get("views", {}).items():
|
||||
missing = view.get("missing_tag_ids", [])
|
||||
missing_text = "无" if not missing else ",".join(map(str, missing))
|
||||
lines.append(
|
||||
f"- {VIEW_NAMES_ZH.get(str(name), str(name))}："
|
||||
f"{'就绪' if view.get('ready') else '等待'}，"
|
||||
f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'}，"
|
||||
f"{float(view.get('detection_hz', 0.0)):.1f}Hz，"
|
||||
f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，"
|
||||
f"当前缺失Tag={missing_text}"
|
||||
)
|
||||
extrinsics_error = payload.get("camera_extrinsics_error")
|
||||
if extrinsics_error:
|
||||
lines.append(f"外参文件：{extrinsics_error}")
|
||||
lines.append(f"JSON结果：{payload.get('result_path') or '尚未生成'}")
|
||||
lines.append(
|
||||
f"修正URDF：{payload.get('corrected_urdf_path') or '尚未生成'}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
-3845
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,272 +0,0 @@
|
||||
"""Launch front-camera trajectory-circle CMC pitch zero measurement."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
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
|
||||
|
||||
|
||||
def _launch_stack(context):
|
||||
serial_number = LaunchConfiguration("serial_number").perform(context)
|
||||
if (
|
||||
not serial_number
|
||||
or serial_number == "UNSET"
|
||||
or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
|
||||
or serial_number in {".", ".."}
|
||||
):
|
||||
raise RuntimeError(
|
||||
"serial_number is required and may contain only letters, "
|
||||
"digits, dot, underscore and dash"
|
||||
)
|
||||
|
||||
requested_session = LaunchConfiguration("session_dir").perform(context)
|
||||
output_root = Path(
|
||||
LaunchConfiguration("output_root").perform(context)
|
||||
).expanduser().resolve()
|
||||
if requested_session:
|
||||
session_dir = Path(requested_session).expanduser().resolve()
|
||||
else:
|
||||
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session_dir = output_root / serial_number / timestamp
|
||||
session_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
zero_config = LaunchConfiguration("zero_config").perform(context)
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration("start_camera")),
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": LaunchConfiguration("camera_serial_number"),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration("camera_name"),
|
||||
"frame_id": LaunchConfiguration("camera_frame_id"),
|
||||
"image_width": ParameterValue(
|
||||
LaunchConfiguration("image_width"), value_type=int
|
||||
),
|
||||
"image_height": ParameterValue(
|
||||
LaunchConfiguration("image_height"), value_type=int
|
||||
),
|
||||
"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": ParameterValue(
|
||||
LaunchConfiguration("auto_exposure"), value_type=bool
|
||||
),
|
||||
"camera_info_url": LaunchConfiguration("camera_info_url"),
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
raw_topic = "/camera/camera/color/image_raw"
|
||||
camera_info_topic = "/camera/camera/color/camera_info"
|
||||
rect_topic = "/camera/camera/color/image_rect"
|
||||
vision_container = ComposableNodeContainer(
|
||||
name="g20_thumb_zero_vision_container",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
package="image_proc",
|
||||
plugin="image_proc::RectifyNode",
|
||||
name="rectify_color",
|
||||
namespace="/camera/camera/color",
|
||||
remappings=[
|
||||
("image", raw_topic),
|
||||
("camera_info", camera_info_topic),
|
||||
("image_rect", rect_topic),
|
||||
],
|
||||
parameters=[{"queue_size": 1}],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
),
|
||||
ComposableNode(
|
||||
package="apriltag_ros",
|
||||
plugin="AprilTagNode",
|
||||
name="apriltag",
|
||||
namespace="/apriltag",
|
||||
parameters=[
|
||||
tag_config,
|
||||
{
|
||||
"detector.decimate": ParameterValue(
|
||||
LaunchConfiguration("apriltag_decimate"),
|
||||
value_type=float,
|
||||
)
|
||||
},
|
||||
],
|
||||
remappings=[
|
||||
("image_rect", rect_topic),
|
||||
("camera_info", camera_info_topic),
|
||||
],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
),
|
||||
],
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
)
|
||||
|
||||
sdk = Node(
|
||||
package="linker_hand_ros2_sdk",
|
||||
executable="linker_hand_sdk",
|
||||
name="linker_hand_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": "left",
|
||||
"hand_joint": "G20",
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": "/g20",
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"startup_torque": 80,
|
||||
"state_poll_rate": 10.0,
|
||||
"repeat_position_commands": False,
|
||||
"is_touch": False,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
zero_node = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="cmc_pitch_zero_node",
|
||||
name="g20_thumb_cmc_pitch_zero",
|
||||
output="screen",
|
||||
parameters=[
|
||||
zero_config,
|
||||
{
|
||||
"serial_number": serial_number,
|
||||
"session_dir": str(session_dir),
|
||||
"commands_enabled": ParameterValue(
|
||||
LaunchConfiguration("commands_enabled"),
|
||||
value_type=bool,
|
||||
),
|
||||
"image_topic": rect_topic,
|
||||
"publish_debug_image": ParameterValue(
|
||||
LaunchConfiguration("publish_debug_image"),
|
||||
value_type=bool,
|
||||
),
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
return [
|
||||
LogInfo(msg=f"G20 CMC pitch zero session: {session_dir}"),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"Only T0(ID 0) and T3(ID 1) are required; "
|
||||
"T4/T5 detections are ignored"
|
||||
)
|
||||
),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"Motor 0 performs three 255->64->255 sweeps; "
|
||||
"zero angles come from the fitted T3-centre trajectory radius"
|
||||
)
|
||||
),
|
||||
camera,
|
||||
vision_container,
|
||||
sdk,
|
||||
zero_node,
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_serial_number", default_value="DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_model", default_value="MV-CS020-10UM"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_name", default_value="hikrobot_front_DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_frame_id", default_value="camera_color_optical_frame"
|
||||
),
|
||||
DeclareLaunchArgument("image_width", default_value="1624"),
|
||||
DeclareLaunchArgument("image_height", default_value="1240"),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_info_url",
|
||||
default_value=str(
|
||||
Path.home()
|
||||
/ ".ros"
|
||||
/ "camera_info"
|
||||
/ "hikrobot_DB2163742.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
|
||||
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
||||
DeclareLaunchArgument(
|
||||
"publish_debug_image", default_value="true"
|
||||
),
|
||||
DeclareLaunchArgument("start_camera", default_value="true"),
|
||||
DeclareLaunchArgument("start_sdk", default_value="true"),
|
||||
DeclareLaunchArgument(
|
||||
"output_root",
|
||||
default_value=str(Path.cwd() / "calibration_output"),
|
||||
),
|
||||
DeclareLaunchArgument("session_dir", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"zero_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "cmc_pitch_zero.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "front_tags.yaml"
|
||||
),
|
||||
),
|
||||
OpaqueFunction(function=_launch_stack),
|
||||
]
|
||||
)
|
||||
@@ -1,276 +0,0 @@
|
||||
"""Launch front-camera trajectory-circle CMC roll zero/travel calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
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
|
||||
|
||||
|
||||
def _launch_stack(context):
|
||||
serial_number = LaunchConfiguration("serial_number").perform(context)
|
||||
if (
|
||||
not serial_number
|
||||
or serial_number == "UNSET"
|
||||
or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
|
||||
or serial_number in {".", ".."}
|
||||
):
|
||||
raise RuntimeError(
|
||||
"serial_number is required and may contain only letters, "
|
||||
"digits, dot, underscore and dash"
|
||||
)
|
||||
|
||||
requested_session = LaunchConfiguration("session_dir").perform(context)
|
||||
output_root = Path(
|
||||
LaunchConfiguration("output_root").perform(context)
|
||||
).expanduser().resolve()
|
||||
if requested_session:
|
||||
session_dir = Path(requested_session).expanduser().resolve()
|
||||
else:
|
||||
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session_dir = output_root / serial_number / timestamp
|
||||
session_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
calibration_config = LaunchConfiguration(
|
||||
"calibration_config"
|
||||
).perform(context)
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration("start_camera")),
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": LaunchConfiguration("camera_serial_number"),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration("camera_name"),
|
||||
"frame_id": LaunchConfiguration("camera_frame_id"),
|
||||
"image_width": ParameterValue(
|
||||
LaunchConfiguration("image_width"), value_type=int
|
||||
),
|
||||
"image_height": ParameterValue(
|
||||
LaunchConfiguration("image_height"), value_type=int
|
||||
),
|
||||
"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": ParameterValue(
|
||||
LaunchConfiguration("auto_exposure"), value_type=bool
|
||||
),
|
||||
"camera_info_url": LaunchConfiguration("camera_info_url"),
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
raw_topic = "/camera/camera/color/image_raw"
|
||||
camera_info_topic = "/camera/camera/color/camera_info"
|
||||
rect_topic = "/camera/camera/color/image_rect"
|
||||
vision_container = ComposableNodeContainer(
|
||||
name="g20_thumb_roll_vision_container",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
package="image_proc",
|
||||
plugin="image_proc::RectifyNode",
|
||||
name="rectify_color",
|
||||
namespace="/camera/camera/color",
|
||||
remappings=[
|
||||
("image", raw_topic),
|
||||
("camera_info", camera_info_topic),
|
||||
("image_rect", rect_topic),
|
||||
],
|
||||
parameters=[{"queue_size": 1}],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
),
|
||||
ComposableNode(
|
||||
package="apriltag_ros",
|
||||
plugin="AprilTagNode",
|
||||
name="apriltag",
|
||||
namespace="/apriltag",
|
||||
parameters=[
|
||||
tag_config,
|
||||
{
|
||||
"detector.decimate": ParameterValue(
|
||||
LaunchConfiguration("apriltag_decimate"),
|
||||
value_type=float,
|
||||
)
|
||||
},
|
||||
],
|
||||
remappings=[
|
||||
("image_rect", rect_topic),
|
||||
("camera_info", camera_info_topic),
|
||||
],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
),
|
||||
],
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
)
|
||||
|
||||
sdk = Node(
|
||||
package="linker_hand_ros2_sdk",
|
||||
executable="linker_hand_sdk",
|
||||
name="linker_hand_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": "left",
|
||||
"hand_joint": "G20",
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": "/g20",
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"startup_torque": 80,
|
||||
"state_poll_rate": 10.0,
|
||||
"repeat_position_commands": False,
|
||||
"is_touch": False,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
calibration_node = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="cmc_roll_calibration_node",
|
||||
name="g20_thumb_cmc_roll_calibration",
|
||||
output="screen",
|
||||
parameters=[
|
||||
calibration_config,
|
||||
{
|
||||
"serial_number": serial_number,
|
||||
"session_dir": str(session_dir),
|
||||
"commands_enabled": ParameterValue(
|
||||
LaunchConfiguration("commands_enabled"),
|
||||
value_type=bool,
|
||||
),
|
||||
"image_topic": rect_topic,
|
||||
"publish_debug_image": ParameterValue(
|
||||
LaunchConfiguration("publish_debug_image"),
|
||||
value_type=bool,
|
||||
),
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
return [
|
||||
LogInfo(msg=f"G20 CMC roll calibration session: {session_dir}"),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"Only T0(ID 0) and T3(ID 1) are required; "
|
||||
"T4/T5 detections are ignored"
|
||||
)
|
||||
),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"Motor 5 performs three 255->0->255 sweeps; "
|
||||
"static captures measure both zero and angular travel"
|
||||
)
|
||||
),
|
||||
camera,
|
||||
vision_container,
|
||||
sdk,
|
||||
calibration_node,
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_serial_number", default_value="DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_model", default_value="MV-CS020-10UM"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_name", default_value="hikrobot_front_DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_frame_id", default_value="camera_color_optical_frame"
|
||||
),
|
||||
DeclareLaunchArgument("image_width", default_value="1624"),
|
||||
DeclareLaunchArgument("image_height", default_value="1240"),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_info_url",
|
||||
default_value=str(
|
||||
Path.home()
|
||||
/ ".ros"
|
||||
/ "camera_info"
|
||||
/ "hikrobot_DB2163742.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
|
||||
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
||||
DeclareLaunchArgument(
|
||||
"publish_debug_image", default_value="true"
|
||||
),
|
||||
DeclareLaunchArgument("start_camera", default_value="true"),
|
||||
DeclareLaunchArgument("start_sdk", default_value="true"),
|
||||
DeclareLaunchArgument(
|
||||
"output_root",
|
||||
default_value=str(Path.cwd() / "calibration_output"),
|
||||
),
|
||||
DeclareLaunchArgument("session_dir", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config",
|
||||
default_value=str(
|
||||
package_share
|
||||
/ "config"
|
||||
/ "cmc_roll_zero_travel.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "front_tags.yaml"
|
||||
),
|
||||
),
|
||||
OpaqueFunction(function=_launch_stack),
|
||||
]
|
||||
)
|
||||
@@ -1,391 +0,0 @@
|
||||
"""Launch the complete front-camera G20 thumb calibration stack."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
ExecuteProcess,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
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
|
||||
|
||||
|
||||
def _launch_stack(context):
|
||||
serial_number = LaunchConfiguration("serial_number").perform(context)
|
||||
if not serial_number or serial_number == "UNSET":
|
||||
raise RuntimeError(
|
||||
"serial_number is required, for example serial_number:=G20_LEFT_001"
|
||||
)
|
||||
if (
|
||||
re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
|
||||
or serial_number in {".", ".."}
|
||||
):
|
||||
raise RuntimeError(
|
||||
"serial_number may contain only letters, digits, dot, underscore and dash"
|
||||
)
|
||||
requested_session = LaunchConfiguration("session_dir").perform(context)
|
||||
output_root = Path(LaunchConfiguration("output_root").perform(context)).resolve()
|
||||
if requested_session:
|
||||
session_dir = Path(requested_session).expanduser().resolve()
|
||||
else:
|
||||
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session_dir = output_root / serial_number / timestamp
|
||||
session_dir.mkdir(parents=True, exist_ok=True)
|
||||
bag_path = session_dir / "rosbag"
|
||||
|
||||
calibration_config = LaunchConfiguration("calibration_config").perform(context)
|
||||
tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower()
|
||||
if use_roi_text not in {"true", "false"}:
|
||||
raise RuntimeError("use_roi must be true or false")
|
||||
use_roi = use_roi_text == "true"
|
||||
|
||||
roi_values = {}
|
||||
for name in ("roi_x", "roi_y", "roi_width", "roi_height"):
|
||||
text = LaunchConfiguration(name).perform(context)
|
||||
try:
|
||||
roi_values[name] = int(text)
|
||||
except ValueError as error:
|
||||
raise RuntimeError(f"{name} must be an integer") from error
|
||||
if roi_values["roi_x"] < 0 or roi_values["roi_y"] < 0:
|
||||
raise RuntimeError("roi_x and roi_y must be non-negative")
|
||||
if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0:
|
||||
raise RuntimeError("roi_width and roi_height must be positive")
|
||||
|
||||
try:
|
||||
image_width = int(LaunchConfiguration("image_width").perform(context))
|
||||
image_height = int(LaunchConfiguration("image_height").perform(context))
|
||||
except ValueError as error:
|
||||
raise RuntimeError("image_width and image_height must be integers") from error
|
||||
if image_width <= 0 or image_height <= 0:
|
||||
raise RuntimeError("image_width and image_height must be positive")
|
||||
if use_roi:
|
||||
if (
|
||||
roi_values["roi_x"] + roi_values["roi_width"] > image_width
|
||||
or roi_values["roi_y"] + roi_values["roi_height"] > image_height
|
||||
):
|
||||
raise RuntimeError(
|
||||
"ROI lies outside camera image "
|
||||
f"{image_width}x{image_height}"
|
||||
)
|
||||
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration("start_camera")),
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": LaunchConfiguration("camera_serial_number"),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration("camera_name"),
|
||||
"frame_id": LaunchConfiguration("camera_frame_id"),
|
||||
"image_width": ParameterValue(
|
||||
LaunchConfiguration("image_width"), value_type=int
|
||||
),
|
||||
"image_height": ParameterValue(
|
||||
LaunchConfiguration("image_height"), value_type=int
|
||||
),
|
||||
"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": ParameterValue(
|
||||
LaunchConfiguration("auto_exposure"), value_type=bool
|
||||
),
|
||||
"camera_info_url": LaunchConfiguration("camera_info_url"),
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
vision_components = []
|
||||
if use_roi:
|
||||
processed_image_raw_topic = "/g20_thumb_roi/image_raw"
|
||||
processed_camera_info_topic = "/g20_thumb_roi/camera_info"
|
||||
processed_image_rect_topic = "/g20_thumb_roi/image_rect"
|
||||
vision_components.append(
|
||||
ComposableNode(
|
||||
package="image_proc",
|
||||
plugin="image_proc::CropDecimateNode",
|
||||
name="crop_color_roi",
|
||||
namespace="/g20_thumb_roi",
|
||||
remappings=[
|
||||
("in/image_raw", "/camera/camera/color/image_raw"),
|
||||
("in/camera_info", "/camera/camera/color/camera_info"),
|
||||
("out/image_raw", processed_image_raw_topic),
|
||||
("out/camera_info", processed_camera_info_topic),
|
||||
],
|
||||
parameters=[
|
||||
{
|
||||
"queue_size": 5,
|
||||
"decimation_x": 1,
|
||||
"decimation_y": 1,
|
||||
"offset_x": roi_values["roi_x"],
|
||||
"offset_y": roi_values["roi_y"],
|
||||
"width": roi_values["roi_width"],
|
||||
"height": roi_values["roi_height"],
|
||||
}
|
||||
],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
)
|
||||
)
|
||||
rectifier_namespace = "/g20_thumb_roi"
|
||||
rectifier_name = "rectify_color_roi"
|
||||
else:
|
||||
processed_image_raw_topic = "/camera/camera/color/image_raw"
|
||||
processed_camera_info_topic = "/camera/camera/color/camera_info"
|
||||
processed_image_rect_topic = "/camera/camera/color/image_rect"
|
||||
rectifier_namespace = "/camera/camera/color"
|
||||
rectifier_name = "rectify_color"
|
||||
|
||||
vision_components.append(
|
||||
ComposableNode(
|
||||
package="image_proc",
|
||||
plugin="image_proc::RectifyNode",
|
||||
name=rectifier_name,
|
||||
namespace=rectifier_namespace,
|
||||
remappings=[
|
||||
("image", processed_image_raw_topic),
|
||||
("camera_info", processed_camera_info_topic),
|
||||
("image_rect", processed_image_rect_topic),
|
||||
],
|
||||
parameters=[{"queue_size": 1}],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
)
|
||||
)
|
||||
|
||||
vision_components.append(
|
||||
ComposableNode(
|
||||
package="apriltag_ros",
|
||||
plugin="AprilTagNode",
|
||||
name="apriltag",
|
||||
namespace="/apriltag",
|
||||
parameters=[
|
||||
tag_config,
|
||||
{
|
||||
"detector.decimate": ParameterValue(
|
||||
LaunchConfiguration("apriltag_decimate"),
|
||||
value_type=float,
|
||||
)
|
||||
},
|
||||
],
|
||||
remappings=[
|
||||
("image_rect", processed_image_rect_topic),
|
||||
("camera_info", processed_camera_info_topic),
|
||||
],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
)
|
||||
)
|
||||
|
||||
vision_container = ComposableNodeContainer(
|
||||
name="g20_thumb_vision_container",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
composable_node_descriptions=vision_components,
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
)
|
||||
|
||||
sdk = Node(
|
||||
package="linker_hand_ros2_sdk",
|
||||
executable="linker_hand_sdk",
|
||||
name="linker_hand_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": "left",
|
||||
"hand_joint": "G20",
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": "/g20",
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"startup_torque": 80,
|
||||
"state_poll_rate": 10.0,
|
||||
"repeat_position_commands": False,
|
||||
"is_touch": False,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
calibration = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="calibration_node",
|
||||
name="g20_thumb_calibration",
|
||||
output="screen",
|
||||
parameters=[
|
||||
calibration_config,
|
||||
tag_config,
|
||||
{
|
||||
"serial_number": serial_number,
|
||||
"session_dir": str(session_dir),
|
||||
"commands_enabled": LaunchConfiguration("commands_enabled"),
|
||||
"calibration_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"continuous_motion_mode": ParameterValue(
|
||||
LaunchConfiguration("continuous_motion_mode"),
|
||||
value_type=str,
|
||||
),
|
||||
"angle_estimation_mode": ParameterValue(
|
||||
LaunchConfiguration("angle_estimation_mode"),
|
||||
value_type=str,
|
||||
),
|
||||
"camera_serial_number": LaunchConfiguration("camera_serial_number"),
|
||||
"rosbag_path": str(bag_path),
|
||||
"camera_info_topic": processed_camera_info_topic,
|
||||
"image_topic": processed_image_rect_topic,
|
||||
"publish_debug_image": LaunchConfiguration(
|
||||
"publish_debug_image"
|
||||
),
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
bag = ExecuteProcess(
|
||||
condition=IfCondition(LaunchConfiguration("record_bag")),
|
||||
cmd=[
|
||||
"ros2",
|
||||
"bag",
|
||||
"record",
|
||||
"--storage",
|
||||
"mcap",
|
||||
"--storage-preset-profile",
|
||||
"zstd_fast",
|
||||
"--max-bag-size",
|
||||
"10737418240",
|
||||
"--output",
|
||||
str(bag_path),
|
||||
processed_image_raw_topic,
|
||||
processed_camera_info_topic,
|
||||
"/apriltag/detections",
|
||||
"/tf",
|
||||
"/g20/cb_left_hand_control_cmd",
|
||||
"/g20/cb_left_hand_state",
|
||||
"/g20/cb_left_hand_info",
|
||||
"/g20_thumb_calibration/status",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
actions = [
|
||||
LogInfo(msg=f"G20 thumb calibration session: {session_dir}"),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"G20 thumb image ROI: "
|
||||
f"x={roi_values['roi_x']}, y={roi_values['roi_y']}, "
|
||||
f"width={roi_values['roi_width']}, "
|
||||
f"height={roi_values['roi_height']}"
|
||||
if use_roi
|
||||
else "G20 thumb image ROI: disabled"
|
||||
)
|
||||
),
|
||||
camera,
|
||||
vision_container,
|
||||
]
|
||||
actions.extend([sdk, calibration, bag])
|
||||
return actions
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
default_output = str(Path.cwd() / "calibration_output")
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_serial_number", default_value="DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_model", default_value="MV-CS020-10UM"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_name", default_value="hikrobot_front_DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_frame_id", default_value="camera_color_optical_frame"
|
||||
),
|
||||
DeclareLaunchArgument("image_width", default_value="1624"),
|
||||
DeclareLaunchArgument("image_height", default_value="1240"),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_info_url",
|
||||
default_value=str(
|
||||
Path.home()
|
||||
/ ".ros"
|
||||
/ "camera_info"
|
||||
/ "hikrobot_DB2163742.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"publish_debug_image", default_value="false"
|
||||
),
|
||||
DeclareLaunchArgument("use_roi", default_value="false"),
|
||||
DeclareLaunchArgument("roi_x", default_value="128"),
|
||||
DeclareLaunchArgument("roi_y", default_value="192"),
|
||||
DeclareLaunchArgument("roi_width", default_value="1024"),
|
||||
DeclareLaunchArgument("roi_height", default_value="528"),
|
||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||
DeclareLaunchArgument(
|
||||
"continuous_motion_mode", default_value="endpoint"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"angle_estimation_mode",
|
||||
default_value="trajectory_center_3d",
|
||||
),
|
||||
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
|
||||
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
||||
DeclareLaunchArgument("start_camera", default_value="true"),
|
||||
DeclareLaunchArgument("start_sdk", default_value="true"),
|
||||
DeclareLaunchArgument("record_bag", default_value="false"),
|
||||
DeclareLaunchArgument("output_root", default_value=default_output),
|
||||
DeclareLaunchArgument("session_dir", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config",
|
||||
default_value=str(package_share / "config" / "calibration.yaml"),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(package_share / "config" / "front_tags.yaml"),
|
||||
),
|
||||
OpaqueFunction(function=_launch_stack),
|
||||
]
|
||||
)
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/g20_thumb_apriltag_calibration
|
||||
[install]
|
||||
install_scripts=$base/lib/g20_thumb_apriltag_calibration
|
||||
@@ -1,69 +0,0 @@
|
||||
from glob import glob
|
||||
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
|
||||
package_name = "g20_thumb_apriltag_calibration"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=find_packages(),
|
||||
data_files=[
|
||||
(
|
||||
"share/ament_index/resource_index/packages",
|
||||
["resource/" + package_name],
|
||||
),
|
||||
("share/" + package_name, ["package.xml", "README.md"]),
|
||||
(
|
||||
"share/" + package_name + "/config",
|
||||
glob("config/*.yaml") + glob("config/*.xml"),
|
||||
),
|
||||
("share/" + package_name + "/launch", glob("launch/*.launch.py")),
|
||||
],
|
||||
install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
|
||||
tests_require=["pytest"],
|
||||
zip_safe=True,
|
||||
maintainer="lxp",
|
||||
maintainer_email="support@linker-robotics.com",
|
||||
description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand",
|
||||
license="MIT",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
(
|
||||
"hikrobot_camera_node = "
|
||||
"g20_thumb_apriltag_calibration.hikrobot_camera:main"
|
||||
),
|
||||
"calibration_node = g20_thumb_apriltag_calibration.node:main",
|
||||
(
|
||||
"cmc_pitch_zero_node = "
|
||||
"g20_thumb_apriltag_calibration.zero_node:main"
|
||||
),
|
||||
(
|
||||
"cmc_roll_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.zero_node:main"
|
||||
),
|
||||
(
|
||||
"three_camera_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.three_camera_node:main"
|
||||
),
|
||||
(
|
||||
"three_camera_extrinsics_node = "
|
||||
"g20_thumb_apriltag_calibration.extrinsics_node:main"
|
||||
),
|
||||
(
|
||||
"offline_replay = "
|
||||
"g20_thumb_apriltag_calibration.offline_replay:main"
|
||||
),
|
||||
(
|
||||
"camera_alignment_view = "
|
||||
"g20_thumb_apriltag_calibration.alignment_view:main"
|
||||
),
|
||||
(
|
||||
"calibrated_joint_state_bridge = "
|
||||
"g20_thumb_apriltag_calibration."
|
||||
"calibrated_joint_state_bridge:main"
|
||||
),
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -1,87 +0,0 @@
|
||||
import copy
|
||||
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
|
||||
G20_COMMAND_NAMES,
|
||||
G20_URDF_JOINT_NAMES,
|
||||
CalibratedCommandMapper,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
def _payload(side: str = "right") -> dict:
|
||||
profile = get_hand_calibration_profile(side)
|
||||
curve = [round((255 - command) * 0.001, 8) for command in range(256)]
|
||||
joints = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
joint = {
|
||||
"motor_index": spec.motor_index,
|
||||
"angle_rad": list(curve),
|
||||
}
|
||||
if spec.active:
|
||||
joint["zero_command_u8"] = 255
|
||||
joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0}
|
||||
else:
|
||||
joint["passive"] = True
|
||||
if spec.source_joint is not None:
|
||||
joint["source_joint"] = spec.source_joint
|
||||
joints[name] = joint
|
||||
return {
|
||||
"schema_version": 4,
|
||||
"model": "G20",
|
||||
"side": side,
|
||||
"serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT",
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": [255] * 20,
|
||||
"joints": joints,
|
||||
"quality": {
|
||||
"passed": True,
|
||||
"validation_mae_rad": 0.01,
|
||||
"validation_p95_rad": 0.02,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
command = list(range(20))
|
||||
result = dict(
|
||||
zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))
|
||||
)
|
||||
|
||||
assert result["thumb_cmc_pitch"] == pytest.approx(0.255)
|
||||
assert result["thumb_cmc_roll"] == pytest.approx(0.250)
|
||||
assert result["thumb_cmc_yaw"] == pytest.approx(0.245)
|
||||
assert result["thumb_mcp"] == pytest.approx(0.240)
|
||||
assert result["thumb_ip"] == pytest.approx(0.240)
|
||||
assert result["pinky_pip"] == pytest.approx(0.236)
|
||||
assert result["pinky_dip"] == pytest.approx(0.236)
|
||||
|
||||
|
||||
def test_mapper_uses_names_instead_of_message_order() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
command = list(range(20))
|
||||
names = list(reversed(G20_COMMAND_NAMES))
|
||||
positions = list(reversed(command))
|
||||
|
||||
assert mapper.map_positions(positions, names) == mapper.map_positions(command)
|
||||
|
||||
|
||||
def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None:
|
||||
with pytest.raises(ValueError, match="does not match"):
|
||||
CalibratedCommandMapper(_payload("left"), expected_side="right")
|
||||
|
||||
payload = copy.deepcopy(_payload())
|
||||
payload["quality"]["passed"] = False
|
||||
with pytest.raises(ValueError, match="quality.passed"):
|
||||
CalibratedCommandMapper(payload, expected_side="right")
|
||||
|
||||
|
||||
def test_mapper_rejects_incomplete_named_command() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
with pytest.raises(ValueError, match="missing named channels"):
|
||||
mapper.map_positions([255.0], ["thumb_cmc_pitch"])
|
||||
@@ -1,500 +0,0 @@
|
||||
import math
|
||||
from dataclasses import replace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
ACTIVE_JOINTS,
|
||||
IMAGE_TRAJECTORY_JOINTS,
|
||||
JOINT_SPECS,
|
||||
LEFT_HAND_PROFILE,
|
||||
MEASURED_JOINTS,
|
||||
PASSIVE_JOINTS,
|
||||
RIGHT_HAND_PROFILE,
|
||||
SPLAY_JOINTS,
|
||||
SWEEP_SPECS,
|
||||
VIEW_TAGS,
|
||||
build_calibration_motion_command,
|
||||
build_calibration_speed_profile,
|
||||
build_compact_payload,
|
||||
build_full_hand_command,
|
||||
center_splay_curve,
|
||||
fit_joint_center_curve,
|
||||
fit_joint_image_curve,
|
||||
fit_measured_joint_curve,
|
||||
fit_projected_zero,
|
||||
get_hand_calibration_profile,
|
||||
measure_joint_observation,
|
||||
validate_compact_payload,
|
||||
)
|
||||
|
||||
|
||||
def _records() -> list[dict[str, object]]:
|
||||
commands = list(range(0, 256, 16))
|
||||
if commands[-1] != 255:
|
||||
commands.append(255)
|
||||
records: list[dict[str, object]] = []
|
||||
centre = np.asarray([0.006, -0.004, 0.012])
|
||||
radius = 0.025
|
||||
image_centre = np.asarray([30.0, -12.0])
|
||||
image_radius = 100.0
|
||||
for cycle in range(3):
|
||||
for direction, sequence in (
|
||||
("decreasing", reversed(commands)),
|
||||
("increasing", commands),
|
||||
):
|
||||
for command in sequence:
|
||||
angle = 0.70 * (255.0 - command) / 255.0
|
||||
point = centre + np.asarray(
|
||||
[radius * math.cos(angle), radius * math.sin(angle), 0.0]
|
||||
)
|
||||
# At command 255 the inward vector points along image +x, so
|
||||
# table_projected_zero_rad is exactly zero.
|
||||
image_point = image_centre + np.asarray(
|
||||
[
|
||||
-image_radius * math.cos(angle),
|
||||
image_radius * math.sin(angle),
|
||||
]
|
||||
)
|
||||
records.append(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_translation_xyz_m": point.tolist(),
|
||||
"image_relative_xy_px": image_point.tolist(),
|
||||
}
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def test_joint_layout_covers_16_active_and_5_passive_joints() -> None:
|
||||
assert len(JOINT_SPECS) == 21
|
||||
assert len(ACTIVE_JOINTS) == 16
|
||||
assert len(PASSIVE_JOINTS) == 5
|
||||
|
||||
|
||||
def test_right_profile_measures_pinky_and_inherits_to_other_fingers() -> None:
|
||||
profile = get_hand_calibration_profile("right")
|
||||
assert profile is RIGHT_HAND_PROFILE
|
||||
assert profile.reference_finger == "pinky"
|
||||
assert [spec.motor_index for spec in profile.sweep_specs] == [
|
||||
0, 5, 15, 9, 4, 19, 10
|
||||
]
|
||||
assert profile.view_tags["front"]["pinky_roll"] == 10
|
||||
assert profile.view_tags["side"] == {
|
||||
"side_base": 4,
|
||||
"pinky_mcp": 5,
|
||||
"pinky_pip": 6,
|
||||
"pinky_dip": 7,
|
||||
}
|
||||
assert profile.preflight_view_roles["side"] == (
|
||||
"side_base",
|
||||
"pinky_mcp",
|
||||
"pinky_pip",
|
||||
"pinky_dip",
|
||||
)
|
||||
thumb_pitch = profile.joint_specs["thumb_cmc_pitch"]
|
||||
assert thumb_pitch.view == "front"
|
||||
assert thumb_pitch.parent_role == "front_base"
|
||||
assert thumb_pitch.child_role == "thumb_cmc"
|
||||
assert profile.joint_specs["index_mcp_roll"].source_joint == (
|
||||
"pinky_mcp_roll"
|
||||
)
|
||||
assert profile.joint_specs["middle_mcp_pitch"].source_joint == (
|
||||
"pinky_mcp_pitch"
|
||||
)
|
||||
assert profile.joint_specs["ring_pip"].source_joint == "pinky_pip"
|
||||
assert profile.joint_specs["index_dip"].source_joint == "pinky_dip"
|
||||
|
||||
roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 9)
|
||||
command = build_calibration_motion_command(
|
||||
roll, 127, profile=profile
|
||||
)
|
||||
assert command[9] == 127
|
||||
assert command[6:9] == [255, 255, 255]
|
||||
speeds = build_calibration_speed_profile(
|
||||
roll,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
profile=profile,
|
||||
)
|
||||
assert speeds == [15, 15, 15, 15, 5]
|
||||
assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set(
|
||||
range(11)
|
||||
)
|
||||
assert VIEW_TAGS["front"]["index_roll"] == 10
|
||||
assert VIEW_TAGS["side"] == {
|
||||
"side_base": 4,
|
||||
"index_mcp": 5,
|
||||
"index_pip": 6,
|
||||
"index_dip": 7,
|
||||
}
|
||||
assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9}
|
||||
assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10]
|
||||
|
||||
|
||||
def test_joint_trajectory_spaces_match_observation_geometry() -> None:
|
||||
assert IMAGE_TRAJECTORY_JOINTS == {
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
"index_mcp_roll",
|
||||
"index_mcp_pitch",
|
||||
"index_pip",
|
||||
}
|
||||
records = _records()
|
||||
for name in IMAGE_TRAJECTORY_JOINTS:
|
||||
fit = fit_measured_joint_curve(name, records)
|
||||
assert fit.circle["space"] == "image_2d"
|
||||
for name in ("index_dip", "thumb_cmc_yaw"):
|
||||
assert "space" not in fit_measured_joint_curve(name, records).circle
|
||||
|
||||
|
||||
def test_full_hand_command_changes_exactly_one_controlled_motor() -> None:
|
||||
result = build_full_hand_command(6, 27)
|
||||
assert result[6] == 27
|
||||
assert result[:6] == [255] * 6
|
||||
with pytest.raises(ValueError, match="controlled"):
|
||||
build_full_hand_command(11, 27)
|
||||
|
||||
|
||||
def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
|
||||
result = build_calibration_motion_command(index_roll, 27, baseline)
|
||||
|
||||
assert result[6:10] == [27, 0, 0, 0]
|
||||
assert result[:6] == baseline[:6]
|
||||
assert result[10:] == baseline[10:]
|
||||
|
||||
|
||||
def test_right_pinky_roll_moves_other_three_fingers_camera_right() -> None:
|
||||
profile = RIGHT_HAND_PROFILE
|
||||
pinky_roll = next(
|
||||
spec for spec in profile.sweep_specs if spec.motor_index == 9
|
||||
)
|
||||
|
||||
result = build_calibration_motion_command(
|
||||
pinky_roll, 27, profile=profile
|
||||
)
|
||||
|
||||
assert result[6:10] == [255, 255, 255, 27]
|
||||
assert profile.roll_clearance_commands == {6: 255, 7: 255, 8: 255}
|
||||
|
||||
|
||||
def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
|
||||
|
||||
result = build_calibration_motion_command(thumb_pitch, 17, baseline)
|
||||
|
||||
assert result[0] == 17
|
||||
assert result[6:10] == [127, 127, 127, 127]
|
||||
|
||||
|
||||
def test_right_thumb_pitch_uses_front_visible_yaw_and_roll_pose() -> None:
|
||||
profile = RIGHT_HAND_PROFILE
|
||||
thumb_pitch = next(
|
||||
spec for spec in profile.sweep_specs if spec.motor_index == 0
|
||||
)
|
||||
|
||||
result = build_calibration_motion_command(
|
||||
thumb_pitch, 17, profile=profile
|
||||
)
|
||||
|
||||
assert result[0] == 17
|
||||
assert result[10] == 255
|
||||
assert result[5] == 255
|
||||
assert profile.thumb_pitch_clearance_commands == {10: 255, 5: 255}
|
||||
|
||||
|
||||
def test_left_thumb_pitch_keeps_legacy_baseline_pose() -> None:
|
||||
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
|
||||
|
||||
result = build_calibration_motion_command(thumb_pitch, 17)
|
||||
|
||||
assert result[0] == 17
|
||||
assert result[5] == 255
|
||||
assert result[10] == 255
|
||||
assert LEFT_HAND_PROFILE.thumb_pitch_clearance_commands == {}
|
||||
|
||||
|
||||
def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10)
|
||||
|
||||
result = build_calibration_motion_command(thumb_yaw, 27, baseline)
|
||||
|
||||
assert result[5] == 145
|
||||
assert result[10] == 27
|
||||
assert result[:5] == baseline[:5]
|
||||
assert result[6:10] == baseline[6:10]
|
||||
assert result[11:] == baseline[11:]
|
||||
|
||||
|
||||
def test_only_index_roll_uses_the_slow_index_finger_speed() -> None:
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1)
|
||||
|
||||
assert build_calibration_speed_profile(
|
||||
index_roll,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 5, 15, 15, 15]
|
||||
assert build_calibration_speed_profile(
|
||||
index_pitch,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 10, 15, 15, 15]
|
||||
|
||||
index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16)
|
||||
assert build_calibration_speed_profile(
|
||||
index_pip,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 10, 15, 15, 15]
|
||||
|
||||
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
|
||||
assert build_calibration_speed_profile(
|
||||
thumb_pitch,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 15, 15, 15, 15]
|
||||
|
||||
|
||||
def test_calibration_speed_profile_rejects_out_of_range_values() -> None:
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
with pytest.raises(ValueError, match="speeds"):
|
||||
build_calibration_speed_profile(
|
||||
index_roll,
|
||||
normal_speed=15,
|
||||
index_roll_speed=256,
|
||||
index_flex_speed=10,
|
||||
)
|
||||
|
||||
|
||||
def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None:
|
||||
records = _records()
|
||||
fit = fit_joint_center_curve(records)
|
||||
assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8)
|
||||
assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4)
|
||||
assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8)
|
||||
assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6)
|
||||
|
||||
|
||||
def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None:
|
||||
records = _records()
|
||||
for record in records:
|
||||
command = int(record["command_u8"])
|
||||
record["relative_translation_xyz_m"] = [
|
||||
0.001 * command,
|
||||
0.0,
|
||||
0.0,
|
||||
]
|
||||
|
||||
fit = fit_measured_joint_curve("thumb_mcp", records)
|
||||
|
||||
assert fit.circle["space"] == "image_2d"
|
||||
assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02)
|
||||
assert fit.angle_rad[255] == pytest.approx(0.0)
|
||||
assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01
|
||||
assert math.degrees(fit.maximum_hysteresis_rad) < 0.01
|
||||
|
||||
command_zero = next(
|
||||
record for record in records if int(record["command_u8"]) == 0
|
||||
)
|
||||
observed = measure_joint_observation(
|
||||
fit,
|
||||
vector_xyz_m=command_zero["relative_translation_xyz_m"],
|
||||
image_vector_xy_px=command_zero["image_relative_xy_px"],
|
||||
)
|
||||
assert observed == pytest.approx(0.70, abs=0.02)
|
||||
|
||||
|
||||
def test_image_curve_rejects_insufficient_projected_arc() -> None:
|
||||
records = _records()
|
||||
for record in records:
|
||||
command = float(record["command_u8"])
|
||||
angle = math.radians(2.0) * (255.0 - command) / 255.0
|
||||
record["image_relative_xy_px"] = [
|
||||
100.0 * math.cos(angle),
|
||||
100.0 * math.sin(angle),
|
||||
]
|
||||
|
||||
with pytest.raises(
|
||||
ValueError, match="joint_image_trajectory_quality_failed:arc"
|
||||
):
|
||||
fit_joint_image_curve(records)
|
||||
|
||||
|
||||
def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None:
|
||||
fit = fit_joint_center_curve(_records())
|
||||
centred, zero_command, midpoint = center_splay_curve(fit)
|
||||
assert midpoint == pytest.approx(0.35, abs=1.0e-4)
|
||||
assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4)
|
||||
assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4)
|
||||
assert zero_command in {127, 128}
|
||||
assert abs(centred.angle_rad[zero_command]) <= 0.002
|
||||
|
||||
|
||||
def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, _ = center_splay_curve(base)
|
||||
splay = replace(
|
||||
splay,
|
||||
angle_rad=tuple(
|
||||
value - splay.angle_rad[zero_command]
|
||||
for value in splay.angle_rad
|
||||
),
|
||||
)
|
||||
measured = {
|
||||
name: splay if name == "index_mcp_roll" else base
|
||||
for name in MEASURED_JOINTS
|
||||
}
|
||||
offsets = {name: 0.01 for name in ACTIVE_JOINTS}
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [zero_command] * 4
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_LEFT_001",
|
||||
measured_fits=measured,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[0.01, -0.02],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
)
|
||||
validate_compact_payload(payload)
|
||||
assert set(payload) == {
|
||||
"schema_version",
|
||||
"model",
|
||||
"side",
|
||||
"serial_number",
|
||||
"angle_unit",
|
||||
"command_range",
|
||||
"baseline_command_u8",
|
||||
"joints",
|
||||
"quality",
|
||||
}
|
||||
assert len(payload["joints"]) == 21
|
||||
assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values())
|
||||
assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16
|
||||
assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5
|
||||
assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll"
|
||||
assert (
|
||||
payload["joints"]["middle_mcp_roll"]["angle_rad"]
|
||||
== payload["joints"]["index_mcp_roll"]["angle_rad"]
|
||||
)
|
||||
assert payload["joints"]["index_mcp_roll"]["zero_angles"] == {
|
||||
"urdf_zero_offset_rad": pytest.approx(0.01)
|
||||
}
|
||||
for name in SPLAY_JOINTS:
|
||||
assert payload["joints"][name]["zero_command_u8"] == zero_command
|
||||
|
||||
|
||||
def test_compact_payload_allows_skipped_random_validation() -> None:
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, _ = center_splay_curve(base)
|
||||
splay = replace(
|
||||
splay,
|
||||
angle_rad=tuple(
|
||||
value - splay.angle_rad[zero_command]
|
||||
for value in splay.angle_rad
|
||||
),
|
||||
)
|
||||
measured = {
|
||||
name: splay if name == "index_mcp_roll" else base
|
||||
for name in MEASURED_JOINTS
|
||||
}
|
||||
offsets = {name: 0.0 for name in ACTIVE_JOINTS}
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [zero_command] * 4
|
||||
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_LEFT_001",
|
||||
measured_fits=measured,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
)
|
||||
|
||||
validate_compact_payload(payload)
|
||||
assert payload["quality"] == {
|
||||
"passed": True,
|
||||
"validation_mae_rad": None,
|
||||
"validation_p95_rad": None,
|
||||
}
|
||||
|
||||
|
||||
def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
|
||||
profile = RIGHT_HAND_PROFILE
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, _ = center_splay_curve(base)
|
||||
splay = replace(
|
||||
splay,
|
||||
angle_rad=tuple(
|
||||
value - splay.angle_rad[zero_command]
|
||||
for value in splay.angle_rad
|
||||
),
|
||||
)
|
||||
measured = {
|
||||
name: splay if name == "pinky_mcp_roll" else base
|
||||
for name in profile.measured_joints
|
||||
}
|
||||
offsets = {
|
||||
name: (0.01 if profile.joint_specs[name].source_joint is None else 0.0)
|
||||
for name in profile.active_joints
|
||||
}
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [zero_command] * 4
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_RIGHT_001",
|
||||
measured_fits=measured,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[0.01],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
side="right",
|
||||
)
|
||||
|
||||
validate_compact_payload(payload)
|
||||
assert payload["schema_version"] == 4
|
||||
assert payload["side"] == "right"
|
||||
assert set(payload) == {
|
||||
"schema_version",
|
||||
"model",
|
||||
"side",
|
||||
"serial_number",
|
||||
"angle_unit",
|
||||
"command_range",
|
||||
"baseline_command_u8",
|
||||
"joints",
|
||||
"quality",
|
||||
}
|
||||
for finger in ("index", "middle", "ring"):
|
||||
assert payload["joints"][f"{finger}_mcp_roll"]["source_joint"] == (
|
||||
"pinky_mcp_roll"
|
||||
)
|
||||
assert payload["joints"][f"{finger}_mcp_pitch"]["source_joint"] == (
|
||||
"pinky_mcp_pitch"
|
||||
)
|
||||
assert payload["joints"][f"{finger}_pip"]["source_joint"] == (
|
||||
"pinky_pip"
|
||||
)
|
||||
for suffix in ("mcp_roll", "mcp_pitch", "pip"):
|
||||
joint = payload["joints"][f"{finger}_{suffix}"]
|
||||
source = payload["joints"][joint["source_joint"]]
|
||||
assert joint["angle_rad"] == source["angle_rad"]
|
||||
assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0}
|
||||
assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01}
|
||||
@@ -1,44 +0,0 @@
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.offline_replay import (
|
||||
_latest_attempt_records,
|
||||
_output_suffix,
|
||||
)
|
||||
|
||||
|
||||
def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict:
|
||||
return {
|
||||
"kind": "sample",
|
||||
"joint": joint,
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"attempt": attempt,
|
||||
}
|
||||
|
||||
|
||||
def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
|
||||
rows = [
|
||||
{"kind": "session_start"},
|
||||
_sample("pinky_pip", 0, "decreasing", 1),
|
||||
_sample("pinky_pip", 0, "decreasing", 3),
|
||||
_sample("pinky_pip", 0, "increasing", 1),
|
||||
_sample("pinky_pip", 1, "decreasing", 2),
|
||||
_sample("thumb_cmc_yaw", 0, "decreasing", 1),
|
||||
]
|
||||
|
||||
selected = _latest_attempt_records(rows)
|
||||
|
||||
assert [
|
||||
record["attempt"] for record in selected["pinky_pip"]
|
||||
] == [3, 1, 2]
|
||||
assert [
|
||||
record["attempt"] for record in selected["thumb_cmc_yaw"]
|
||||
] == [1]
|
||||
|
||||
|
||||
def test_output_suffix_is_safe_and_explicit() -> None:
|
||||
assert _output_suffix(None) == ""
|
||||
assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
|
||||
for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
|
||||
with pytest.raises(ValueError, match="output tag"):
|
||||
_output_suffix(invalid)
|
||||
@@ -1,35 +0,0 @@
|
||||
from g20_thumb_apriltag_calibration.storage import (
|
||||
append_jsonl,
|
||||
atomic_write_json,
|
||||
completed_scan_keys,
|
||||
load_json,
|
||||
load_jsonl,
|
||||
)
|
||||
|
||||
|
||||
def test_jsonl_checkpoint_and_resume_keys(tmp_path) -> None:
|
||||
raw_path = tmp_path / "raw_samples.jsonl"
|
||||
record = {
|
||||
"kind": "sample",
|
||||
"phase": "root",
|
||||
"cycle": 0,
|
||||
"direction": "decreasing",
|
||||
"command_u8": 255,
|
||||
}
|
||||
append_jsonl(raw_path, record)
|
||||
append_jsonl(raw_path, {"kind": "validation", "command_u8": 10})
|
||||
loaded = load_jsonl(raw_path)
|
||||
assert loaded[0] == record
|
||||
assert completed_scan_keys(loaded) == {("root", 0, "decreasing", 255)}
|
||||
|
||||
checkpoint_path = tmp_path / "checkpoint.json"
|
||||
atomic_write_json(checkpoint_path, {"state": "PAUSED", "records": 1})
|
||||
assert load_json(checkpoint_path) == {"state": "PAUSED", "records": 1}
|
||||
|
||||
|
||||
def test_resume_ignores_only_a_truncated_final_jsonl_record(tmp_path) -> None:
|
||||
raw_path = tmp_path / "raw_samples.jsonl"
|
||||
append_jsonl(raw_path, {"kind": "sample", "phase": "root"})
|
||||
with raw_path.open("a", encoding="utf-8") as stream:
|
||||
stream.write('{"kind":"sample"\n\n')
|
||||
assert load_jsonl(raw_path) == [{"kind": "sample", "phase": "root"}]
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,977 +0,0 @@
|
||||
import math
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from g20_thumb_apriltag_calibration.extrinsics import (
|
||||
camera_info_fingerprint,
|
||||
dump_three_camera_extrinsics,
|
||||
load_three_camera_extrinsics,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
AXIS_JOINTS,
|
||||
DIRECT_ZERO_JOINTS,
|
||||
INHERITED_ZERO_JOINTS,
|
||||
JointAxisMeasurement,
|
||||
UrdfKinematicModel,
|
||||
_angles_from_state,
|
||||
_zero_sensitive_axis_error_rad,
|
||||
fit_joint_axis_measurement,
|
||||
fit_rotation_joint_curve,
|
||||
solve_urdf_zero_offsets,
|
||||
get_zero_calibration_profile,
|
||||
write_zero_corrected_urdf,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
ACTIVE_JOINTS,
|
||||
JOINT_SPECS,
|
||||
MEASURED_JOINTS,
|
||||
PASSIVE_JOINTS,
|
||||
JointCurveFit,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
REPOSITORY = Path(__file__).resolve().parents[3]
|
||||
SOURCE_URDF = REPOSITORY / (
|
||||
"src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
|
||||
"linker_hand/g20_left/linkerhand_g20_left.urdf"
|
||||
)
|
||||
RIGHT_SOURCE_URDF = REPOSITORY / (
|
||||
"src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
|
||||
"linker_hand/g20_right/linkerhand_g20_right.urdf"
|
||||
)
|
||||
|
||||
|
||||
def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
|
||||
parent = np.asarray([0.0, 0.0, 1.0])
|
||||
predicted = np.asarray([1.0, 0.0, 0.0])
|
||||
cone_mismatch = np.asarray(
|
||||
[math.cos(math.radians(10.0)), 0.0, math.sin(math.radians(10.0))]
|
||||
)
|
||||
zero_mismatch = np.asarray(
|
||||
[math.cos(math.radians(3.0)), math.sin(math.radians(3.0)), 0.0]
|
||||
)
|
||||
|
||||
assert _zero_sensitive_axis_error_rad(
|
||||
predicted, cone_mismatch, parent
|
||||
) == pytest.approx(0.0, abs=1.0e-12)
|
||||
assert math.degrees(
|
||||
_zero_sensitive_axis_error_rad(predicted, zero_mismatch, parent)
|
||||
) == pytest.approx(3.0, abs=1.0e-9)
|
||||
|
||||
|
||||
def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None:
|
||||
parent = np.asarray([0.0, 0.0, 1.0])
|
||||
cone = math.radians(32.0)
|
||||
phase = math.radians(7.0)
|
||||
predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)])
|
||||
observed = Rotation.from_rotvec(parent * phase).apply(predicted)
|
||||
|
||||
error = _zero_sensitive_axis_error_rad(predicted, observed, parent)
|
||||
|
||||
assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9)
|
||||
|
||||
|
||||
def _payload(transform: np.ndarray) -> dict[str, list[float]]:
|
||||
return {
|
||||
"translation_xyz_m": transform[:3, 3].tolist(),
|
||||
"quaternion_xyzw": Rotation.from_matrix(
|
||||
transform[:3, :3]
|
||||
).as_quat().tolist(),
|
||||
}
|
||||
|
||||
|
||||
def _arbitrary_tag_records() -> tuple[list[dict], np.ndarray, np.ndarray]:
|
||||
axis_parent = np.asarray([0.23, -0.31, 0.922], dtype=float)
|
||||
axis_parent /= np.linalg.norm(axis_parent)
|
||||
centre_parent = np.asarray([0.012, -0.008, 0.021])
|
||||
radial = np.cross(axis_parent, np.asarray([0.7, 0.1, -0.2]))
|
||||
radial = 0.035 * radial / np.linalg.norm(radial)
|
||||
child_tag_mount = Rotation.from_euler(
|
||||
"xyz", [1.1, -0.7, 0.45]
|
||||
)
|
||||
common_from_parent = np.eye(4)
|
||||
common_from_parent[:3, :3] = Rotation.from_euler(
|
||||
"xyz", [-0.8, 0.55, 1.3]
|
||||
).as_matrix()
|
||||
common_from_parent[:3, 3] = [0.41, -0.12, 0.73]
|
||||
expected_axis = common_from_parent[:3, :3] @ axis_parent
|
||||
expected_point = (
|
||||
common_from_parent[:3, :3] @ centre_parent
|
||||
+ common_from_parent[:3, 3]
|
||||
)
|
||||
|
||||
commands = list(range(0, 256, 16)) + [255]
|
||||
records = []
|
||||
for cycle in range(3):
|
||||
for direction in ("decreasing", "increasing"):
|
||||
for command in commands:
|
||||
angle = math.radians(62.0) * (255.0 - command) / 255.0
|
||||
motion = Rotation.from_rotvec(axis_parent * angle)
|
||||
relative_rotation = motion * child_tag_mount
|
||||
relative_translation = centre_parent + motion.apply(radial)
|
||||
child_common = common_from_parent.copy()
|
||||
child_common[:3, :3] = (
|
||||
common_from_parent[:3, :3]
|
||||
@ relative_rotation.as_matrix()
|
||||
)
|
||||
child_common[:3, 3] = (
|
||||
common_from_parent[:3, :3] @ relative_translation
|
||||
+ common_from_parent[:3, 3]
|
||||
)
|
||||
state = [255.0] * 20
|
||||
state[5] = float(command)
|
||||
records.append(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_translation_xyz_m": relative_translation.tolist(),
|
||||
"relative_quaternion_xyzw": relative_rotation.as_quat().tolist(),
|
||||
"parent_pose_common": _payload(common_from_parent),
|
||||
"child_pose_common": _payload(child_common),
|
||||
"state_u8": state,
|
||||
}
|
||||
)
|
||||
return records, expected_axis, expected_point
|
||||
|
||||
|
||||
def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
curve = fit_rotation_joint_curve(records, zero_command_u8=255)
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"thumb_cmc_roll", records, cycle=0, zero_command_u8=255
|
||||
)
|
||||
|
||||
observed_axis = np.asarray(measurement.axis_common_xyz)
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert float(observed_axis @ expected_axis) > math.cos(math.radians(0.05))
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 1.0e-6
|
||||
assert curve.angle_rad[255] == pytest.approx(0.0, abs=1.0e-9)
|
||||
assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"])
|
||||
def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth(
|
||||
joint: str,
|
||||
) -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
|
||||
axis_parent = parent_rotation.inv().apply(expected_axis)
|
||||
tangent = np.cross(axis_parent, np.asarray([0.4, -0.2, 0.7]))
|
||||
tangent /= np.linalg.norm(tangent)
|
||||
# Reproduce monocular planar-PnP depth bias: the centre trajectory remains
|
||||
# precise in its dominant directions but receives a command-correlated
|
||||
# component that makes a free 3-D plane normal substantially wrong.
|
||||
biased_records = []
|
||||
for record in records:
|
||||
biased = dict(record)
|
||||
point = np.asarray(record["relative_translation_xyz_m"], dtype=float)
|
||||
depth_bias = 0.30 * float(point @ tangent)
|
||||
biased["relative_translation_xyz_m"] = (
|
||||
point + depth_bias * axis_parent
|
||||
).tolist()
|
||||
biased_records.append(biased)
|
||||
|
||||
measurement = fit_joint_axis_measurement(
|
||||
joint, biased_records, cycle=0, zero_command_u8=255
|
||||
)
|
||||
|
||||
observed_axis = np.asarray(measurement.axis_common_xyz)
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert abs(float(observed_axis @ expected_axis)) > math.cos(
|
||||
math.radians(0.05)
|
||||
)
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 0.003
|
||||
assert measurement.rotation_circle_axis_difference_rad > math.radians(5.0)
|
||||
assert measurement.plane_rms_m < 0.003
|
||||
assert measurement.radial_rms_m < 0.003
|
||||
|
||||
|
||||
def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
|
||||
axis_parent = common_from_parent.inv().apply(expected_axis)
|
||||
# Exact end-on depth is a gauge along the physical axis and therefore
|
||||
# cannot alter the observable axis line. An oblique camera has a small
|
||||
# irreducible coupling between monocular depth and radial position; that
|
||||
# case must be bounded by the residual/holdout gates, not asserted to be
|
||||
# exactly recoverable from one view.
|
||||
view_normal_parent = axis_parent
|
||||
view_normal_common = common_from_parent.apply(view_normal_parent)
|
||||
biased = []
|
||||
for record in records:
|
||||
changed = dict(record)
|
||||
fraction = (255.0 - float(record["command_u8"])) / 255.0
|
||||
depth_bias = 0.03 * (fraction - 0.5)
|
||||
changed["relative_translation_xyz_m"] = (
|
||||
np.asarray(record["relative_translation_xyz_m"], dtype=float)
|
||||
+ depth_bias * view_normal_parent
|
||||
).tolist()
|
||||
biased.append(changed)
|
||||
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"thumb_cmc_pitch",
|
||||
biased,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
view_normal_common_xyz=view_normal_common,
|
||||
)
|
||||
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert measurement.axis_point_source == "pose_trajectory_image_plane"
|
||||
assert measurement.pose_axis_line_rms_m < 1.0e-6
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 1.0e-6
|
||||
|
||||
|
||||
def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None:
|
||||
records, expected_axis, _ = _arbitrary_tag_records()
|
||||
assert not any(record["command_u8"] == 127 for record in records)
|
||||
|
||||
curve = fit_rotation_joint_curve(records, zero_command_u8=127)
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"index_mcp_roll", records, cycle=0, zero_command_u8=127
|
||||
)
|
||||
|
||||
observed_axis = np.asarray(measurement.axis_common_xyz)
|
||||
assert abs(float(observed_axis @ expected_axis)) > math.cos(
|
||||
math.radians(0.05)
|
||||
)
|
||||
assert curve.angle_rad[127] == pytest.approx(0.0, abs=1.0e-9)
|
||||
|
||||
|
||||
def test_passive_axis_can_use_trusted_upstream_direction_constraint() -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
|
||||
physical_axis_parent = common_from_parent.inv().apply(expected_axis)
|
||||
wrong_axis_parent = np.cross(
|
||||
physical_axis_parent, np.asarray([0.2, 0.8, -0.1])
|
||||
)
|
||||
wrong_axis_parent /= np.linalg.norm(wrong_axis_parent)
|
||||
mount = Rotation.from_quat(records[0]["relative_quaternion_xyzw"])
|
||||
contradictory = []
|
||||
for record in records:
|
||||
changed = dict(record)
|
||||
angle = math.radians(62.0) * (
|
||||
255.0 - float(record["command_u8"])
|
||||
) / 255.0
|
||||
changed["relative_quaternion_xyzw"] = (
|
||||
Rotation.from_rotvec(wrong_axis_parent * angle) * mount
|
||||
).as_quat().tolist()
|
||||
contradictory.append(changed)
|
||||
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"index_dip",
|
||||
contradictory,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
axis_common_constraint=expected_axis,
|
||||
)
|
||||
|
||||
observed_axis = np.asarray(measurement.axis_common_xyz)
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert abs(float(observed_axis @ expected_axis)) > math.cos(
|
||||
math.radians(0.05)
|
||||
)
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 1.0e-6
|
||||
|
||||
|
||||
def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None:
|
||||
cameras = {
|
||||
view: {
|
||||
"serial_number": f"SERIAL_{view}",
|
||||
"width": 1624,
|
||||
"height": 1240,
|
||||
"intrinsics_sha256": camera_info_fingerprint(
|
||||
width=1624,
|
||||
height=1240,
|
||||
camera_matrix=np.asarray(
|
||||
[[1100.0, 0.0, 812.0], [0.0, 1099.0, 620.0], [0.0, 0.0, 1.0]]
|
||||
),
|
||||
),
|
||||
}
|
||||
for view in ("front", "side", "top")
|
||||
}
|
||||
transforms = {"front": np.eye(4), "side": np.eye(4), "top": np.eye(4)}
|
||||
transforms["side"][:3, :3] = Rotation.from_euler("y", 0.7).as_matrix()
|
||||
transforms["side"][:3, 3] = [0.2, 0.0, 0.1]
|
||||
transforms["top"][:3, :3] = Rotation.from_euler("x", -0.9).as_matrix()
|
||||
transforms["top"][:3, 3] = [-0.1, 0.3, 0.2]
|
||||
destination = tmp_path / "extrinsics.yaml"
|
||||
|
||||
dump_three_camera_extrinsics(
|
||||
destination,
|
||||
cameras=cameras,
|
||||
front_from_view=transforms,
|
||||
quality={
|
||||
"passed": True,
|
||||
"reprojection_rms_px": 0.3,
|
||||
"maximum_rotation_repeatability_deg": 0.2,
|
||||
"maximum_translation_repeatability_m": 0.001,
|
||||
"front_side_captures": 15,
|
||||
"front_top_captures": 15,
|
||||
},
|
||||
)
|
||||
loaded = load_three_camera_extrinsics(destination)
|
||||
|
||||
assert loaded.cameras["front"].serial_number == "SERIAL_front"
|
||||
assert np.allclose(loaded.transform("side"), transforms["side"])
|
||||
assert np.allclose(loaded.transform("top"), transforms["top"])
|
||||
assert loaded.camera_matches(
|
||||
"front",
|
||||
serial_number="SERIAL_front",
|
||||
width=1624,
|
||||
height=1240,
|
||||
intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
|
||||
)
|
||||
assert not loaded.camera_matches(
|
||||
"front",
|
||||
serial_number="WRONG_SERIAL",
|
||||
width=1624,
|
||||
height=1240,
|
||||
intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
|
||||
)
|
||||
|
||||
|
||||
def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]:
|
||||
joint = next(
|
||||
element
|
||||
for element in ET.parse(path).getroot().findall("joint")
|
||||
if element.get("name") == name
|
||||
)
|
||||
origin = joint.find("origin")
|
||||
axis = joint.find("axis")
|
||||
xyz = np.asarray([float(value) for value in origin.get("xyz").split()])
|
||||
rpy = np.asarray([float(value) for value in origin.get("rpy").split()])
|
||||
axis_xyz = np.asarray([float(value) for value in axis.get("xyz").split()])
|
||||
return np.block(
|
||||
[
|
||||
[Rotation.from_euler("xyz", rpy).as_matrix(), xyz[:, None]],
|
||||
[np.asarray([[0.0, 0.0, 0.0, 1.0]])],
|
||||
]
|
||||
), axis_xyz / np.linalg.norm(axis_xyz)
|
||||
|
||||
|
||||
def _joint_limit(path: Path, name: str) -> tuple[float, float]:
|
||||
joint = next(
|
||||
element
|
||||
for element in ET.parse(path).getroot().findall("joint")
|
||||
if element.get("name") == name
|
||||
)
|
||||
limit = joint.find("limit")
|
||||
return float(limit.get("lower")), float(limit.get("upper"))
|
||||
|
||||
|
||||
def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Path) -> None:
|
||||
offset = math.radians(7.3)
|
||||
destination = write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": offset},
|
||||
timestamp="20260806_120000",
|
||||
)
|
||||
original, axis = _joint_origin(SOURCE_URDF, "thumb_cmc_yaw")
|
||||
corrected, _ = _joint_origin(destination, "thumb_cmc_yaw")
|
||||
expected = original.copy()
|
||||
expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
|
||||
axis * offset
|
||||
).as_matrix()
|
||||
|
||||
assert destination != SOURCE_URDF
|
||||
assert np.allclose(corrected, expected, atol=1.0e-12)
|
||||
with pytest.raises(ValueError, match="refusing to overwrite"):
|
||||
write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": offset},
|
||||
timestamp="20260806_120000",
|
||||
)
|
||||
with pytest.raises(ValueError, match="original CAD URDF"):
|
||||
write_zero_corrected_urdf(
|
||||
source_urdf=destination,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": offset},
|
||||
timestamp="20260806_120001",
|
||||
)
|
||||
with pytest.raises(ValueError, match="finite and within"):
|
||||
write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": math.nan},
|
||||
timestamp="20260806_120002",
|
||||
)
|
||||
|
||||
|
||||
def test_urdf_writer_changes_only_the_16_active_zero_origins(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
before = SOURCE_URDF.read_bytes()
|
||||
offsets = {
|
||||
name: math.radians(0.25 * (index + 1))
|
||||
for index, name in enumerate(ACTIVE_JOINTS)
|
||||
}
|
||||
destination = write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad=offsets,
|
||||
timestamp="20260807_180000",
|
||||
)
|
||||
|
||||
assert len(offsets) == 16
|
||||
assert SOURCE_URDF.read_bytes() == before
|
||||
for name in ACTIVE_JOINTS:
|
||||
original, axis = _joint_origin(SOURCE_URDF, name)
|
||||
corrected, corrected_axis = _joint_origin(destination, name)
|
||||
expected = original.copy()
|
||||
expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
|
||||
axis * offsets[name]
|
||||
).as_matrix()
|
||||
assert np.allclose(corrected, expected, atol=1.0e-12)
|
||||
assert np.allclose(corrected_axis, axis, atol=1.0e-12)
|
||||
for name in PASSIVE_JOINTS:
|
||||
original, axis = _joint_origin(SOURCE_URDF, name)
|
||||
corrected, corrected_axis = _joint_origin(destination, name)
|
||||
assert np.allclose(corrected, original, atol=1.0e-12)
|
||||
assert np.allclose(corrected_axis, axis, atol=1.0e-12)
|
||||
# A zero calibration must not silently expand mechanical/CAD safety
|
||||
# limits. Dynamic measured ranges remain in the calibration JSON.
|
||||
for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS):
|
||||
assert _joint_limit(destination, name) == pytest.approx(
|
||||
_joint_limit(SOURCE_URDF, name)
|
||||
)
|
||||
|
||||
|
||||
def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit:
|
||||
values = np.asarray(
|
||||
[travel * (255.0 - command) / 255.0 for command in range(256)]
|
||||
)
|
||||
values -= values[zero_command]
|
||||
data = tuple(float(value) for value in values)
|
||||
return JointCurveFit(
|
||||
angle_rad=data,
|
||||
decreasing_rad=data,
|
||||
increasing_rad=data,
|
||||
circle={},
|
||||
maximum_monotonic_correction_rad=0.0,
|
||||
maximum_hysteresis_rad=0.0,
|
||||
quality={},
|
||||
)
|
||||
|
||||
|
||||
def _solve_synthetic_offsets(
|
||||
side: str,
|
||||
offset_degrees: list[float],
|
||||
*,
|
||||
inject_oblique_optical_depth_bias: bool = False,
|
||||
inject_secondary_root_axis_bias_degrees: float = 0.0,
|
||||
inject_secondary_root_point_bias_m: float = 0.0,
|
||||
inject_observer_cone_bias_degrees: float = 0.0,
|
||||
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
|
||||
joint_maximum_offset_degrees: dict[str, float] | None = None,
|
||||
):
|
||||
hand = get_hand_calibration_profile(side)
|
||||
zero = get_zero_calibration_profile(side)
|
||||
source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
|
||||
baseline = [255.0] * 20
|
||||
baseline[6:10] = [127.0] * 4
|
||||
curves = {
|
||||
name: _synthetic_curve(
|
||||
int(baseline[hand.joint_specs[name].motor_index]),
|
||||
math.radians(50.0),
|
||||
)
|
||||
for name in hand.measured_joints
|
||||
}
|
||||
if inject_secondary_root_axis_bias_degrees:
|
||||
# Make the thumb root the higher-travel, directly observed direction,
|
||||
# matching the real right-hand data where the short pinky splay arc is
|
||||
# the less reliable root-axis orientation estimate.
|
||||
curves["thumb_cmc_roll"] = _synthetic_curve(
|
||||
int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]),
|
||||
math.radians(70.0),
|
||||
)
|
||||
motor_by_joint = {
|
||||
name: spec.motor_index for name, spec in hand.joint_specs.items()
|
||||
}
|
||||
offsets = {
|
||||
name: math.radians(value)
|
||||
for name, value in zip(zero.direct_zero_joints, offset_degrees)
|
||||
}
|
||||
model = UrdfKinematicModel(source)
|
||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
||||
measurements: list[JointAxisMeasurement] = []
|
||||
for cycle in range(3):
|
||||
for joint in zero.axis_joints:
|
||||
state = list(baseline)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
state[5] = 145.0
|
||||
angles = _angles_from_state(
|
||||
state,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero.inherited_zero_joints,
|
||||
)
|
||||
axis, point = model.axis_line(
|
||||
joint, zero_offsets=offsets, joint_angles=angles
|
||||
)
|
||||
point_common = base_rotation.apply(point) + base_translation
|
||||
if (
|
||||
inject_secondary_root_point_bias_m
|
||||
and joint == f"{zero.reference_finger}_mcp_roll"
|
||||
):
|
||||
# A repeatable monocular depth error on the second parallel
|
||||
# root line must affect translation only, never palm rotation
|
||||
# or the inferred thumb-roll zero.
|
||||
point_common = point_common + base_rotation.apply(
|
||||
np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m])
|
||||
)
|
||||
view_normal_common = None
|
||||
if (
|
||||
inject_oblique_optical_depth_bias
|
||||
and joint in zero.phase_parent_joint
|
||||
):
|
||||
parent_axis = model.axis_line(
|
||||
zero.phase_parent_joint[joint],
|
||||
zero_offsets=offsets,
|
||||
joint_angles=angles,
|
||||
)[0]
|
||||
helper = (
|
||||
np.asarray([1.0, 0.0, 0.0])
|
||||
if abs(float(parent_axis[0])) < 0.8
|
||||
else np.asarray([0.0, 1.0, 0.0])
|
||||
)
|
||||
tilt_axis = np.cross(parent_axis, helper)
|
||||
tilt_axis /= np.linalg.norm(tilt_axis)
|
||||
view_normal = Rotation.from_rotvec(
|
||||
math.radians(15.0) * tilt_axis
|
||||
).apply(parent_axis)
|
||||
view_normal_common = tuple(base_rotation.apply(view_normal))
|
||||
# Simulate an independent planar-PnP depth error on the child
|
||||
# Tag. It is large enough to drive the old 3-D phase solve to
|
||||
# a configured offset bound.
|
||||
point_common = point_common + 0.03 * np.asarray(
|
||||
view_normal_common
|
||||
)
|
||||
axis_common = base_rotation.apply(axis)
|
||||
if (
|
||||
inject_observer_cone_bias_degrees
|
||||
and joint == "thumb_cmc_pitch"
|
||||
):
|
||||
parent_axis = model.axis_line(
|
||||
zero.axis_parent_joint[joint],
|
||||
zero_offsets=offsets,
|
||||
joint_angles=angles,
|
||||
)[0]
|
||||
cone_normal = np.cross(axis, parent_axis)
|
||||
cone_normal /= np.linalg.norm(cone_normal)
|
||||
axis_common = base_rotation.apply(
|
||||
Rotation.from_rotvec(
|
||||
math.radians(inject_observer_cone_bias_degrees)
|
||||
* cone_normal
|
||||
).apply(axis)
|
||||
)
|
||||
if (
|
||||
inject_secondary_root_axis_bias_degrees
|
||||
and joint == f"{zero.reference_finger}_mcp_roll"
|
||||
):
|
||||
helper = np.asarray([0.0, 0.0, 1.0])
|
||||
if abs(float(axis_common @ helper)) > 0.8:
|
||||
helper = np.asarray([0.0, 1.0, 0.0])
|
||||
bias_axis = np.cross(axis_common, helper)
|
||||
bias_axis /= np.linalg.norm(bias_axis)
|
||||
axis_common = Rotation.from_rotvec(
|
||||
math.radians(inject_secondary_root_axis_bias_degrees)
|
||||
* bias_axis
|
||||
).apply(axis_common)
|
||||
measurements.append(
|
||||
JointAxisMeasurement(
|
||||
joint=joint,
|
||||
cycle=cycle,
|
||||
axis_common_xyz=tuple(axis_common),
|
||||
point_common_xyz_m=tuple(point_common),
|
||||
condition_state_u8=tuple(state),
|
||||
plane_rms_m=0.0002,
|
||||
radial_rms_m=0.0002,
|
||||
rotation_circle_axis_difference_rad=math.radians(0.1),
|
||||
view_normal_common_xyz=view_normal_common,
|
||||
pose_axis_line_rms_m=(
|
||||
pose_axis_line_rms_by_joint_m or {}
|
||||
).get(joint, 0.0),
|
||||
)
|
||||
)
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=source,
|
||||
measurements=measurements,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
hand_type=side,
|
||||
joint_maximum_offset_rad={
|
||||
name: math.radians(value)
|
||||
for name, value in (joint_maximum_offset_degrees or {}).items()
|
||||
},
|
||||
)
|
||||
return zero, result
|
||||
|
||||
|
||||
def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None:
|
||||
zero, result = _solve_synthetic_offsets("right", [0.1] * 7)
|
||||
assert result.passed is True
|
||||
static_policy = {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}
|
||||
for name, value in result.direct_offsets_rad.items():
|
||||
assert value == pytest.approx(static_policy.get(name, 0.0))
|
||||
|
||||
|
||||
def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None:
|
||||
right = get_zero_calibration_profile("right")
|
||||
left = get_zero_calibration_profile("left")
|
||||
|
||||
assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad
|
||||
|
||||
|
||||
def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None:
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0]
|
||||
)
|
||||
reference_roll = f"{zero.reference_finger}_mcp_roll"
|
||||
assert result.passed is True
|
||||
assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
|
||||
|
||||
def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None:
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
inject_secondary_root_axis_bias_degrees=15.0,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
expected = dict(
|
||||
zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
|
||||
)
|
||||
expected.update(
|
||||
{
|
||||
name: math.degrees(value)
|
||||
for name, value in {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}.items()
|
||||
}
|
||||
)
|
||||
for name, value in expected.items():
|
||||
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
|
||||
value, abs=0.05
|
||||
)
|
||||
|
||||
|
||||
def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None:
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
inject_secondary_root_point_bias_m=0.02,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert math.degrees(
|
||||
result.direct_offsets_rad["thumb_cmc_roll"]
|
||||
) == pytest.approx(2.0, abs=0.05)
|
||||
assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0
|
||||
|
||||
|
||||
def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None:
|
||||
offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0]
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
offsets,
|
||||
joint_maximum_offset_degrees={"thumb_mcp": 45.0},
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
|
||||
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0))
|
||||
assert "thumb_ip" not in result.validation_error_by_joint_rad
|
||||
for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
|
||||
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
|
||||
for name in ("pinky_mcp_pitch", "pinky_pip"):
|
||||
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0
|
||||
assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None:
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
inject_oblique_optical_depth_bias=True,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
expected = dict(
|
||||
zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
|
||||
)
|
||||
expected.update(
|
||||
{
|
||||
name: math.degrees(value)
|
||||
for name, value in {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}.items()
|
||||
}
|
||||
)
|
||||
for name, value in expected.items():
|
||||
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
|
||||
value, abs=0.05
|
||||
)
|
||||
|
||||
|
||||
def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None:
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
inject_observer_cone_bias_degrees=8.0,
|
||||
)
|
||||
|
||||
assert result.passed is False
|
||||
assert result.failure_reasons["thumb_cmc_yaw"] == (
|
||||
"zero_axis_cone_mismatch_too_large"
|
||||
)
|
||||
|
||||
|
||||
def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None:
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002},
|
||||
)
|
||||
|
||||
assert result.passed is False
|
||||
assert result.failure_reasons["thumb_cmc_pitch"] == (
|
||||
"zero_phase_axis_line_residual_too_large"
|
||||
)
|
||||
|
||||
|
||||
def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
|
||||
zero = get_zero_calibration_profile("left")
|
||||
baseline = [255.0] * 20
|
||||
baseline[6:10] = [127.0] * 4
|
||||
curves = {
|
||||
name: _synthetic_curve(
|
||||
int(baseline[JOINT_SPECS[name].motor_index]),
|
||||
math.radians(50.0),
|
||||
)
|
||||
for name in MEASURED_JOINTS
|
||||
}
|
||||
motor_by_joint = {
|
||||
name: spec.motor_index for name, spec in JOINT_SPECS.items()
|
||||
}
|
||||
true_offsets = {
|
||||
name: math.radians(value)
|
||||
for name, value in zip(
|
||||
DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]
|
||||
)
|
||||
}
|
||||
model = UrdfKinematicModel(SOURCE_URDF)
|
||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
||||
measurements = []
|
||||
yaw_axis_without_clearance = None
|
||||
yaw_axis_with_clearance = None
|
||||
for cycle in range(3):
|
||||
for joint in AXIS_JOINTS:
|
||||
state = list(baseline)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
state[5] = 145.0
|
||||
angles = _angles_from_state(
|
||||
state, curves=curves, motor_by_joint=motor_by_joint
|
||||
)
|
||||
axis, point = model.axis_line(
|
||||
joint,
|
||||
zero_offsets=true_offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
yaw_axis_with_clearance = axis.copy()
|
||||
baseline_angles = _angles_from_state(
|
||||
baseline, curves=curves, motor_by_joint=motor_by_joint
|
||||
)
|
||||
yaw_axis_without_clearance = model.axis_line(
|
||||
joint,
|
||||
zero_offsets=true_offsets,
|
||||
joint_angles=baseline_angles,
|
||||
)[0]
|
||||
measurements.append(
|
||||
JointAxisMeasurement(
|
||||
joint=joint,
|
||||
cycle=cycle,
|
||||
axis_common_xyz=tuple(base_rotation.apply(axis)),
|
||||
point_common_xyz_m=tuple(
|
||||
base_rotation.apply(point) + base_translation
|
||||
),
|
||||
condition_state_u8=tuple(state),
|
||||
plane_rms_m=0.0002,
|
||||
radial_rms_m=0.0002,
|
||||
rotation_circle_axis_difference_rad=math.radians(0.1),
|
||||
)
|
||||
)
|
||||
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=SOURCE_URDF,
|
||||
measurements=measurements,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
)
|
||||
|
||||
assert math.degrees(
|
||||
math.acos(
|
||||
np.clip(yaw_axis_with_clearance @ yaw_axis_without_clearance, -1.0, 1.0)
|
||||
)
|
||||
) > 1.0
|
||||
assert result.passed is True
|
||||
static_policy = {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}
|
||||
for name, expected in true_offsets.items():
|
||||
if name in static_policy:
|
||||
expected = static_policy[name]
|
||||
assert result.direct_offsets_rad[name] == pytest.approx(
|
||||
expected, abs=math.radians(0.05)
|
||||
)
|
||||
assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS)
|
||||
assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx(
|
||||
0.0, abs=math.radians(0.05)
|
||||
)
|
||||
for target in INHERITED_ZERO_JOINTS:
|
||||
assert result.all_active_offsets_rad[target] == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys()
|
||||
|
||||
|
||||
def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None:
|
||||
hand = get_hand_calibration_profile("right")
|
||||
zero = get_zero_calibration_profile("right")
|
||||
baseline = [255.0] * 20
|
||||
baseline[6:10] = [127.0] * 4
|
||||
curves = {
|
||||
name: _synthetic_curve(
|
||||
int(baseline[hand.joint_specs[name].motor_index]),
|
||||
math.radians(50.0),
|
||||
)
|
||||
for name in hand.measured_joints
|
||||
}
|
||||
motor_by_joint = {
|
||||
name: spec.motor_index for name, spec in hand.joint_specs.items()
|
||||
}
|
||||
true_offsets = {
|
||||
name: math.radians(value)
|
||||
for name, value in zip(
|
||||
zero.direct_zero_joints,
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
)
|
||||
}
|
||||
model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
|
||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
||||
measurements: list[JointAxisMeasurement] = []
|
||||
for cycle in range(3):
|
||||
states: dict[str, list[float]] = {}
|
||||
lines: dict[str, tuple[np.ndarray, np.ndarray]] = {}
|
||||
for joint in zero.axis_joints:
|
||||
state = list(baseline)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
state[5] = 145.0
|
||||
states[joint] = state
|
||||
angles = _angles_from_state(
|
||||
state,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero.inherited_zero_joints,
|
||||
)
|
||||
lines[joint] = model.axis_line(
|
||||
joint,
|
||||
zero_offsets=true_offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
original_lines = {
|
||||
name: (axis.copy(), point.copy())
|
||||
for name, (axis, point) in lines.items()
|
||||
}
|
||||
for observer, parent in zero.phase_parent_joint.items():
|
||||
parent_axis, parent_point = lines[parent]
|
||||
original_parent_axis, original_parent_point = original_lines[parent]
|
||||
child_axis, original_child_point = original_lines[observer]
|
||||
radial = original_child_point - original_parent_point
|
||||
radial -= original_parent_axis * float(
|
||||
radial @ original_parent_axis
|
||||
)
|
||||
# Preserve angular phase while deliberately corrupting link radius
|
||||
# and along-axis depth. These components must not move a zero.
|
||||
lines[observer] = (
|
||||
child_axis,
|
||||
parent_point + 1.25 * radial + 0.02 * parent_axis,
|
||||
)
|
||||
for joint in zero.axis_joints:
|
||||
axis, point = lines[joint]
|
||||
measurements.append(
|
||||
JointAxisMeasurement(
|
||||
joint=joint,
|
||||
cycle=cycle,
|
||||
axis_common_xyz=tuple(base_rotation.apply(axis)),
|
||||
point_common_xyz_m=tuple(
|
||||
base_rotation.apply(point) + base_translation
|
||||
),
|
||||
condition_state_u8=tuple(states[joint]),
|
||||
plane_rms_m=0.0002,
|
||||
radial_rms_m=0.0002,
|
||||
rotation_circle_axis_difference_rad=math.radians(0.1),
|
||||
)
|
||||
)
|
||||
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=RIGHT_SOURCE_URDF,
|
||||
measurements=measurements,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
hand_type="right",
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
static_policy = {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}
|
||||
for name, expected in true_offsets.items():
|
||||
if name in static_policy:
|
||||
expected = static_policy[name]
|
||||
assert result.direct_offsets_rad[name] == pytest.approx(
|
||||
expected, abs=math.radians(0.05)
|
||||
)
|
||||
for target in zero.inherited_zero_joints:
|
||||
assert result.all_active_offsets_rad[target] == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
assert set(result.all_active_offsets_rad) == set(hand.active_joints)
|
||||
@@ -9,6 +9,20 @@ class HandConfig:
|
||||
joint_names_en: Optional[List[str]] = None
|
||||
init_pos: List[int] = field(default_factory=list)
|
||||
preset_actions: Optional[Dict[str, List[int]]] = None
|
||||
preset_action_overrides: Optional[Dict[str, Dict[str, List[int]]]] = None
|
||||
# Integer GUI values are divided by this scale before publishing.
|
||||
# Legacy hands use raw u8 values (scale=1); O12 uses milliradians.
|
||||
position_scale: int = 1
|
||||
position_unit: str = "u8"
|
||||
|
||||
def get_preset_actions(self, hand_type: str) -> Dict[str, List[int]]:
|
||||
"""Return preset actions with hand-specific values applied."""
|
||||
actions = dict(self.preset_actions or {})
|
||||
if self.preset_action_overrides:
|
||||
actions.update(
|
||||
self.preset_action_overrides.get(hand_type.lower(), {})
|
||||
)
|
||||
return actions
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 常量字典（仅构建一次）
|
||||
@@ -75,10 +89,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
|
||||
"握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
|
||||
"张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"OK": [0, 0, 255, 255, 255, 151, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 255, 255, 255],
|
||||
"拇指对中指": [0, 255, 0, 255, 255, 119, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 220, 255, 255],
|
||||
"拇指对无名指": [0, 255, 255, 0, 255, 88, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 229, 254],
|
||||
"拇指对小指": [0, 255, 255, 255, 0, 49, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 215],
|
||||
"OK": [0, 0, 255, 255, 255, 138, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 211, 255, 255, 255],
|
||||
"拇指对中指": [0, 255, 0, 255, 255, 107, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 202, 255, 255],
|
||||
"拇指对无名指": [0, 255, 255, 0, 255, 88, 171, 148, 105, 42, 59, 255, 255, 255, 255, 255, 255, 255, 206, 254],
|
||||
"拇指对小指": [0, 255, 255, 255, 0, 32, 170, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 203],
|
||||
"准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
|
||||
"壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
|
||||
"贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],
|
||||
@@ -100,8 +114,6 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"动作9": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"末端1": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
|
||||
"末端2": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"末端3": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
|
||||
@@ -224,13 +236,20 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"叁": [92, 87, 255, 255, 255, 0],
|
||||
"肆": [92, 87, 255, 255, 255, 255],
|
||||
"伍": [255, 255, 255, 255, 255, 255],
|
||||
"OK": [139, 91, 103, 250, 250, 250],
|
||||
"OK": [95, 75, 116, 255, 255, 255],
|
||||
"拇指对中指": [88, 2, 255, 114, 255, 255],
|
||||
"点赞": [250, 79, 0, 0, 0, 0],
|
||||
"握拳": [102, 18, 0, 0, 0, 0],
|
||||
}
|
||||
},
|
||||
preset_action_overrides={
|
||||
"right": {
|
||||
"OK": [95, 83, 122, 255, 255, 255],
|
||||
"拇指对中指": [95, 8, 255, 114, 255, 255],
|
||||
},
|
||||
},
|
||||
),
|
||||
"L6": HandConfig(
|
||||
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
|
||||
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
|
||||
joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
|
||||
init_pos=[250] * 6,
|
||||
preset_actions={
|
||||
@@ -240,7 +259,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"叁": [0, 39, 255, 255, 255, 0],
|
||||
"肆": [0, 0, 255, 255, 255, 255],
|
||||
"伍": [255, 255, 255, 255, 255, 255],
|
||||
"OK": [74, 13, 153, 255, 255, 255],
|
||||
"OK": [62, 5, 151, 255, 255, 255],
|
||||
"点赞": [255, 255, 0, 0, 0, 0],
|
||||
"握拳": [79, 11, 0, 0, 0, 0],
|
||||
"序列动作1": [250, 250, 250, 250, 250, 250],
|
||||
@@ -256,7 +275,56 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"拇指压感准备1": [139, 18, 130, 0, 0, 0],
|
||||
"拇指压感测试": [39, 30, 122, 250, 250, 250],
|
||||
"拇指压感准备2": [139, 18, 130, 0, 0, 0]
|
||||
}
|
||||
},
|
||||
preset_action_overrides={
|
||||
"right": {
|
||||
"OK": [58, 6, 153, 255, 255, 255],
|
||||
},
|
||||
},
|
||||
),
|
||||
"O12": HandConfig(
|
||||
# O12 active-angle API order. Keep this independent from the
|
||||
# physical motor order exposed by some SDK metadata.
|
||||
joint_names_en=[
|
||||
"thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
|
||||
"index_abad", "index_mcp", "index_pip",
|
||||
"middle_abad", "middle_mcp", "middle_pip",
|
||||
"ring_mcp", "pinky_mcp",
|
||||
],
|
||||
joint_names=[
|
||||
"拇指旋转", "拇指侧摆", "拇指根部", "拇指中部",
|
||||
"食指侧摆", "食指根部", "食指中部",
|
||||
"中指侧摆", "中指根部", "中指中部",
|
||||
"无名指根部", "小指根部",
|
||||
],
|
||||
# Values are milliradians so QSlider can retain 0.001 rad resolution.
|
||||
init_pos=[0] * 12,
|
||||
preset_actions={
|
||||
"展开": [0] * 12,
|
||||
"食指轻弯": [0, 0, 0, 0, 0, 300, 400, 0, 0, 0, 0, 0],
|
||||
"半握": [
|
||||
300, -200, -200, -500,
|
||||
0, 650, 700, 0, 650, 800, 700, 700,
|
||||
],
|
||||
},
|
||||
preset_action_overrides={
|
||||
"right": {
|
||||
"拇指对食指": [
|
||||
245, -766, -818, 0, 0, 1046, 0, 0, 0, 0, 0, 0,
|
||||
],
|
||||
"拇指对中指": [
|
||||
489, -843, -685, 0, 0, 0, 0, 0, 1147, 0, 0, 0,
|
||||
],
|
||||
"拇指对无名指": [
|
||||
699, -779, -827, 0, 0, 0, 0, 0, 0, 0, 690, 0,
|
||||
],
|
||||
"拇指对小指": [
|
||||
786, -1065, -827, 0, 0, 0, 0, 0, 0, 0, 0, 645,
|
||||
],
|
||||
},
|
||||
},
|
||||
position_scale=1000,
|
||||
position_unit="rad",
|
||||
),
|
||||
}
|
||||
HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS)
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
import sys
|
||||
import time, json
|
||||
import math
|
||||
import threading
|
||||
from dataclasses import dataclass
|
||||
from typing import List, Dict
|
||||
@@ -33,6 +34,16 @@ _CANONICAL_COMMAND_NAMES = {
|
||||
"thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch",
|
||||
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
|
||||
],
|
||||
"L6": [
|
||||
"thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch",
|
||||
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
|
||||
],
|
||||
"O12": [
|
||||
"thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
|
||||
"index_abad", "index_mcp", "index_pip",
|
||||
"middle_abad", "middle_mcp", "middle_pip",
|
||||
"ring_mcp", "pinky_mcp",
|
||||
],
|
||||
}
|
||||
|
||||
_CANONICAL_COMMAND_BOUNDS = {
|
||||
@@ -43,17 +54,38 @@ _CANONICAL_COMMAND_BOUNDS = {
|
||||
*[(0, 255)] * 5,
|
||||
],
|
||||
"O6": [(0, 255)] * 6,
|
||||
"L6": [(0, 255)] * 6,
|
||||
}
|
||||
|
||||
# Integer slider bounds in milliradians, derived from the O12 active-angle
|
||||
# limits. Right and left thumb signs differ; all other limits are shared.
|
||||
_O12_COMMAND_BOUNDS_MRAD = {
|
||||
"right": [
|
||||
(0, 942), (-1387, 0), (-827, 0), (-1291, 0),
|
||||
(-261, 261), (0, 1352), (0, 1530), (-261, 261),
|
||||
(0, 1357), (0, 1815), (0, 1535), (0, 1535),
|
||||
],
|
||||
"left": [
|
||||
(-942, 0), (0, 1387), (-827, 0), (-1291, 0),
|
||||
(-261, 261), (0, 1352), (0, 1530), (-261, 261),
|
||||
(0, 1357), (0, 1815), (0, 1535), (0, 1535),
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
class ROS2NodeManager(QObject):
|
||||
"""ROS2节点管理器，处理ROS通信"""
|
||||
status_updated = pyqtSignal(str, str) # 状态类型, 消息内容
|
||||
feedback_updated = pyqtSignal(object)
|
||||
|
||||
def __init__(self, node_name: str = "hand_control_node"):
|
||||
super().__init__()
|
||||
self.node = None
|
||||
self.publisher = None
|
||||
self.publisher_arc = None
|
||||
self.state_subscription = None
|
||||
self.speed_pub = None
|
||||
self.torque_pub = None
|
||||
self.joint_state = JointState()
|
||||
self.joint_state.header = Header()
|
||||
|
||||
@@ -83,27 +115,40 @@ class ROS2NodeManager(QObject):
|
||||
self.node.get_parameter('topic_prefix').value
|
||||
)
|
||||
|
||||
if self.is_arc == True:
|
||||
if self.hand_joint == "O12":
|
||||
topic_base = f'/o12/{self.hand_type}'
|
||||
self.publisher = self.node.create_publisher(
|
||||
JointState, f'{topic_base}/joint_cmd', 10
|
||||
)
|
||||
self.state_subscription = self.node.create_subscription(
|
||||
JointState,
|
||||
f'{topic_base}/joint_states',
|
||||
self._o12_feedback_callback,
|
||||
10,
|
||||
)
|
||||
elif self.is_arc == True:
|
||||
# 创建发布者
|
||||
self.publisher_arc = self.node.create_publisher(
|
||||
JointState,
|
||||
self.topic(f'/cb_{self.hand_type}_hand_control_cmd_arc'),
|
||||
10
|
||||
)
|
||||
# 创建发布者
|
||||
self.publisher = self.node.create_publisher(
|
||||
JointState,
|
||||
self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
|
||||
10
|
||||
)
|
||||
# 创建旧型号发布者
|
||||
if self.hand_joint != "O12":
|
||||
self.publisher = self.node.create_publisher(
|
||||
JointState,
|
||||
self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
|
||||
10
|
||||
)
|
||||
self.snapshot_publisher = self.node.create_publisher(
|
||||
JointState, self.topic('/calibration_pose_snapshot'), 10
|
||||
)
|
||||
# 新增 speed / torque 发布者
|
||||
self.speed_pub = self.node.create_publisher(
|
||||
String, self.topic('/cb_hand_setting_cmd'), 10)
|
||||
self.torque_pub = self.node.create_publisher(
|
||||
String, self.topic('/cb_hand_setting_cmd'), 10)
|
||||
if self.hand_joint != "O12":
|
||||
self.speed_pub = self.node.create_publisher(
|
||||
String, self.topic('/cb_hand_setting_cmd'), 10)
|
||||
self.torque_pub = self.node.create_publisher(
|
||||
String, self.topic('/cb_hand_setting_cmd'), 10)
|
||||
self.status_updated.emit(
|
||||
"info",
|
||||
f"ROS2节点初始化成功: {self.topic_prefix or '/'} "
|
||||
@@ -136,15 +181,36 @@ class ROS2NodeManager(QObject):
|
||||
while rclpy.ok() and self.node:
|
||||
rclpy.spin_once(self.node, timeout_sec=0.1)
|
||||
|
||||
def _o12_feedback_callback(self, msg: JointState):
|
||||
"""Forward 12-channel O12 radian feedback safely into the Qt thread."""
|
||||
if len(msg.position) != 12:
|
||||
self.status_updated.emit(
|
||||
"error", f"O12位置反馈长度错误: {len(msg.position)}，应为12"
|
||||
)
|
||||
return
|
||||
self.feedback_updated.emit([float(value) for value in msg.position])
|
||||
|
||||
def command_bounds(self, count: int):
|
||||
if self.hand_joint == "O12":
|
||||
bounds = _O12_COMMAND_BOUNDS_MRAD.get(self.hand_type)
|
||||
else:
|
||||
bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
|
||||
if not bounds or len(bounds) != count:
|
||||
return [(0, 255)] * count
|
||||
return bounds
|
||||
|
||||
def bound_positions(self, positions: List[int]) -> List[int]:
|
||||
bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
|
||||
if not bounds or len(bounds) != len(positions):
|
||||
return list(positions)
|
||||
bounds = self.command_bounds(len(positions))
|
||||
return [
|
||||
max(minimum, min(maximum, int(value)))
|
||||
for value, (minimum, maximum) in zip(positions, bounds)
|
||||
]
|
||||
|
||||
def wire_positions(self, positions: List[int]) -> List[float]:
|
||||
positions = self.bound_positions(positions)
|
||||
scale = _HAND_CONFIGS[self.hand_joint].position_scale
|
||||
return [float(value) / float(scale) for value in positions]
|
||||
|
||||
def publish_joint_state(self, positions: List[int]):
|
||||
"""发布关节状态消息"""
|
||||
if not self.publisher or not self.node:
|
||||
@@ -153,15 +219,21 @@ class ROS2NodeManager(QObject):
|
||||
|
||||
try:
|
||||
positions = self.bound_positions(positions)
|
||||
wire_positions = self.wire_positions(positions)
|
||||
self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
|
||||
self.joint_state.position = [float(pos) for pos in positions]
|
||||
self.joint_state.position = wire_positions
|
||||
# self.joint_state.velocity = [0.1] * len(positions)
|
||||
# self.joint_state.effort = [0.01] * len(positions)
|
||||
# 如果有关节名称，添加到消息中
|
||||
#hand_config = HandConfig.from_hand_type(self.hand_joint)
|
||||
hand_config = _HAND_CONFIGS[self.hand_joint]
|
||||
canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
|
||||
if canonical_names and len(canonical_names) == len(positions):
|
||||
# O12 SDK 1.1.8 may expose names in physical motor order while the
|
||||
# position array uses active-angle order. An empty name array makes
|
||||
# the documented position order unambiguous to the driver.
|
||||
if self.hand_joint == "O12":
|
||||
self.joint_state.name = []
|
||||
elif canonical_names and len(canonical_names) == len(positions):
|
||||
self.joint_state.name = canonical_names
|
||||
elif len(hand_config.joint_names) == len(positions):
|
||||
if hand_config.joint_names_en != None:
|
||||
@@ -170,7 +242,7 @@ class ROS2NodeManager(QObject):
|
||||
self.joint_state.name = hand_config.joint_names
|
||||
|
||||
self.publisher.publish(self.joint_state)
|
||||
if self.is_arc == True:
|
||||
if self.is_arc == True and self.hand_joint != "O12":
|
||||
if self.hand_joint == "O6":
|
||||
if self.hand_type == "left":
|
||||
pose = range_to_arc_left(positions,self.hand_joint)
|
||||
@@ -205,14 +277,21 @@ class ROS2NodeManager(QObject):
|
||||
positions = self.bound_positions(positions)
|
||||
self.publish_joint_state(positions)
|
||||
self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
|
||||
self.joint_state.position = [float(pos) for pos in positions]
|
||||
self.joint_state.position = self.wire_positions(positions)
|
||||
canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
|
||||
if canonical_names:
|
||||
if canonical_names and self.hand_joint != "O12":
|
||||
self.joint_state.name = canonical_names
|
||||
elif self.hand_joint == "O12":
|
||||
self.joint_state.name = []
|
||||
self.snapshot_publisher.publish(self.joint_state)
|
||||
self.status_updated.emit("info", "当前标定姿势快照已发布")
|
||||
|
||||
def publish_speed(self, val: int):
|
||||
if self.hand_joint == "O12":
|
||||
self.status_updated.emit(
|
||||
"warning", "O12不使用旧版u8速度接口；请通过角度轨迹限制速度"
|
||||
)
|
||||
return
|
||||
if self.hand_joint.upper() in ("O6", "L6"):
|
||||
joint_len = 6
|
||||
elif self.hand_joint == "L7":
|
||||
@@ -232,6 +311,11 @@ class ROS2NodeManager(QObject):
|
||||
self.speed_pub.publish(msg)
|
||||
|
||||
def publish_torque(self, val: int):
|
||||
if self.hand_joint == "O12":
|
||||
self.status_updated.emit(
|
||||
"warning", "O12不使用旧版u8扭矩接口；位置GUI已禁用该设置"
|
||||
)
|
||||
return
|
||||
if self.hand_joint.upper() in ("O6", "L6"):
|
||||
joint_len = 6
|
||||
elif self.hand_joint == "L7":
|
||||
@@ -273,11 +357,17 @@ class HandControlGUI(QWidget):
|
||||
# 设置ROS管理器
|
||||
self.ros_manager = ros_manager
|
||||
self.ros_manager.status_updated.connect(self.update_status)
|
||||
self.ros_manager.feedback_updated.connect(self.on_feedback_updated)
|
||||
|
||||
# 获取手部配置
|
||||
self.hand_joint = self.ros_manager.hand_joint
|
||||
self.hand_type = self.ros_manager.hand_type
|
||||
self.hand_config = _HAND_CONFIGS[self.hand_joint]
|
||||
self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
|
||||
self.feedback_positions = None
|
||||
# O12 only publishes after an explicit slider/preset action. This
|
||||
# prevents opening or closing the real hand merely by launching GUI.
|
||||
self.command_dirty = self.hand_joint != "O12"
|
||||
|
||||
# 初始化UI
|
||||
self.init_ui()
|
||||
@@ -288,6 +378,20 @@ class HandControlGUI(QWidget):
|
||||
self.publish_timer.timeout.connect(self.publish_joint_state)
|
||||
self.publish_timer.start()
|
||||
|
||||
def format_position(self, value: int) -> str:
|
||||
"""Format an integer GUI value in the hand's actual command unit."""
|
||||
scale = self.hand_config.position_scale
|
||||
if self.hand_config.position_unit == "rad":
|
||||
radians = float(value) / float(scale)
|
||||
return f"{radians:.3f} rad / {math.degrees(radians):.1f}°"
|
||||
return str(value)
|
||||
|
||||
def command_positions(self) -> List[float]:
|
||||
"""Return current slider values converted to wire units."""
|
||||
return self.ros_manager.wire_positions(
|
||||
[slider.value() for slider in self.sliders]
|
||||
)
|
||||
|
||||
def init_ui(self):
|
||||
"""初始化用户界面"""
|
||||
# 设置窗口属性
|
||||
@@ -465,13 +569,11 @@ class HandControlGUI(QWidget):
|
||||
for i, (name, value) in enumerate(zip(
|
||||
self.hand_config.joint_names, self.hand_config.init_pos
|
||||
)):
|
||||
bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
|
||||
minimum, maximum = (
|
||||
bounds[i] if bounds and i < len(bounds) else (0, 255)
|
||||
)
|
||||
bounds = self.ros_manager.command_bounds(len(self.hand_config.init_pos))
|
||||
minimum, maximum = bounds[i]
|
||||
value = max(minimum, min(maximum, int(value)))
|
||||
# 创建标签
|
||||
label = QLabel(f"{name}: {value}")
|
||||
label = QLabel(f"{name}: {self.format_position(value)}")
|
||||
label.setMinimumWidth(120)
|
||||
|
||||
# 创建滑动条
|
||||
@@ -535,9 +637,9 @@ class HandControlGUI(QWidget):
|
||||
def create_system_preset_buttons(self, parent_layout):
|
||||
"""创建系统预设动作按钮"""
|
||||
self.preset_buttons = [] # 清空按钮列表
|
||||
if self.hand_config.preset_actions:
|
||||
if self.preset_actions:
|
||||
buttons = []
|
||||
for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
|
||||
for idx, (name, positions) in enumerate(self.preset_actions.items()):
|
||||
button = QPushButton(name)
|
||||
button.setProperty("category", "preset")
|
||||
button.clicked.connect(
|
||||
@@ -564,6 +666,9 @@ class HandControlGUI(QWidget):
|
||||
|
||||
# —— 2. 新增：速度与扭矩设置（每行一个）——
|
||||
quick_set_gb = QGroupBox("快速设置")
|
||||
if self.hand_joint == "O12":
|
||||
quick_set_gb.setEnabled(False)
|
||||
quick_set_gb.setToolTip("O12不使用旧版0-255速度/扭矩设置接口")
|
||||
qv_layout = QVBoxLayout(quick_set_gb)
|
||||
|
||||
# 速度行
|
||||
@@ -692,7 +797,10 @@ class HandControlGUI(QWidget):
|
||||
"""滑动条值改变事件处理"""
|
||||
if 0 <= index < len(self.slider_labels):
|
||||
joint_name = self.hand_config.joint_names[index]
|
||||
self.slider_labels[index].setText(f"{joint_name}: {value}")
|
||||
self.slider_labels[index].setText(
|
||||
f"{joint_name}: {self.format_position(value)}"
|
||||
)
|
||||
self.command_dirty = True
|
||||
|
||||
# 更新数值显示
|
||||
self.update_value_display()
|
||||
@@ -700,10 +808,23 @@ class HandControlGUI(QWidget):
|
||||
def update_value_display(self):
|
||||
"""更新数值显示面板内容"""
|
||||
# 获取所有滑动条的当前值
|
||||
values = [slider.value() for slider in self.sliders]
|
||||
|
||||
# 格式化显示为列表形式
|
||||
self.value_display.setText(f"{values}")
|
||||
command = self.command_positions()
|
||||
command_text = ", ".join(f"{value:.3f}" for value in command)
|
||||
lines = [f"目标({self.hand_config.position_unit}): [{command_text}]"]
|
||||
if self.feedback_positions is not None:
|
||||
feedback_text = ", ".join(
|
||||
f"{value:.3f}" for value in self.feedback_positions
|
||||
)
|
||||
lines.append(f"反馈(rad): [{feedback_text}]")
|
||||
self.value_display.setText("\n".join(lines))
|
||||
|
||||
def on_feedback_updated(self, positions):
|
||||
"""Display actual O12 joint feedback without moving command sliders."""
|
||||
self.feedback_positions = list(positions)
|
||||
self.update_value_display()
|
||||
if hasattr(self, "connection_status"):
|
||||
self.connection_status.setText("O12右手已连接并收到位置反馈")
|
||||
self.connection_status.setObjectName("StatusInfo")
|
||||
|
||||
def on_preset_action_clicked(self, positions: List[int]):
|
||||
"""预设动作按钮点击事件处理"""
|
||||
@@ -742,7 +863,25 @@ class HandControlGUI(QWidget):
|
||||
self.cycle_button.setText("循环运行预设动作")
|
||||
self.reset_preset_buttons_color()
|
||||
|
||||
self.status_updated.emit("warning", "已停止所有动作")
|
||||
if self.hand_joint == "O12" and self.feedback_positions is not None:
|
||||
# Hold the latest measured pose instead of merely stopping topic
|
||||
# publication while a previous position target is still active.
|
||||
scale = self.hand_config.position_scale
|
||||
hold_values = [
|
||||
int(round(value * scale)) for value in self.feedback_positions
|
||||
]
|
||||
hold_values = self.ros_manager.bound_positions(hold_values)
|
||||
for slider, value in zip(self.sliders, hold_values):
|
||||
slider.blockSignals(True)
|
||||
slider.setValue(value)
|
||||
slider.blockSignals(False)
|
||||
self.command_dirty = True
|
||||
self.publish_joint_state()
|
||||
self.update_value_display()
|
||||
self.status_updated.emit("warning", "已下发当前反馈位置并保持")
|
||||
else:
|
||||
self.command_dirty = False
|
||||
self.status_updated.emit("warning", "已停止连续动作和新的位置发布")
|
||||
|
||||
def on_save_pose_clicked(self):
|
||||
"""发布当前滑块姿势，供hand_pose_capture写入profile。"""
|
||||
@@ -751,7 +890,7 @@ class HandControlGUI(QWidget):
|
||||
|
||||
def on_cycle_clicked(self):
|
||||
"""循环运行预设动作按钮点击事件处理"""
|
||||
if not self.hand_config.preset_actions:
|
||||
if not self.preset_actions:
|
||||
QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行")
|
||||
return
|
||||
|
||||
@@ -774,19 +913,19 @@ class HandControlGUI(QWidget):
|
||||
|
||||
def run_next_action(self):
|
||||
"""运行下一个预设动作"""
|
||||
if not self.hand_config.preset_actions:
|
||||
if not self.preset_actions:
|
||||
return
|
||||
|
||||
# 重置所有按钮颜色
|
||||
self.reset_preset_buttons_color()
|
||||
|
||||
# 计算下一个动作索引
|
||||
self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
|
||||
self.current_action_index = (self.current_action_index + 1) % len(self.preset_actions)
|
||||
|
||||
# 获取下一个动作
|
||||
action_names = list(self.hand_config.preset_actions.keys())
|
||||
action_names = list(self.preset_actions.keys())
|
||||
action_name = action_names[self.current_action_index]
|
||||
action_positions = self.hand_config.preset_actions[action_name]
|
||||
action_positions = self.preset_actions[action_name]
|
||||
|
||||
# 执行动作
|
||||
self.on_preset_action_clicked(action_positions)
|
||||
@@ -811,6 +950,7 @@ class HandControlGUI(QWidget):
|
||||
"""关节类型改变事件处理"""
|
||||
self.hand_joint = joint_type
|
||||
self.hand_config = _HAND_CONFIGS[self.hand_joint]
|
||||
self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
|
||||
|
||||
# 更新手部信息
|
||||
info_text = f"""手部类型: {self.hand_type}
|
||||
@@ -829,8 +969,11 @@ class HandControlGUI(QWidget):
|
||||
|
||||
def publish_joint_state(self):
|
||||
"""发布当前关节状态"""
|
||||
if self.hand_joint == "O12" and not self.command_dirty:
|
||||
return
|
||||
positions = [slider.value() for slider in self.sliders]
|
||||
self.ros_manager.publish_joint_state(positions)
|
||||
self.command_dirty = False
|
||||
|
||||
def update_status(self, status_type: str, message: str):
|
||||
"""更新状态显示"""
|
||||
|
||||
@@ -0,0 +1,42 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Start the HCAN-connected O12 right hand and its safe position GUI."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, TimerAction
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
o12_driver = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("omnihand_node"),
|
||||
"launch",
|
||||
"omnihand_pro_2025_node.launch.py",
|
||||
])
|
||||
)
|
||||
)
|
||||
|
||||
o12_gui = Node(
|
||||
package="gui_control",
|
||||
executable="gui_control",
|
||||
name="o12_right_gui_control",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"hand_type": "right",
|
||||
"hand_joint": "O12",
|
||||
# Commands are coalesced, so dragging a slider sends at most 10 Hz.
|
||||
"topic_hz": 10,
|
||||
"is_arc": False,
|
||||
"topic_prefix": "",
|
||||
}],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
o12_driver,
|
||||
# Give the CANFD driver a moment to claim and initialise the adapter.
|
||||
TimerAction(period=1.5, actions=[o12_gui]),
|
||||
])
|
||||
@@ -13,6 +13,7 @@
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<exec_depend>linker_hand_ros2_sdk</exec_depend>
|
||||
<exec_depend>omnihand_node</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
from gui_control.config.constants import HAND_CONFIGS
|
||||
|
||||
|
||||
def test_l6_gui_uses_the_sdk_channel_order() -> None:
|
||||
assert HAND_CONFIGS["L6"].joint_names_en == [
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
]
|
||||
|
||||
|
||||
def test_l6_ok_preset_is_hand_specific() -> None:
|
||||
config = HAND_CONFIGS["L6"]
|
||||
|
||||
assert config.get_preset_actions("left")["OK"] == [
|
||||
62, 5, 151, 255, 255, 255,
|
||||
]
|
||||
assert config.get_preset_actions("right")["OK"] == [
|
||||
58, 6, 153, 255, 255, 255,
|
||||
]
|
||||
@@ -0,0 +1,46 @@
|
||||
from gui_control.config.constants import HAND_CONFIGS
|
||||
|
||||
|
||||
def test_o12_gui_uses_active_angle_order_and_milliradians() -> None:
|
||||
config = HAND_CONFIGS["O12"]
|
||||
|
||||
assert config.joint_names_en == [
|
||||
"thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
|
||||
"index_abad", "index_mcp", "index_pip",
|
||||
"middle_abad", "middle_mcp", "middle_pip",
|
||||
"ring_mcp", "pinky_mcp",
|
||||
]
|
||||
assert config.position_scale == 1000
|
||||
assert config.position_unit == "rad"
|
||||
assert len(config.init_pos) == 12
|
||||
assert all(len(pose) == 12 for pose in config.preset_actions.values())
|
||||
|
||||
|
||||
def test_o12_right_fingertip_preset_actions_use_requested_radians() -> None:
|
||||
config = HAND_CONFIGS["O12"]
|
||||
actions = config.get_preset_actions("right")
|
||||
|
||||
assert actions["拇指对食指"] == [
|
||||
245, -766, -818, 0, 0, 1046, 0, 0, 0, 0, 0, 0,
|
||||
]
|
||||
assert actions["拇指对中指"] == [
|
||||
489, -843, -685, 0, 0, 0, 0, 0, 1147, 0, 0, 0,
|
||||
]
|
||||
assert actions["拇指对无名指"] == [
|
||||
699, -779, -827, 0, 0, 0, 0, 0, 0, 0, 690, 0,
|
||||
]
|
||||
assert actions["拇指对小指"] == [
|
||||
786, -1065, -827, 0, 0, 0, 0, 0, 0, 0, 0, 645,
|
||||
]
|
||||
assert all(len(actions[name]) == 12 for name in (
|
||||
"拇指对食指", "拇指对中指", "拇指对无名指", "拇指对小指",
|
||||
))
|
||||
|
||||
|
||||
def test_o12_fingertip_preset_actions_are_right_hand_only() -> None:
|
||||
actions = HAND_CONFIGS["O12"].get_preset_actions("left")
|
||||
|
||||
assert "拇指对食指" not in actions
|
||||
assert "拇指对中指" not in actions
|
||||
assert "拇指对无名指" not in actions
|
||||
assert "拇指对小指" not in actions
|
||||
@@ -0,0 +1,18 @@
|
||||
from gui_control.config.constants import HAND_CONFIGS
|
||||
|
||||
|
||||
def test_o6_ok_preset_is_hand_specific() -> None:
|
||||
config = HAND_CONFIGS["O6"]
|
||||
|
||||
assert config.get_preset_actions("left")["OK"] == [
|
||||
95, 75, 116, 255, 255, 255,
|
||||
]
|
||||
assert config.get_preset_actions("right")["OK"] == [
|
||||
95, 83, 122, 255, 255, 255,
|
||||
]
|
||||
assert config.get_preset_actions("left")["拇指对中指"] == [
|
||||
88, 2, 255, 114, 255, 255,
|
||||
]
|
||||
assert config.get_preset_actions("right")["拇指对中指"] == [
|
||||
95, 8, 255, 114, 255, 255,
|
||||
]
|
||||
+10
@@ -7,6 +7,7 @@ from enum import Enum
|
||||
from utils.open_can import OpenCan
|
||||
from can.exceptions import CanError
|
||||
from utils.color_msg import ColorMsg
|
||||
from utils.feedback import ReceivedPositionFrames
|
||||
current_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
target_dir = os.path.abspath(os.path.join(current_dir, ".."))
|
||||
sys.path.append(target_dir)
|
||||
@@ -178,6 +179,10 @@ class LinkerHandG20Can:
|
||||
|
||||
# 串联控制数据存储
|
||||
self.x41, self.x42, self.x43, self.x44, self.x45 = [], [], [], [], []
|
||||
frame_layout = [[(frame, index) for index in range(6)] for frame in range(0x41, 0x46)]
|
||||
layout = self.joint_state_to_cmd_state(frame_layout)
|
||||
self.position_feedback = ReceivedPositionFrames(
|
||||
{frame: 6 for frame in range(0x41, 0x46)}, [item if item != 0 else None for item in layout])
|
||||
self.x49, self.x4A, self.x4B, self.x4C, self.x4D = [0] * 6, [0] * 6, [0] * 6, [0] * 6, [0] * 6
|
||||
self.x51, self.x52, self.x53, self.x54, self.x55 = [], [], [], [], []
|
||||
self.x59, self.x5A, self.x5B, self.x5C, self.x5D = [], [], [], [], []
|
||||
@@ -354,6 +359,8 @@ class LinkerHandG20Can:
|
||||
response_data = msg.data[1:]
|
||||
if len(list(response_data)) == 0:
|
||||
return
|
||||
if frame_type in range(0x41, 0x46):
|
||||
self.position_feedback.accept(frame_type, response_data, msg.timestamp)
|
||||
# 并联控制指令响应
|
||||
if frame_type == 0x01: self.x01 = list(response_data)
|
||||
elif frame_type == 0x02: self.x02 = list(response_data)
|
||||
@@ -1040,6 +1047,9 @@ class LinkerHandG20Can:
|
||||
return None
|
||||
return self.joint_state_to_cmd_state(state=state)
|
||||
|
||||
def get_feedback_snapshot(self):
|
||||
return self.position_feedback.snapshot()
|
||||
|
||||
def get_current_pub_status(self):
|
||||
"""API接口:获取手指当前状态"""
|
||||
self.get_current_status()
|
||||
|
||||
+42
-2
@@ -1,9 +1,12 @@
|
||||
from collections import deque
|
||||
|
||||
import can
|
||||
import time, sys
|
||||
import threading
|
||||
import numpy as np
|
||||
from utils.open_can import OpenCan
|
||||
from utils.color_msg import ColorMsg
|
||||
from utils.feedback import ReceivedPositionFrames
|
||||
from can.exceptions import CanError
|
||||
|
||||
|
||||
@@ -15,6 +18,7 @@ class LinkerHandL6Can:
|
||||
self.open_can = OpenCan(load_yaml=yaml)
|
||||
|
||||
self.x01 = [0] * 6 # 关节位置
|
||||
self.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)])
|
||||
self.x02 = [-1] * 6 # 转矩限制
|
||||
self.x05 = [0] * 6 # 速度
|
||||
self.x07 = [-1] * 6 # 加速度
|
||||
@@ -57,6 +61,14 @@ class LinkerHandL6Can:
|
||||
self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
|
||||
self.is_lock = False
|
||||
self.version = None
|
||||
# L6 replies to a six-byte 0x01 position command with an immediate
|
||||
# byte-for-byte echo on the same CAN ID. A zero-payload 0x01 state
|
||||
# query also replies on that ID, but with the measured positions.
|
||||
# Keep the two transactions distinct so command echoes never enter
|
||||
# the published feedback stream used by calibration.
|
||||
self._position_echo_lock = threading.Lock()
|
||||
self._pending_position_echoes = deque(maxlen=32)
|
||||
self._position_echo_timeout_seconds = 0.02
|
||||
# Start the receiving thread
|
||||
self.running = True
|
||||
self.receive_thread = threading.Thread(target=self.receive_response)
|
||||
@@ -111,6 +123,11 @@ class LinkerHandL6Can:
|
||||
frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
|
||||
data = [frame_property_value] + [int(val) for val in data_list]
|
||||
msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
|
||||
if frame_property_value == 0x01 and len(data_list) == 6:
|
||||
with self._position_echo_lock:
|
||||
self._pending_position_echoes.append(
|
||||
(time.monotonic(), tuple(int(value) for value in data_list))
|
||||
)
|
||||
try:
|
||||
self.bus.send(msg)
|
||||
except can.CanError as e:
|
||||
@@ -201,7 +218,24 @@ class LinkerHandL6Can:
|
||||
except:
|
||||
return
|
||||
if frame_type == 0x01: # 0x01
|
||||
self.x01 = list(response_data)
|
||||
response = tuple(int(value) for value in response_data)
|
||||
now = time.monotonic()
|
||||
is_position_echo = False
|
||||
with self._position_echo_lock:
|
||||
while (
|
||||
self._pending_position_echoes
|
||||
and now - self._pending_position_echoes[0][0]
|
||||
> self._position_echo_timeout_seconds
|
||||
):
|
||||
self._pending_position_echoes.popleft()
|
||||
for pending in tuple(self._pending_position_echoes):
|
||||
if pending[1] == response:
|
||||
self._pending_position_echoes.remove(pending)
|
||||
is_position_echo = True
|
||||
break
|
||||
if not is_position_echo:
|
||||
self.x01 = list(response)
|
||||
self.position_feedback.accept(frame_type, response, msg.timestamp)
|
||||
elif frame_type == 0x02: # 0x02
|
||||
self.x02 = list(response_data)
|
||||
elif frame_type == 0x05: # Set speed
|
||||
@@ -295,6 +329,9 @@ class LinkerHandL6Can:
|
||||
def get_current_pub_status(self):
|
||||
return self.x01
|
||||
|
||||
def get_feedback_snapshot(self):
|
||||
return self.position_feedback.snapshot()
|
||||
|
||||
def get_speed(self):
|
||||
#self.send_frame(0x05, [],sleep=0.003)
|
||||
#print("L6暂不支持读取实时速度")
|
||||
@@ -391,7 +428,10 @@ class LinkerHandL6Can:
|
||||
return self.x35
|
||||
|
||||
def get_finger_order(self):
|
||||
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
|
||||
# L6 channel 1 is the physical CMC roll actuator. Older SDK releases
|
||||
# exposed the channel as ``thumb_cmc_yaw`` even though the wire order
|
||||
# and mechanism have always been roll.
|
||||
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
|
||||
|
||||
def show_fun_table(self):
|
||||
pass
|
||||
|
||||
+6
@@ -4,6 +4,7 @@ import threading
|
||||
import numpy as np
|
||||
from utils.open_can import OpenCan
|
||||
from utils.color_msg import ColorMsg
|
||||
from utils.feedback import ReceivedPositionFrames
|
||||
from can.exceptions import CanError
|
||||
|
||||
|
||||
@@ -15,6 +16,7 @@ class LinkerHandO6Can:
|
||||
self.open_can = OpenCan(load_yaml=yaml)
|
||||
|
||||
self.x01 = [0] * 6 # 关节位置
|
||||
self.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)])
|
||||
self.x02 = [-1] * 6 # 转矩限制
|
||||
self.x05 = [0] * 6 # 速度
|
||||
self.x07 = [-1] * 6 # 加速度
|
||||
@@ -224,6 +226,7 @@ class LinkerHandO6Can:
|
||||
return
|
||||
if frame_type == 0x01: # 0x01
|
||||
self.x01 = list(response_data)
|
||||
self.position_feedback.accept(frame_type, response_data, msg.timestamp)
|
||||
elif frame_type == 0x02: # 0x02
|
||||
self.x02 = list(response_data)
|
||||
elif frame_type == 0x05: # Set speed
|
||||
@@ -317,6 +320,9 @@ class LinkerHandO6Can:
|
||||
def get_current_pub_status(self):
|
||||
return self.x01
|
||||
|
||||
def get_feedback_snapshot(self):
|
||||
return self.position_feedback.snapshot()
|
||||
|
||||
def get_speed(self):
|
||||
self.send_frame(0x05, [],sleep=0.002)
|
||||
#print("L6暂不支持读取实时速度")
|
||||
|
||||
+1
-1
@@ -372,7 +372,7 @@ class LinkerHandL6RS485:
|
||||
return [0] * 6
|
||||
|
||||
def get_finger_order(self):
|
||||
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
|
||||
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
|
||||
|
||||
# --------------------------------------------------
|
||||
# 便捷方法
|
||||
|
||||
@@ -207,6 +207,14 @@ class LinkerHandApi:
|
||||
getter = getattr(self.hand, "get_cached_current_status", None)
|
||||
return getter() if getter is not None else None
|
||||
|
||||
@property
|
||||
def supports_feedback_timestamps(self):
|
||||
return callable(getattr(self.hand, "get_feedback_snapshot", None))
|
||||
|
||||
def get_feedback_snapshot(self):
|
||||
getter = getattr(self.hand, "get_feedback_snapshot", None)
|
||||
return getter() if getter is not None else None
|
||||
|
||||
|
||||
def get_state_for_pub(self):
|
||||
return self.hand.get_current_pub_status()
|
||||
|
||||
@@ -0,0 +1,65 @@
|
||||
"""Atomic CAN position snapshots with the original receive timestamps."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
from threading import Lock
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FeedbackSnapshot:
|
||||
positions: tuple
|
||||
channel_stamps_ns: tuple[int, ...]
|
||||
|
||||
@property
|
||||
def stamp_ns(self):
|
||||
return max(self.channel_stamps_ns)
|
||||
|
||||
def as_dict(self, names):
|
||||
return {"schema": "can_feedback_v1", "name": list(names),
|
||||
"position": list(self.positions), "channel_stamps_ns": list(self.channel_stamps_ns),
|
||||
"stamp_ns": self.stamp_ns}
|
||||
|
||||
|
||||
class ReceivedPositionFrames:
|
||||
"""Keep receipt identity even if stationary feedback has identical values.
|
||||
|
||||
``layout`` lists the frame and value index for each public SDK channel.
|
||||
None represents a protocol-reserved constant-zero slot, whose freshness
|
||||
is bounded by the oldest actual frame. No polling/publication clock is
|
||||
allowed to update a snapshot's timestamps.
|
||||
"""
|
||||
|
||||
def __init__(self, sizes, layout):
|
||||
self.sizes, self.layout = dict(sizes), tuple(layout)
|
||||
self._frames = {}
|
||||
self._lock = Lock()
|
||||
|
||||
def accept(self, frame, values, timestamp):
|
||||
if frame not in self.sizes or len(values) != self.sizes[frame]:
|
||||
return False
|
||||
if not isinstance(timestamp, (int, float)) or not math.isfinite(timestamp) or timestamp <= 0:
|
||||
return False
|
||||
stamp = int(round(timestamp * 1_000_000_000))
|
||||
with self._lock:
|
||||
previous = self._frames.get(frame)
|
||||
if previous is not None and stamp <= previous[0]:
|
||||
return False
|
||||
self._frames[frame] = (stamp, tuple(values))
|
||||
return True
|
||||
|
||||
def snapshot(self):
|
||||
with self._lock:
|
||||
if self._frames.keys() != self.sizes.keys():
|
||||
return None
|
||||
oldest = min(value[0] for value in self._frames.values())
|
||||
positions, stamps = [], []
|
||||
for item in self.layout:
|
||||
if item is None:
|
||||
positions.append(0)
|
||||
stamps.append(oldest)
|
||||
else:
|
||||
frame, index = item
|
||||
stamp, values = self._frames[frame]
|
||||
positions.append(values[index])
|
||||
stamps.append(stamp)
|
||||
return FeedbackSnapshot(tuple(positions), tuple(stamps))
|
||||
@@ -168,6 +168,8 @@ class LinkerHand(Node):
|
||||
self.last_position_command_time = None
|
||||
|
||||
self.last_hand_state = [-1] * 10
|
||||
self.last_hand_state_stamp_ns = None
|
||||
self._last_timed_feedback_stamps = None
|
||||
self.last_hand_vel = [-1] * 10
|
||||
self.force = [[-1] * 5] * 4
|
||||
self.matrix_dic = {
|
||||
@@ -245,6 +247,7 @@ class LinkerHand(Node):
|
||||
COMMAND_QOS,
|
||||
)
|
||||
self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/cb_{self.hand_type}_hand_state'),10)
|
||||
self.timed_state_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_state_timed'), 30)
|
||||
self.hand_info_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_info'), 10)
|
||||
if self.is_touch == True:
|
||||
if self.modbus != "None":
|
||||
@@ -391,7 +394,7 @@ class LinkerHand(Node):
|
||||
self.last_hand_vel_cmd = None
|
||||
|
||||
def _poll_state_if_due(self):
|
||||
if self.hand_state_pub.get_subscription_count() < 1:
|
||||
if self.hand_state_pub.get_subscription_count() < 1 and self.timed_state_pub.get_subscription_count() < 1:
|
||||
return
|
||||
now = time.monotonic()
|
||||
if state_reads_deferred(
|
||||
@@ -400,8 +403,7 @@ class LinkerHand(Node):
|
||||
self.command_quiet_period,
|
||||
self.defer_state_reads_while_commanding,
|
||||
):
|
||||
# pub_state continues to publish the last completed state as a
|
||||
# heartbeat. A fresh blocking read is made after motion settles.
|
||||
# A repeated heartbeat retains its original measurement time.
|
||||
return
|
||||
if not state_poll_due(
|
||||
self.last_state_poll_time, now, self.state_poll_period
|
||||
@@ -411,6 +413,7 @@ class LinkerHand(Node):
|
||||
# another read on the following timer callback.
|
||||
self.last_state_poll_time = now
|
||||
self.last_hand_state = self.api.get_state()
|
||||
self.last_hand_state_stamp_ns = self.get_clock().now().nanoseconds
|
||||
time.sleep(0.003)
|
||||
if state_poll_due(
|
||||
self.last_velocity_poll_time, now, self.velocity_poll_period
|
||||
@@ -478,9 +481,7 @@ class LinkerHand(Node):
|
||||
|
||||
def pub_state(self):
|
||||
while True:
|
||||
if self.hand_state_pub.get_subscription_count() > 0:
|
||||
msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
|
||||
self.hand_state_pub.publish(msg)
|
||||
self._publish_feedback()
|
||||
if self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
|
||||
msg = Float32MultiArray()
|
||||
msg.data = [float(val) for sublist in self.force for val in sublist]
|
||||
@@ -498,6 +499,26 @@ class LinkerHand(Node):
|
||||
self.hand_info_pub.publish(msg)
|
||||
time.sleep(self.hz)
|
||||
|
||||
def _publish_feedback(self):
|
||||
"""Publish one atomic measured state; never give cached values a new time."""
|
||||
if self.api.supports_feedback_timestamps:
|
||||
snapshot = self.api.get_feedback_snapshot()
|
||||
if snapshot is None:
|
||||
return
|
||||
positions, stamp = snapshot.positions, snapshot.stamp_ns
|
||||
if (snapshot.channel_stamps_ns != self._last_timed_feedback_stamps
|
||||
and self.timed_state_pub.get_subscription_count() > 0):
|
||||
message = String()
|
||||
message.data = json.dumps(snapshot.as_dict(self.api.get_finger_order()))
|
||||
self.timed_state_pub.publish(message)
|
||||
self._last_timed_feedback_stamps = snapshot.channel_stamps_ns
|
||||
else:
|
||||
positions, stamp = self.last_hand_state, self.last_hand_state_stamp_ns
|
||||
if stamp is None:
|
||||
return
|
||||
if self.hand_state_pub.get_subscription_count() > 0:
|
||||
self.hand_state_pub.publish(self.joint_state_msg(positions, self.last_hand_vel, stamp_ns=stamp))
|
||||
|
||||
def pub_matrix_mass(self, dic):
|
||||
"""发布矩阵数据合值 单位g 克 JSON格式"""
|
||||
msg = String()
|
||||
@@ -556,10 +577,12 @@ class LinkerHand(Node):
|
||||
msg.data = json.dumps(self.matrix_dic)
|
||||
self.matrix_touch_pub.publish(msg)
|
||||
|
||||
def joint_state_msg(self, pose,vel=[]):
|
||||
def joint_state_msg(self, pose, vel=(), *, stamp_ns=None):
|
||||
joint_state = JointState()
|
||||
joint_state.header = Header()
|
||||
joint_state.header.stamp = self.get_clock().now().to_msg()
|
||||
if stamp_ns is None:
|
||||
stamp_ns = self.get_clock().now().nanoseconds
|
||||
joint_state.header.stamp.sec, joint_state.header.stamp.nanosec = divmod(int(stamp_ns), 1_000_000_000)
|
||||
joint_state.name = self.api.get_finger_order()
|
||||
joint_state.position = [float(x) for x in pose]
|
||||
if len(vel) > 1:
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
"""A CAN receipt, rather than a heartbeat, defines position sample identity."""
|
||||
|
||||
import json
|
||||
from types import SimpleNamespace
|
||||
|
||||
from linker_hand_ros2_sdk.LinkerHand.utils.feedback import ReceivedPositionFrames
|
||||
from linker_hand_ros2_sdk.linker_hand import LinkerHand
|
||||
|
||||
|
||||
def test_cached_unchanged_and_out_of_order_frames_keep_measurement_identity():
|
||||
frames = ReceivedPositionFrames({1: 2}, [(1, 0), (1, 1)])
|
||||
assert frames.snapshot() is None
|
||||
assert not frames.accept(1, [4], 1.)
|
||||
assert not frames.accept(1, [4, 5], 0.)
|
||||
assert frames.accept(1, [4, 5], 1.)
|
||||
first = frames.snapshot()
|
||||
assert first == frames.snapshot()
|
||||
assert not frames.accept(1, [8, 9], .9)
|
||||
assert not frames.accept(1, [8, 9], 1.)
|
||||
assert frames.snapshot() == first
|
||||
assert frames.accept(1, [4, 5], 2.) # A new stationary observation is real.
|
||||
assert frames.snapshot().positions == first.positions
|
||||
assert frames.snapshot().stamp_ns == 2_000_000_000
|
||||
assert first.stamp_ns == 1_000_000_000
|
||||
|
||||
|
||||
def test_split_finger_snapshot_does_not_retimestamp_held_channels():
|
||||
frames = ReceivedPositionFrames({1: 2, 2: 2}, [(2, 1), (1, 0), None])
|
||||
frames.accept(1, [10, 11], 1.)
|
||||
assert frames.snapshot() is None
|
||||
frames.accept(2, [20, 21], 1.02)
|
||||
first = frames.snapshot()
|
||||
assert first.positions == (21, 10, 0)
|
||||
assert first.channel_stamps_ns == (1_020_000_000, 1_000_000_000, 1_000_000_000)
|
||||
frames.accept(2, [20, 22], 1.04)
|
||||
assert frames.snapshot().channel_stamps_ns == (1_040_000_000, 1_000_000_000, 1_000_000_000)
|
||||
|
||||
|
||||
def test_sdk_republication_cannot_refresh_cached_state_timestamps():
|
||||
frames = ReceivedPositionFrames({1: 2}, [(1, 0), (1, 1)])
|
||||
joint_messages, timed_messages = [], []
|
||||
publisher = lambda messages: SimpleNamespace(get_subscription_count=lambda: 1, publish=messages.append)
|
||||
host = SimpleNamespace(api=SimpleNamespace(supports_feedback_timestamps=True,
|
||||
get_feedback_snapshot=frames.snapshot, get_finger_order=lambda: ['a', 'b']),
|
||||
_last_timed_feedback_stamps=None, last_hand_vel=[], hand_state_pub=publisher(joint_messages),
|
||||
timed_state_pub=publisher(timed_messages))
|
||||
host.joint_state_msg = lambda *args, **kwargs: LinkerHand.joint_state_msg(host, *args, **kwargs)
|
||||
LinkerHand._publish_feedback(host)
|
||||
assert not joint_messages and not timed_messages
|
||||
frames.accept(1, [10, 20], 100.)
|
||||
LinkerHand._publish_feedback(host)
|
||||
LinkerHand._publish_feedback(host)
|
||||
assert len(joint_messages) == 2 and len(timed_messages) == 1
|
||||
assert all(message.header.stamp.sec == 100 for message in joint_messages)
|
||||
assert json.loads(timed_messages[0].data)['channel_stamps_ns'] == [100_000_000_000]*2
|
||||
frames.accept(1, [10, 20], 101.)
|
||||
LinkerHand._publish_feedback(host)
|
||||
assert len(timed_messages) == 2 and joint_messages[-1].header.stamp.sec == 101
|
||||
|
||||
|
||||
def test_g20_timed_layout_matches_the_public_sdk_order():
|
||||
from linker_hand_ros2_sdk.LinkerHand.core.can.linker_hand_g20_can import LinkerHandG20Can
|
||||
hand = LinkerHandG20Can.__new__(LinkerHandG20Can)
|
||||
layout = hand.joint_state_to_cmd_state([[(frame, index) for index in range(6)] for frame in range(0x41, 0x46)])
|
||||
frames = ReceivedPositionFrames({frame: 6 for frame in range(0x41, 0x46)}, [item or None for item in layout])
|
||||
raw = []
|
||||
for finger, frame in enumerate(range(0x41, 0x46)):
|
||||
raw.append([finger*10+i for i in range(6)])
|
||||
frames.accept(frame, raw[-1], 100.+finger*.01)
|
||||
snapshot = frames.snapshot()
|
||||
assert snapshot.positions == tuple(hand.joint_state_to_cmd_state(raw))
|
||||
assert snapshot.channel_stamps_ns[0] == 100_000_000_000
|
||||
assert snapshot.channel_stamps_ns[4] == 100_040_000_000
|
||||
@@ -0,0 +1,75 @@
|
||||
import ast
|
||||
from collections import deque
|
||||
from pathlib import Path
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1] / "linker_hand_ros2_sdk/LinkerHand/core"
|
||||
EXPECTED = [
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
]
|
||||
|
||||
|
||||
def _finger_order(path: Path, class_name: str) -> list[str]:
|
||||
module = ast.parse(path.read_text(encoding="utf-8"))
|
||||
selected = next(
|
||||
item
|
||||
for item in module.body
|
||||
if isinstance(item, ast.ClassDef) and item.name == class_name
|
||||
)
|
||||
method = next(
|
||||
item
|
||||
for item in selected.body
|
||||
if isinstance(item, ast.FunctionDef) and item.name == "get_finger_order"
|
||||
)
|
||||
returned = next(item for item in method.body if isinstance(item, ast.Return))
|
||||
return ast.literal_eval(returned.value)
|
||||
|
||||
|
||||
def test_l6_can_and_rs485_publish_the_same_physical_channel_order() -> None:
|
||||
assert _finger_order(PACKAGE / "can/linker_hand_l6_can.py", "LinkerHandL6Can") == EXPECTED
|
||||
assert _finger_order(
|
||||
PACKAGE / "rs485/linker_hand_l6_rs485.py", "LinkerHandL6RS485"
|
||||
) == EXPECTED
|
||||
|
||||
|
||||
def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
|
||||
linker_hand_root = PACKAGE.parent
|
||||
sys.path.insert(0, str(linker_hand_root))
|
||||
try:
|
||||
from core.can.linker_hand_l6_can import LinkerHandL6Can
|
||||
from utils.feedback import ReceivedPositionFrames
|
||||
finally:
|
||||
sys.path.remove(str(linker_hand_root))
|
||||
|
||||
hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
|
||||
hand.can_id = 0x27
|
||||
hand.x01 = [10, 20, 30, 40, 50, 60]
|
||||
hand.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)])
|
||||
hand._position_echo_lock = threading.Lock()
|
||||
command = (255, 2, 253, 253, 253, 253)
|
||||
hand._pending_position_echoes = deque(
|
||||
[(time.monotonic(), command)], maxlen=32
|
||||
)
|
||||
hand._position_echo_timeout_seconds = 0.02
|
||||
|
||||
hand.process_response(
|
||||
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)), timestamp=100.)
|
||||
)
|
||||
assert hand.x01 == [10, 20, 30, 40, 50, 60]
|
||||
assert not hand._pending_position_echoes
|
||||
|
||||
measured = (250, 3, 252, 252, 252, 252)
|
||||
hand.process_response(
|
||||
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)), timestamp=101.)
|
||||
)
|
||||
assert hand.x01 == list(measured)
|
||||
assert hand.get_feedback_snapshot().channel_stamps_ns == (101_000_000_000,) * 6
|
||||
Submodule
+1
Submodule src/linkerhand-l30-sdk added at 0103fc55f9
@@ -0,0 +1,169 @@
|
||||
# 统一标定需求代码审查（2026-09-10）
|
||||
|
||||
## 结论与审查范围
|
||||
|
||||
审查当前工作区（包含未提交的统一重构、精简 JSON v2 和本会话启动参数修复），
|
||||
不是只审查 Git HEAD，也没有把历史 PASS 当作当前实机验收。
|
||||
通用采集、拟合、URDF 修正、最终文件验收和发布架构已经存在，但仍有迁移残留和精度验证缺口。
|
||||
本轮修正可由代码及隔离测试确认的问题，没有改变原始 CAD、机械限位、SDK、贴 Tag 配置或相机外参。
|
||||
没有启动 SDK、相机或发送机械手运动指令。
|
||||
|
||||
## 已修复的问题
|
||||
|
||||
| 问题 | 影响 | 修改 |
|
||||
| --- | --- | --- |
|
||||
| `vendor_sdk_config:=` 等空 launch 实参 | O6/L6/G20 可能在节点启动前退出 | runner 省略空参数，使用 ROS CLI 的实际解析器覆盖四型号 |
|
||||
| 在线 IO 使用固定 front/side/top 外参加载器 | 新 Profile 即使静态校验通过，在线仍可能不能使用其他机位 | 直接用通用外参加载器，视图和参考机位来自 Profile |
|
||||
| launch 固定三机位、型号文件名回退和过期参数 | 新型号需修改启动代码；部分开关没有实际作用 | 按 Profile 创建机位；要求受保护 Profile/原始 URDF/Tag 路径；移除旧诊断、速度、scope 和输出目录覆盖参数 |
|
||||
| MVS 无图像时只显示“等待设备” | 操作者无法区分 SDK、相机或检测链路故障 | 统一显示每机位有效内参/检测消息等待项、SDK 条件和内参不匹配原因 |
|
||||
| READY 后设备断流仍保留可开始状态 | 启动请求可能使用已失效的就绪条件 | 开始前可撤销 READY；Start 再次检查完整设备条件 |
|
||||
| 实时 CameraInfo 只比较 P 的部分字段 | K、D、R 改变但 P 未变时，整流图可能与原外参不一致 | 实时校验宽高及 K/D/R/P 指纹；开始后不匹配按原有坐标变化故障停止继续采集 |
|
||||
| 每个稳态采样点清空整方向统计 | 显示有效帧和覆盖率归零，误导进度判断 | 分离方向数据与当前稳态点数据，重扫/换方向才重置方向统计 |
|
||||
| 标题只显示序列号 | 序列号不含型号时无法确认当前型号 | 所有型号统一显示 model、side、serial_number |
|
||||
| 正式 finalizer 仍允许旧格式绕过指令拟合 | 新型号配置旧版本可能没有所需的 SDK 指令→rad 产物 | 正式 Profile 与 finalizer 均只接受统一输出版本 2，取消旧输出分派 |
|
||||
| 完成回调读取精简 JSON 已删除的 `quality` | 文件可能已经发布，但节点在结束时抛 KeyError | 删除没有消费者的 `final_quality` 赋值，增加无 quality 字段的完成回调回归 |
|
||||
| 视觉暂停与控制定时器之间的状态竞争 | 定时器等待锁期间已暂停，取得锁后仍可能继续动作 | 在控制锁内再次检查终止状态 |
|
||||
|
||||
开始前等待相机消息的 2 秒窗口不用于扫描中的实时暂停；空检测数组算视觉链路存活。
|
||||
短时 Tag 丢失、低检测率等仍按方向结束的数据质量策略处理，同速重扫一次后仍不足才暂停。
|
||||
|
||||
清理的在线残留包括无人读取的回调计数、命令频率缓存/方法、型号质量标签、旧 tracker reset hook，
|
||||
以及每次反馈回调不必要构造的备用 Adapter。没有删除 SDK 协议接口的抽象方法。
|
||||
|
||||
五个过渡期写出器已移出生产包并保留原文件：
|
||||
`calibration_output/code_review_retired_serializers_umhxlepj/`。
|
||||
包括 `select.py`、`generic_v1.py`、`legacy_v4.py`、`legacy_v6.py`、`native_v7.py`。
|
||||
历史读取器和历史诊断工具仍在；此归档不包含或改写用户的历史标定产物。
|
||||
|
||||
## 视频复核后的补充修复：共用姿态分支筛选
|
||||
|
||||
视频复核发现旧配置把 `pnp_reprojection_tie_px` 和姿态拒绝阈值均设为 `1.5 px`，
|
||||
导致通过质量门槛的 IPPE 候选都参加时间连续性比较，旧镜像分支可能压过明确更好的图像拟合。
|
||||
用现有候选姿态复现时，旧分支 `0.24 px` 会压过另一分支 `0.05 px`。
|
||||
|
||||
公共模块统一默认近似同误差阈值为 `0.03 px`；ROS 与离线采集继承该值。
|
||||
五份旧 YAML 的重复覆盖已移除，四产品的 `calibration_config_sha256` 已同步更新。
|
||||
新增校验拒绝非有限、负值或不小于图像拒绝阈值的分支容差，防止旧的 `1.5/1.5` 配置再次生效。
|
||||
保持实际近似同误差时的连续性选择、原有倾角/图像质量限制和方向结束重扫策略。
|
||||
因此前文“配置保持不变”仅描述首轮结构审查；这次补充修复修改了上述算法配置及其哈希。
|
||||
|
||||
覆盖五份配置和四型号默认采集的九个复现用例，在修复前全部失败，修复后全部通过。
|
||||
第一组姿态、配置、采集与加载实际参数文件的四型号隔离 ROS host 检查共 51 项通过。
|
||||
第二组公共采集/拟合与发布、O12 姿态解析、来源保护、四型号配置及启动检查共 141 项通过，
|
||||
耗时 114.53 秒。两组为本次修改的定向回归，没有宣称本次又执行过全包测试。
|
||||
本次 `colcon build --packages-select linkerhand_calibration --symlink-install` 通过；
|
||||
安装环境读取四产品配置、校验受保护哈希及公共默认值均通过，`git diff --check` 通过。
|
||||
这不代表所有平面双解已消除，也不替代下述相机时序和实机重复性验证。
|
||||
|
||||
## 仍不能由本轮软件审查保证的事项
|
||||
|
||||
### P1：真实采集时间与传输积压
|
||||
|
||||
`hikrobot_camera.py::_publish_frame` 使用主机取到帧时的 ROS 时间戳，未将设备曝光时间映射到统一时钟。
|
||||
SDK 帧结构有设备时间戳和帧号，但设备时钟单位、偏移、漂移与主机时钟同步还没有在当前硬件验证。
|
||||
`runtime/ros/io.py::_state_callback` 对无时间戳反馈也采用接收时间。
|
||||
因此，USB/驱动缓存可能把旧图像与新反馈配对；50 ms 的消息时间戳检查和“只取最新帧”不能排除该问题。
|
||||
本会话已经实测到三台相机共享 480 Mbps Hub 上行，且正面/上方取帧超时。
|
||||
|
||||
后续应在恢复 USB 3.x 链路后，核验设备时间戳、帧号、曝光与反馈时序，并用独立运动数据检查配对误差。
|
||||
不能猜测设备时钟单位、直接给时间戳减一个常数，或放宽质量阈值作为修复。
|
||||
|
||||
### P1：重复性和实机/仿真验收尚未完成
|
||||
|
||||
前三轮训练、第四轮独立验证、冻结 Tag 安装和双文件重读校验已接入。
|
||||
但同手多次独立重采、重新摆放和重新贴 Tag、不同保持姿态/多关节组合动作的实际验收仍缺证据。
|
||||
L6/O6 部分关节由小指参数迁移，不属于所有关节独立实测。
|
||||
发布报告仍应如实保留 `arbitrary_multiaxis_validated: false`。
|
||||
|
||||
外部仿真要加载本次修正 URDF，用同一 SDK 指令话题及配套 JSON 驱动；用实机反馈直接驱动仿真，
|
||||
只能显示反馈对应姿态，不能证明指令映射正确。桥本身是消息转换器，不是独立精度测量工具。
|
||||
|
||||
### P2：数学模块和旧配置仍有维护负担
|
||||
|
||||
`core/fitting/spatial.py` 约 3,656 行，包含轴线拟合、基座位姿、零位优化、可观测性和验证，
|
||||
其中 `solve_urdf_zero_offsets` 从约第 1,921 行开始，嵌套函数及共享局部状态较多。
|
||||
它是当前生产算法，不是可直接删除的废代码。后续拆分应按这些数学职责进行，
|
||||
用固定输入的参数、残差、失败原因和产物回归验证等价性，不应复制成各型号独立算法。
|
||||
|
||||
当前约 58 个兼容模块仍用于旧导入路径、历史回放/迁移工具和旧回归；不能仅因目录名称或静态引用少就删除。
|
||||
几行的公开兼容导出不是第二套拟合算法。旧型号诊断、旧 URDF plan 和部分旧 YAML 参数仍需明确退休范围后继续收缩。
|
||||
受保护的历史配置在本轮保持不变；旧 YAML 中一些已不被在线节点读取的参数，不应当作当前算法的生效开关。
|
||||
|
||||
原始 G20/O12 的 mimic/限位冲突和 CAD 保留策略仍需机械资料确认；当前通用 URDF 修正
|
||||
主要处理零位、限位和线性 mimic，不估计连杆长度、轴位置、mesh 或惯量。
|
||||
|
||||
## 按需求核对
|
||||
|
||||
| 需求 | 当前判断 |
|
||||
| --- | --- |
|
||||
| 多型号共用拟合和 URDF 修正 | 已实现统一生产链；SDK 新协议仍需 Adapter 和 ROS 启动绑定 |
|
||||
| 新型号只关心 SDK、贴 Tag、避让 | 基本成立，但必须声明关节绑定、可观测性、迁移/保留策略和可修改字段，不能从未知安装的单轴 Tag 自动猜出 CAD 零位 |
|
||||
| 统一 SDK 指令→rad JSON 与修正 URDF | 正式仅生成统一 v2 指令表；u8 为 256 项，rad 为显式节点；被动表由标准 mimic 推导并验收，详细证据保留在报告 |
|
||||
| 开始前可移动、开始后固定 | 每次 Start 丢弃预览参考，正式锁定；可见固定基准有漂移监测；遮挡对象是否移动不能实时保证 |
|
||||
| 正确、可重复 | 有独立验证和来源保护，但仍受采集时序与未完成实机验收限制 |
|
||||
| 少暂停且原因明确 | 统一策略保留；开始前明确缺失设备，不用短时视觉丢失实时打断扫描 |
|
||||
| 统一进度且明确型号 | 已修正型号、设备等待原因及方向统计显示 |
|
||||
| 同源指令实机/仿真验证 | 有统一转换桥；实际动态/组合动作对比尚未完成 |
|
||||
|
||||
## 本轮验证
|
||||
|
||||
- 审查中完整标定包回归：**544 通过、1 跳过，383.25 秒**。跳过项要求显式提供
|
||||
`O12_REPLAY_RAW` / `O12_REPLAY_REFERENCE`，没有用合成数据替代历史实测。
|
||||
- 随后补充的启动清理、单一正式输出及归档改动：启动/runner/正式 Profile 拟合/精简产物定向组
|
||||
**74 通过，108.78 秒**，包含四型号与虚拟型号的实际公共拟合和发布路径。
|
||||
- 最后完成回调、暂停竞争、ROS host、等待诊断、实时内参保护：**25 通过，0.98 秒**。
|
||||
与上一组有重叠，不相加冒充一次全量结果；没有宣称最后所有变更又跑过一次全量。
|
||||
- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。
|
||||
安装入口读取四产品的受保护配置、ROS launch 实参解析均通过；167 个生产/兼容 Python 文件 AST 通过；
|
||||
`git diff --check` 通过。
|
||||
- 新 launch 测试实际构造四型号及改名机位的 launch actions，但不执行这些动作。
|
||||
隔离 ROS 节点/标准加载器测试不启动 SDK 或相机，不代表实机验收。
|
||||
|
||||
## 数学核心与 ROS 组合重构的验收记录
|
||||
|
||||
本轮以包含公共 `0.03 px` 姿态分支修复的工作区为基线，只拆分职责、明确数据和并发边界。
|
||||
没有删除基线中仍存在的历史入口，没有修改配置、拟合公式、优化初值/顺序、残差权重或验收阈值。
|
||||
|
||||
- `core/fitting/spatial.py` 保留 45 个原定义符号的显式导出；实际计算在 `spatial_solver/`。
|
||||
`solve.py` 的入口顺序为输入准备、训练求解、统计、独立验证及结果装配。
|
||||
数据通过 `ZeroProblem`、`TrainingProblem`、`TrainingFit` 和各验收结果类型传递。
|
||||
训练接口只接收训练观测，训练几何中的掌部姿态按训练轮筛选。
|
||||
- `core/geometry/pnp.py` 保留 14 个原定义符号；实现拆为 IPPE、单 Tag 跟踪、刚性组和轨迹选择。
|
||||
跟踪默认值集中在 `tag_pose/parameters.py`，ROS 参数只负责加载和 deg/rad 转换。
|
||||
- `UnifiedCalibrationNode` 直接继承 ROS `Node`，组合消息 IO 与 `CalibrationCoordinator`。
|
||||
协调器不加载 ROS 库，复用 `SessionExecution`、采集、运动、固定基准和断点组件。
|
||||
运行参数、观测输入和快照使用明确类型；阶段只有 `CalibrationSession.phase` 一个来源。
|
||||
- `FinalizationController` 组合原 worker 与发布器，协调器接收阶段事件并授权提交。
|
||||
状态锁先于 worker 锁；PnP 在状态锁外计算，提交时检查会话版本、采集/运动对象及稳态边界。
|
||||
暂停、中止与提交互斥；失败不触发自动恢复运动，成功只提交一次。
|
||||
SDK 绑定显式接收反馈新鲜度、时钟、健康订阅和发布接口。
|
||||
|
||||
固定输入记录在本次工作区快照 `/tmp/calibration_structure_baseline_30qf3ozo/`。
|
||||
持久化的对比报告及压缩前后输出位于 `calibration_output/structure_refactor_20260910/`，
|
||||
其中 `comparison.json` 记录输入摘要、差异、产物摘要和拒绝原因，`protected_inputs.json` 保存输入哈希。
|
||||
这些记录是合成数据的结构等价证据，不是实机精度验收。
|
||||
|
||||
| 固定输入 | 比较浮点值数量 | 最大绝对差异 | JSON/拟合结果/阶段事件及 URDF 文本 |
|
||||
| --- | ---: | ---: | --- |
|
||||
| G20 | 46,046 | 0 | 一致 |
|
||||
| L6 | 15,304 | 0 | 一致 |
|
||||
| O6 | 15,304 | 0 | 一致 |
|
||||
| O12 | 14,494 | 0 | 一致 |
|
||||
| 虚拟型号（任务重排） | 15,304 | 0 | 一致 |
|
||||
|
||||
对五组输入分别移除第四轮或注入第四轮 Tag 姿态滑移，10 个拒绝原因和阶段事件与基线一致。
|
||||
167 个配置、Profile、原始 URDF/资源文件的哈希不变。
|
||||
另一个虚拟型号同时重排 SDK 通道、机位名称和扫描任务，经过真实公共采集、finalizer、
|
||||
最终 JSON/URDF 验收和发布，再检查同一 SDK 指令经过模拟传输、查表和标准 mimic 图的一致性。
|
||||
|
||||
验证结果：
|
||||
|
||||
- 整个标定包：581 passed，1 skipped，337.43 s。
|
||||
- SDK 绑定时钟补充后，协调器、四型号 ROS 构造和安全策略回归：27 passed。
|
||||
- 跳过项是 `test_o12_recorded_replay.py`：未提供 `O12_REPLAY_RAW` 与独立参考模型；不计作实机通过。
|
||||
- 改动模块通过 Pyflakes，`git diff --check` 通过。
|
||||
- `colcon build --packages-select linkerhand_calibration --symlink-install` 成功。
|
||||
- 安装后的 12 个 console entry 可导入；两个产品 CLI 的 `--help` 正常。
|
||||
两个 ROS 节点入口均通过四型号受保护 Profile 的转交检查；安装环境下四型号 ROS 构造/Start 回归 4 passed。
|
||||
|
||||
本轮不处理相机采集时刻同步，也不替代实机多次标定和同话题实机/仿真运动对照。
|
||||
@@ -0,0 +1,937 @@
|
||||
# 四轮采集与图像拟合的修改前评估
|
||||
|
||||
日期：2026-09-16。目标：稳定完成采集，从可信标定 JSON 重建修正 URDF。
|
||||
|
||||
## 当前结论（2026-09-16）
|
||||
|
||||
当前 O6 按重新实测的 16.5 mm Tag 完成新采集，完整数据经正式离线流程验收通过，
|
||||
已生成合格 JSON、修正 URDF 和发布清单。本次在线收尾发生系统内存耗尽，不能把离线
|
||||
成功表述为在线全过程无中断。已修正收尾读取原始日志时的整文件副本开销，独立验证见后文。
|
||||
没有增加 O6 专用容错分支或降低物理精度门限，也不据此宣称其他型号实机均已通过。
|
||||
|
||||
| 环节 | 当前证据与结论 |
|
||||
| --- | --- |
|
||||
| 四轮连续采集与必要稳态点 | 新会话 `20260916_105528` 完成 36/36，1 个小指方向自动重扫后通过；采集约 6 分 47 秒。随后收尾时发生 OOM，反馈超时暂停；完整数据已用于正式离线验收。此前两次 16 mm 会话也完成采集，但空间验收失败。 |
|
||||
| 反馈时间 | 使用真实 CAN 接收时间；相同缓存不得伪装成新测量。逐通道时间保留至图像插值。 |
|
||||
| 相机时间 | 三台实际设备的曝光事件与帧计数器吻合；设备锁存换算曝光中点，替换约晚 16.5 ms 的取帧时间。 |
|
||||
| 调度 | ROS 控制/反馈顺序处理，图像独立有界队列；实机对照消除了共享锁造成的反馈排队。 |
|
||||
| 映射输入 | 当前报告的分段线性反馈曲线保留曝光时刻的小数输入；实际字节命令与旧整数表保持量化。 |
|
||||
| 图像证据 | 同一运动保存全部机位的原始角点，不依赖任务主机位或 PnP；最新会话保存 16,822 个图像记录。没有增加运动或停点。 |
|
||||
| 图像验收 | 独立三维测量对原始角点仍须 ≤1.5 px；最终文件按原角度/位置门限检查，额外视角只提供物理容差内的图像相容性证据。已知真值反例和反向拒绝测试通过。 |
|
||||
| 平行转轴零位 | 已修正参考点沿轴移动导致零位变化的公共公式；四型号源 URDF 的已知真值及任意轴上参考点检查通过。数学规则不再由型号开关选择。 |
|
||||
| JSON → URDF | `20260916_105528/offline_165mm` 已生成通过验收的文件与 manifest；另外调用重建 CLI，仅从保存 JSON 和源 URDF 得到的文件与发布 URDF 逐字节相同。 |
|
||||
| 整链空间精度 | 新 16.5 mm 会话已通过原门限。主观测运动/稳态独立验收中，各角色最差角度 P95 为 1.873°，位置 P95 为 2.122 mm；最终文件空间与图像检查通过。精度适用范围是观测关节、扫描方向与固定的其他关节姿态；未独立观测的关节仍按配置迁移。 |
|
||||
| 尺度输入核验 | 用户实测更正为 **16.5×16.5 mm** Tag、**27×27 mm** 棋盘，与独立图像检查基本吻合。当前 O6 的 Profile、检测器和标定节点尺寸已同步并记录新指纹；未修改相机参数，未将旧 16 mm 数据冒充新尺寸采集。 |
|
||||
|
||||
先前的分机位空间不一致已在修正实测尺寸后的新数据上通过验收，未靠增加拟合参数
|
||||
或放宽门限达成。当前稳定性问题集中在已确认的收尾内存峰值；保留完整采集数据直接
|
||||
验证收尾修复，不为重复计算重新运动机械手。其余型号仍需对应硬件实测，O6 当前 Tag
|
||||
布局中的其他三指曲线迁移不等于各关节独立标定。
|
||||
|
||||
上述两次未验收候选的三个主动零位差均小于 0.03°，说明当前分段求解可重复，
|
||||
不能据此证明其绝对空间精度。正式发布仍必须通过整链与最终文件验收。
|
||||
|
||||
以下按发生顺序保留评估依据；早期 CAN 未连接等现场状态已由上述新实采结果更新。
|
||||
|
||||
## 已核实的问题
|
||||
|
||||
1. 当前生产采集要求准备运动先确定唯一 PnP/铰链姿态；不唯一时整场暂停，后续三轮训练数据尚未获得。
|
||||
2. 准备阶段的自由铰链几何与最终保留骨长、轴向的 URDF 使用不同参数空间。
|
||||
3. 后续姿态已受准备模型约束，不能把其重复性直接解释为原始图像上的整链正确性。
|
||||
4. JSON 保存后读取并重建 URDF 已有独立字节一致性验证，应保留这条文件链。
|
||||
|
||||
## 修改前的离线实验
|
||||
|
||||
没有改动生产算法、现场硬件、原始采样或原始 URDF。以 `20260915_150015` 保存的原始
|
||||
角点进行诊断：前三轮抽取 480 个 Tag 观测，第四轮 160 个观测仅用于冻结后的检查。
|
||||
直接按源 URDF 的运动链拟合一个共同基座、三个允许修改的零位和每个 Tag 的固定安装；
|
||||
另一个实验允许带固定基准的双向曲线修正。两个实验均使用记录时的相机参数。
|
||||
|
||||
- 固定曲线：39 个参数，32 次迭代收敛，约 0.60 秒。
|
||||
- 联合曲线：119 个参数，23 次迭代收敛，约 1.90 秒，曲线最大修正约 1.38°。
|
||||
- 带 0.05 px 噪声的已知真值实验，三个初值均收敛，三个零位最大误差约 0.023°。
|
||||
- 实采第四轮仍有不合格残差；小指末节最大约 4.15 px。上述实验不是实机通过证据。
|
||||
- 使用后来核验的相机参数进行的私有反事实诊断也没有消除全部残差，不能将旧数据改挂新参数发布。
|
||||
|
||||
脚本、参数、雅可比和报告位于
|
||||
`calibration_output/O6_RIGHT_001/20260915_150015/review/architecture_assessment/`。
|
||||
原型只证明统一参数空间可以计算、在所测合成条件下可恢复；没有证明全型号实机精度。
|
||||
|
||||
## 采用的边界
|
||||
|
||||
- 采集保存原始角点、相机身份、SDK 指令/反馈、方向、轮次与基准图像；固定基准漂移、
|
||||
SDK 故障、时间同步和必要采样覆盖检查仍然有效。
|
||||
- 稳定基准图像先冻结，三维解释后求解；未求解的图像不得标成已测准的角度或零位。
|
||||
- 训练只使用前三轮；第四轮与独立稳态验证不参与几何、安装、曲线或候选的选择。
|
||||
- 图像拟合使用与输出 URDF 一致的拓扑和可修改参数；不自动改变骨长、转轴、相机参数，
|
||||
不新增 CAD 零位假设。显式复制关节继续保留来源声明。
|
||||
- 像素残差小不是角度/位置精度合格的充分条件。原物理门限、参数可确定性和最终文件读取验证需保留。
|
||||
- 模型尚未通过回归与实测验收前，不凭局部收敛结果替换正式发布逻辑，不降低门限以生成 PASS。
|
||||
- 采集完成后的拟合失败应保留完整数据并支持离线重算，区别于运动中暂停。
|
||||
|
||||
## 第二轮评估：不采用未经验证的整体替换
|
||||
|
||||
对同一会话进一步测试直接从原始角点初始化、共同拟合 33 节点双向单调曲线的版本。
|
||||
共 359 个参数、6160 个训练残差，150 次迭代、43.39 秒后仍未收敛；拇指末节
|
||||
第四轮最大误差约 45.18 px，小指末节约 7.77 px。这个结果说明，前一个实验依赖
|
||||
既有拟合初值的局部收敛，不足以证明新流程具备独立初始化和实机鲁棒性。
|
||||
|
||||
因此该原型**未接入生产**，相关采集/基准/拟合草稿已移出程序源码，连同源代码快照保存到
|
||||
上述评估目录的 `unadopted_prototype/` 和 `prototype_source_snapshot.tar.gz`。
|
||||
`monotonic_urdf_image_replay.json` 保留失败数据。下一步不能通过增加迭代次数、减小残差权重
|
||||
或放宽门限将其直接上线。
|
||||
|
||||
经过四型号 FK 对照测试的投影基础 `core/fitting/urdf_image.py` 和对应测试也归档到
|
||||
`unadopted_prototype/`，便于复查。生产源码不保留未接入的备用算法。
|
||||
|
||||
## 本次实际接入的流程修改
|
||||
|
||||
1. 四轮及稳态采集完成、数据同步落盘后，显式关闭图像采集，撤销尚未返回的图像回调。
|
||||
拟合使用已经保存的图像与相机参数；此后相机消息/Tag 位置变化不再改变这份输入或触发
|
||||
采集中暂停。运动采集期间的固定基准漂移检查保持生效。
|
||||
2. 反馈接收、SDK 故障保护和用户中止在后台拟合期间持续生效。
|
||||
3. 数值拟合、文件验收和后台阶段协议错误统一结束为 `FAILED`，保存失败阶段、原始数据路径、
|
||||
完成/计划采集单元数和异常堆栈；不再写成采集中 `PAUSED`。
|
||||
4. 状态输出始终提供原始数据路径，拟合失败提供诊断路径，提示先离线复算。
|
||||
失败结果不会填写已通过的 JSON/URDF 路径，也不会被迟到观测或清理用中止覆盖。
|
||||
5. 修复反馈曲线整链回放中丢失非活动关节到达方向的问题。比如下游关节正在弯曲，
|
||||
上游关节保持不动时，上游仍应使用其到达方向对应的回差曲线，不能自动改用双向均值。
|
||||
反馈回放和稳态指令回放现在使用同一个方向解析函数；记录了不完整或非法方向时明确拒绝。
|
||||
独立两轴合成数据证明：旧处理会使固定 Tag 看起来有超过 4° 的安装误差，修复后
|
||||
训练和第四轮最终 URDF 回放恢复真值。当前旧 O6 数据的对应影响小于 0.03°，不是
|
||||
该次实采大误差的主要原因。数值见 `held_joint_direction_effect.json`。
|
||||
6. 用户确认 O6 顶部 ID7 与正面 ID2 固定在同一末节后，明确设置其安装连杆为
|
||||
`rh_thumb_distal`，不再从侧摆任务名称推断为侧摆根部连杆。Profile 哈希同步更新；
|
||||
旧会话的配置身份保持原样。独立合成场景中的 O6 安装真值同步修正，其他型号不变。
|
||||
单独侧摆且弯曲角固定时，错误连杆可以被固定安装矩阵吸收，因此不能声称该错误
|
||||
一定解释先前全部残差；必须在正确绑定及完整关节状态下继续验证。
|
||||
|
||||
这组流程修改解决采集完成后继续受实时视觉影响和失败状态误导的问题；准备段的几何
|
||||
不一致另按下面的评估处理。保持原文件生成与发布验收链路。
|
||||
|
||||
## 第三轮评估：使用输出 URDF 已保留的几何约束
|
||||
|
||||
源 URDF 中相邻平行转轴的平行关系及垂直轴距，在任意关节零位修正下均不变。
|
||||
旧准备模型却允许这些量自由变化，求出的几何无法由最终保留骨长和轴向的 URDF 表达。
|
||||
因此只对真实 Tag 安装连杆、观测拓扑和源 URDF 均满足条件的相邻平行轴，消去两个
|
||||
自由轴倾角和一个自由轴距参数。相机、Tag 安装与逐帧角度仍由图像估计；不新增
|
||||
CAD 零位约定、不使用 SDK 角度作为拟合先验、不引入额外惩罚权重。
|
||||
|
||||
适用性由共同编译器判断，没有 O6 专用分支；不满足条件时保留原图像模型。
|
||||
冻结模型记录新的策略版本、约束及源 URDF 哈希，最终处理前从源文件重新推导验证。
|
||||
旧模型的序列化字节结构保持不变，避免破坏旧证据哈希。内存记录与磁盘记录均保留会话头。
|
||||
断点策略升级为 `confirmed_branch_v6_source_geometry`；旧自由几何会话仍可离线读取，
|
||||
不能作为新约束流程的断点混入新采集。
|
||||
|
||||
对 `20260915_160040` 保存的小指准备段 104 帧原始角点：
|
||||
|
||||
- 原自由模型返回 `image_motion_families_not_distinguishable`。
|
||||
- 使用源平行关系与 36.9995936 mm 轴距后，四个候选均在原 150 次迭代上限内收敛。
|
||||
- 最优两个候选收敛到相同完整 Tag 姿态，其他候选仍用原统计检验排除。
|
||||
- 最优候选留出帧最大误差约 0.696 px，低于原 1.5 px 门限；正式求解器约 4.69 秒返回成功。
|
||||
- 原像素、角度/轴心不确定性、候选等价性及显著性门限均未放宽。
|
||||
|
||||
独立合成图像验证同向及反向平行轴的真值恢复；四型号源几何验证约束在独立零位变化下
|
||||
仍成立。这证明该准备段可以使用与输出一致的模型解算，**尚不等于新四轮整手实机通过**。
|
||||
结果保存在 `20260915_160040/review/cad_geometry_resolution.json` 及相邻诊断文件。
|
||||
|
||||
随后经实际 `BranchInitialization` 入口回放该会话三个准备段的原始图像，拇指侧摆
|
||||
98 帧、拇指弯曲 88 帧、小指 104 帧均通过，耗时约 0.45 / 3.78 / 4.94 秒。
|
||||
该回放使用用户已确认的当前 Tag 绑定，仅保存诊断；没有改写旧会话或发布新标定。
|
||||
见 `all_preparations_source_geometry.json` 和同目录可复现脚本。
|
||||
|
||||
## 验证顺序
|
||||
|
||||
1. 受约束图像模型的已知真值、多初值、旧证据兼容和错误几何反例。
|
||||
2. 四型号共同采集逻辑与数据身份；反馈/固定基准保护及中止持续生效。
|
||||
3. 前三轮训练身份与第四轮隔离；完整性、重复图像、篡改与旧会话身份检查。
|
||||
4. JSON 落盘后独立重建 URDF、标准 ROS 加载及最终文件验收。
|
||||
5. O6 当前相机配置下重新实采；只有通过实际验收才更新发布结果。
|
||||
|
||||
## 本轮验证结果及实机阻塞
|
||||
|
||||
- 最终几何、真实图像入口、采集边界、来源身份、JSON 独立重建回归：87 项通过（39.81 秒）。
|
||||
- 后台进程、诊断采集、四型号产物与 launch 相关集成检查：80 项通过（157.08 秒）。
|
||||
- 新旧断点策略、恢复与原始图像凭据检查：92 项通过（4.85 秒）。
|
||||
各组有交叉，不将数量相加作为独立测试总数。
|
||||
- 已知真值覆盖两/三关节链和正/反向平行轴；故意把 37 mm 轴距改为 80 mm 时，
|
||||
求解器因原重投影门限不合格而拒绝，不能仅凭源几何声明强行通过。
|
||||
- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。
|
||||
- 用户确认现场为 O6 后，清理了仍订阅同一控制话题的 O30 残留 SDK/GUI。
|
||||
实际执行新的 `calibrate_hand --no-resume` 命令时，启动检查报告 `CAN interface does not exist: can0`。
|
||||
系统仅有 lo/eno1/wlo1；采集未开始、未发送电机指令、没有发布本轮新标定。
|
||||
需恢复 O6 原通信适配器后继续四轮实测；当前不能宣称整手实机精度或全部型号实机通过。
|
||||
启动报告保存在上述 review 目录的 `o6_cad_geometry_startup.json` 和日志中。
|
||||
|
||||
## 第四轮评估：有限恢复、精简停点与最终原始角点验收
|
||||
|
||||
本次优化以保留四轮连续扫描、独立验证集和原精度门限为前提。没有采用直接用运动中
|
||||
角度代替静态指令映射的办法，因为反馈滞后和方向回差会使两种角度不同。
|
||||
|
||||
### 准备段恢复
|
||||
|
||||
`runtime/zero_recovery.py` 为每组尚未冻结零位的关节提供一次局部恢复额度。
|
||||
图像数量不足或候选暂时不确定时，复用该组原归零路径补采；几何已确认而零位图像
|
||||
不足时，仅保持当前姿态重新打开图像窗口。恢复撤销旧观测回调，保留真实日志；
|
||||
不替换已冻结几何/零位、不改变保持条件，不将设备故障当成视觉缺样处理。
|
||||
两种恢复共享一次额度，持续失败仍暂停，不无限重做整场四轮运动。
|
||||
|
||||
### 训练停点精简
|
||||
|
||||
`core/fitting/command_sampling.py` 只读取前三轮连续数据，在原静态训练网格上求最少
|
||||
保留节点，使每关节、每方向在原网格节点的插值变化不超过 0.25°。端点、baseline、
|
||||
中间支撑点和显式非线性补充节点必须保留；训练不足或单调修正过大时保留完整网格。
|
||||
该准则决定采样位置，不证明实际静态误差已经达标。每个保留点仍需采集新稳态图像；
|
||||
原独立稳态验证点全部保留，只有这些验证与最终产物验收通过才可发布。
|
||||
|
||||
计划记录训练来源及自身哈希，采集、恢复、离线回放和拟合共同消费同一声明，禁止
|
||||
使用第四轮或稳态验证点反过来选择训练节点。断点策略升级为
|
||||
`confirmed_branch_v7_final_image_sampling`，旧数据不会混入新流程冒充新证据。
|
||||
|
||||
另一项优化只省去重复准备等待：上一段已经完成到位检查、下一目标的完整指令向量
|
||||
相同、一秒内反馈未变化时复用该结果；真实位移、反馈变化、记录过期或归零准备段
|
||||
仍执行原运动检查。原运动速度、避让和采样稳定性门限保持不变。
|
||||
|
||||
对 O6 `20260915_150015` 的已保存数据离线比较，稳态训练点由每方向 9/9/10 个变为
|
||||
7/6/10 个；独立验证仍为 10/10/11 个，双向总停点由 118 次降至 108 次。
|
||||
小指因训练曲线噪声保留原网格，不以强行减点换取精度风险。
|
||||
原方案和精简方案均通过原独立稳态角度验证，全部关节最大误差仍小于 1.69°。
|
||||
这是旧数据的训练点选择对照，不能当作新四轮实机或最终空间精度通过。
|
||||
原始文件哈希保持不变，脚本及结果为该会话的
|
||||
`review/assess_static_sampling.py`、`review/static_sampling_assessment.json`。
|
||||
|
||||
### 最终文件到原始图像
|
||||
|
||||
`runtime/artifacts/image_evidence.py` 从原始记录绑定图像身份、四角、Tag 尺寸以及
|
||||
受哈希保护的相机外参和 CameraInfo。`core/urdf/image_acceptance.py` 读取最终文件，
|
||||
使用冻结的安装和基座，把 JSON＋URDF 的 FK 直接投影到第四轮及独立稳态验证四角。
|
||||
这里不调用 PnP、优化器或重新配准；同一最终产物还须通过原来的 3D 和角度门限。
|
||||
每帧四角重投影 RMS 沿用 1.5 px 门限，超限保存 `final_image_diagnostics.json`。
|
||||
原始证据缺失、相机身份变化、训练/验证重叠及重复图像都会被拒绝。
|
||||
|
||||
新软件的自动测试与离线数据检查不代表四型号已经逐一实测通过。
|
||||
O6 仍需恢复 `can0` 后运行新的完整四轮，并通过上述最终文件验收。
|
||||
|
||||
本次新增链路的验证结果：
|
||||
|
||||
- 四型号精简采样策略及实际 JSON→URDF→角点验收共 11 项通过（96.41 秒），
|
||||
包含静态验证异常仍被拒绝、第四轮不影响训练点选择等反例。
|
||||
- 局部恢复、真实定时器流程和重复等待边界共 24 项通过（21.62 秒）。
|
||||
持续缺少第十张零位图像在一次补采后仍停止，不能少采冒充合格。
|
||||
- 最终角点、真实相机来源、混合实测/复制产物及后台进程共 23 项通过（25.13 秒）。
|
||||
- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过（1.81 秒）。
|
||||
- 构建后再次执行 O6 正式 `calibrate_hand --no-resume`，仍在配置检查阶段报
|
||||
`CAN interface does not exist: can0`，没有进入运动采集；日志保存为
|
||||
`20260915_160040/review/o6_optimized_flow_startup.log`。
|
||||
|
||||
这些组存在覆盖交叉，不相加作为独立总数；四型号整链使用独立构造的合成观测，
|
||||
真实来源绑定另用 O6 已保存角点检查，两者均不能冒充新实机结果。
|
||||
|
||||
全包回归中同步修正两处旧测试约定：零位不足的用例须观察到唯一一次恢复结束，
|
||||
而非要求第一次超时立即暂停；历史侧面准备图像须调用带源 URDF 约束的当前生产入口，
|
||||
不能继续要求旧自由几何模型必然可解。针对这两处的 7 项复测通过（11.49 秒）。
|
||||
同一份侧面历史数据 121 帧使用当前入口通过，训练/验证仍为互不重叠的 61/60 帧，
|
||||
候选统计门限与绝对像素门限保持不变；没有为了通过测试修改原始数据或生产门限。
|
||||
|
||||
全包首次运行共 1497 项：1494 项通过、上述 2 项旧测试断言失败，另有 1 项因未提供
|
||||
O12 本地实采与独立参考文件而跳过（1175.15 秒）。两处失败修正后的 7 项复测通过，
|
||||
没有剩余已知失败。完整日志、专项回归和构建日志保存在
|
||||
`20260915_160040/review/common_flow_validation/`；不将跳过的 O12 实采检查视为通过。
|
||||
|
||||
## 第五轮评估：训练选择与准备验证的边界
|
||||
|
||||
2026-09-15 19:32 使用新相机外参进行正式 O6 采集，拇指两任务完整采完，
|
||||
小指准备阶段两次均报 `image_motion_families_not_distinguishable`。原始数据与修改前
|
||||
评估保存在 `20260915_193254/review/solver_assessment/`；先完成诊断实验，再修改生产代码。
|
||||
|
||||
整体链路保留：准备几何 → 冻结零位 → 四轮连续扫描 → 必要稳态映射 → JSON → 回读生成
|
||||
URDF → 独立图像验收。本次问题位于准备模型的选择和验证，不涉及末端非线性存储格式。
|
||||
独立稠密 Jacobian 求解证实增加迭代无效；四个初值有三个收敛到同一解，剩余解与其
|
||||
相差约 19°、9 mm，不能把真实空间差异当作浮点误差。
|
||||
|
||||
审查发现原实现按验证误差选择优胜模型，再用同一验证集证明它较好；拟合使用平方误差，
|
||||
统计比较却使用 RMS 差；逐个立即返回还保留了最保守的固定多重比较惩罚。
|
||||
现统一为训练选择、验证否决，候选所用的训练资格不受验证结果或验证异常影响；
|
||||
验证失败不得改选。公共 `image_model_selection.py` 按同一平方损失比较，完成全部候选
|
||||
比较后使用 Holm 校正，原始与校正后的概率均写入诊断。
|
||||
|
||||
1.5 px、0.03 px、1°/1 mm 等价界、原角度和空间验收门限均保留，名义族错误水平仍为 0.01。
|
||||
同帧等价检查按角色名对齐，并拒绝缺失或重复角色；不再依靠 `zip` 恰好有相同顺序。
|
||||
姿态策略升级为 `confirmed_branch_v8_training_model_selection`，旧会话仅供诊断，不能
|
||||
自动续接为新策略采集。新增策略适用于公共流程，不增加 O6 专用回退或停点。
|
||||
|
||||
修改前的候选策略诊断中，两次失败数据均可区分（概率约 0.00373、0.00576），拇指保持通过，
|
||||
真正歧义的合成数据保持拒绝；这些仅是修复依据，不代表整手新实机验收完成。
|
||||
统计假设和条件性边界参考 [SciPy Wilcoxon 文档](https://docs.scipy.org/doc/scipy/reference/generated/scipy.stats.wilcoxon.html)，
|
||||
多重比较方法参考 [Holm 方法文档](https://www.statsmodels.org/stable/generated/statsmodels.stats.multitest.multipletests.html)。
|
||||
|
||||
新策略实机 `20260915_195619` 完整采集 36/36 个方向单元，0 次暂停、0 次局部补采；
|
||||
小指准备一次通过。146 项准备求解与产物链专项检查通过。最终空间验收仍失败，
|
||||
候选 JSON/URDF 未发布；这与采集是否完整是两个不同结论。
|
||||
|
||||
## 第六轮评估：先修复测量时间，再判断空间求解
|
||||
|
||||
该会话的原始角点、生产重放和参数对照保存在 `20260915_195619/review/spatial_assessment/`。
|
||||
轴线投影开关没有改善整体误差；共同 URDF 的 39 参数图像求解虽然收敛，第四轮仍有
|
||||
4.124 px 误差，因此没有采用这一替换。相同反馈和相同方向的第四轮图像还能构成
|
||||
与任何确定性查找表不相容的反例：两帧像素距离 3.753 px，最大误差必有一帧不低于
|
||||
1.877 px，超过现有 1.5 px 门限。该反例不依赖拟合算法，也不证明硬件损坏。
|
||||
|
||||
SDK 代码确认了一个时间来源错误：发布线程给低频轮询缓存反复填上当前时间。
|
||||
相机与发布消息的时间差很小，并不能证明相机与真实反馈同步。修复位于共同数据边界：
|
||||
|
||||
- CAN 接收端原子保存位置和接收时间，O6/L6 单帧、G20 按各指分帧保留。
|
||||
- SDK 的标准状态保留真实接收时间；标定专用接口携带每通道时间，仅发布新增测量快照。
|
||||
- 标定按每通道实际时间插值。重复、乱序、过期或同一接收身份对应不同值的记录不能刷新
|
||||
稳态样本或反馈新鲜度；原始反馈快照落盘，便于复核。
|
||||
- 共用采集策略升级为 `unified_engine_v6_measured_feedback_time`，四型号 Profile 同步声明。
|
||||
源 URDF、相机和精度门限保持原样；旧采集不能改挂新时间策略发布。
|
||||
|
||||
时间契约、缓存、分帧、回归与稳态检查 150 项通过；四型号配置、ROS 接入、断点与产物链
|
||||
集成检查 69 项通过；SDK 与标定两包构建通过。不运动的实机接口核验中，486 条标定反馈
|
||||
全部匹配独立旁路收到的原始 CAN 帧，0 条无来源、0 条重复身份。真实测量频率约 25 Hz，
|
||||
同一时段标准接口发布 1147 条消息，说明之前约 58 Hz 的发布频率不能作为测量频率。
|
||||
该核验启动 SDK，SDK 仍执行其初始化速度/力矩设置；没有发送位置运动命令。
|
||||
完整实机产物仍须用新的测量时间重新采集后验收。
|
||||
|
||||
|
||||
## 第七轮评估：控制消息顺序处理，图像任务独立执行
|
||||
|
||||
真实时间采集 `20260915_202203` 暴露了图像缺样：一段约 0.30 秒的有效图像空白
|
||||
内仍有 15 条真实 CAN 测量，不应扩大同步窗口来掩盖。最初加入每视角有界图像工作线程，
|
||||
但 ROS 端仍保留多线程执行器；`20260915_203104` 恢复导入期间仍因反馈超过一秒暂停。
|
||||
这项中间实现没有完成目标，不能算作修复成功。
|
||||
|
||||
进一步评估保存在 `20260915_203104/review/dispatch_profiling/`：真实 64 条记录编码
|
||||
约 6 ms，不支持直接减小导入条数。实机锁计时和线程栈显示，多个 ROS 回调争用同一把
|
||||
会话锁，图像队列还在持有自己的锁时反向查询会话。仅在诊断入口替换为单线程 ROS
|
||||
执行器后，反馈进入回调的延迟由初段中位约 113 ms、P95 247 ms，降到约 9 ms、17 ms；
|
||||
同一精度、采样和时间策略下已成功复用 8 个合格单元。
|
||||
|
||||
据此整理公共 ROS 接入，而不更换拟合器或增加型号特例：
|
||||
|
||||
- 控制、反馈、相机消息解析和状态更新由一个 ROS 执行器顺序处理，删除冗余回调组。
|
||||
- 每个视角只保留一项执行中的图像任务和一项待处理任务，过载时保留最新待处理图像。
|
||||
- 只有 coordinator 接受的新测量才能推进图像队列的时间水位；多帧协议使用最旧通道时间。
|
||||
队列持锁时不调用 coordinator，开始新会话时清空旧水位和未执行图像。
|
||||
- 原 50 ms 同步界限继续有效；图像估计仍在锁外，提交结果仍检查会话和运动版本。
|
||||
- 图像线程异常传回控制入口，不将线程停止当成正常无图；检查点落盘规则保持原样。
|
||||
|
||||
调度、ROS 接入、真实测量时间、coordinator 和断点恢复共 51 项针对性检查通过；
|
||||
图像元数据并发、过期结果提交和隔离计算共 28 项检查通过；标定包构建通过。
|
||||
单线程对照运行及后续正式运行的终态以各会话记录为准；这里的延迟改善不能代替
|
||||
四轮完整性、独立角点和 JSON＋URDF 的最终空间验收。
|
||||
|
||||
|
||||
## 第八轮评估：相机曝光时间与取帧时间分离
|
||||
|
||||
单线程对照会话 `20260915_203750` 完成 36/36 单元（复用 8 个已验证单元）、0 次暂停，
|
||||
生成候选 JSON 和由该 JSON 重建的 URDF，但顶部 ID7 的训练安装位置 P95 为 18.285 mm，
|
||||
超过 3 mm，产物未发布。其余四个实测 Tag 角色通过该项空间检查。共同 URDF 的 39 参数
|
||||
原始角点拟合虽然收敛，第四轮仍达 4.779 px，因而没有替换生产拟合器。
|
||||
|
||||
随后检查图像时间来源，发现相机驱动在 `MV_CC_GetImageBuffer` 返回后使用当前 ROS
|
||||
时间，不能表示曝光时刻。实测三台相机的设备时钟、40 次独立时钟锁存，以及曝光开始/
|
||||
结束事件，确认：
|
||||
|
||||
- 三台取帧时间分别比设备帧时间晚约 16.44 / 16.52 / 16.49 ms；不应靠拟合器吸收该延迟。
|
||||
- 帧计数时间与独立曝光开始事件相差约 150 ns；固定曝光时可使用曝光中点时间配对。
|
||||
- 当前固件的 `DeviceTimestampIncrement` 返回 100,000,000，实测约 10 ns/tick。
|
||||
设备 XML 的单位说明与此不一致，因此程序启动必须用独立经过时间核验计数单位。
|
||||
|
||||
`camera_timing.py` 单独负责设备/主机时钟对应和曝光时间换算。每 0.5 秒重新锁存，
|
||||
选择往返区间最短的有效测量；偶发缓慢事务丢弃后有界重试，不能刷新原两秒有效期。
|
||||
错误单位、时钟跳变、计数器重置、重复帧和过期对应均不能伪装成新图像。
|
||||
相机驱动在图像及 CameraInfo 中使用同一曝光中点时间，保留每次锁存和每帧换算记录
|
||||
`camera_timing_<view>.jsonl`。固定外参和内参不因软件时间修正而改变。
|
||||
|
||||
共用采集策略升级为 `unified_engine_v7_camera_device_time`，四型号 Profile 及其受保护
|
||||
哈希同步更新；旧会话保留诊断用途，不能直接续接到新时间规则。改动没有增加停点、
|
||||
调整非线性查找表语义或放宽最终精度门限。
|
||||
|
||||
时钟、真实三相机记录、ROS 接入、配置、启动图和断点/队列共 57 项检查通过；构建通过。
|
||||
随后实际驱动对三台相机各连续发布 242 帧、执行 16 次校时，无时序或时钟错误。
|
||||
O6 运动期间另旁路监听 CAN：370 条位置命令的响应均不是原命令回显，仍可作为真实状态
|
||||
观测；没有把 L6 的回显过滤规则未经核实套到 O6。
|
||||
|
||||
完整新策略实采会话为 `20260915_205704`，不复用断点、不使用调度诊断包装。
|
||||
共完成 36/36 个单元，0 次人工暂停；拇指弯曲第四轮增加方向和小指第三轮增加方向
|
||||
各自动重扫一次。候选 JSON/URDF 已生成，Tag 安装刚性训练检查失败，未发布 manifest。
|
||||
正式失败时位置 P95：小指末节 3.108 mm、拇指末节 6.476 mm、拇指弯曲 3.875 mm、
|
||||
顶部拇指 10.086 mm。会话中 `review/spatial_assessment/live_run_summary.json` 保留统计。
|
||||
|
||||
## 第九轮评估：反馈插值契约与空间模型边界
|
||||
|
||||
反馈拟合使用曝光时刻的连续插值值，报告声明 `piecewise_linear`；原 `JointMapping`
|
||||
和 `SerializedJointMapping` 却对所有 `_u8` 输入再次取整，使训练和读取语义不同。
|
||||
统一规则为：现代反馈曲线连续插值，实际 `command_u8` 量化，历史整数表保持原兼容行为。
|
||||
序列化读取在量化前检查输入支持域，不能将越界小数取整后当成合法测量。
|
||||
此修改不改变非线性曲线本身、不重拟合零位、不调整采集版本或原始记录。
|
||||
|
||||
先用相同 39 参数整链诊断做对照：保留原 URDF 骨长、轴向和已拟合曲线，前三轮拟合
|
||||
共同基座、允许的零位及固定安装；独立第四轮不参与优化。取消反馈取整后，小指末节
|
||||
最大像素误差由 3.390 降至 2.449 px，但仍超过原 1.5 px，因此该诊断模型未接入生产。
|
||||
另加有界双向曲线修正的 119 参数实验，第四轮小指末节最大仍为 3.136 px，且出现
|
||||
非单调局部曲线；增加参数并没有提供足以替换现有生产求解器的证据。
|
||||
|
||||
正式公共回放也验证：反馈取整修复不能消除空间不一致。位置 P95 为小指末节 3.080 mm、
|
||||
拇指末节 6.489 mm、拇指弯曲 3.877 mm、顶部拇指 10.088 mm；保持失败且未发布。
|
||||
原始会话与在线候选文件未覆盖；新回放位于 `20260915_205704/review/spatial_assessment/`
|
||||
下的 `continuous_production_replay/`。同目录保留诊断脚本、参数、Jacobian 和测试日志。
|
||||
|
||||
补充整链可行性对照仍只使用前三轮、相同 39 个参数：将训练目标换成按原 2°/3 mm
|
||||
尺度归一化的三维位姿残差，局部收敛后拇指弯曲零位触及原 20° 边界；第四轮顶部
|
||||
位置 P95 仍为 5.148 mm，原始图像最大误差增至 28.718 px。此结果没有证明全局无解，
|
||||
但否定了将这一局部三维优化结果直接用于替换现有求解器的做法。
|
||||
原始保存位姿经受保护的相机变换投回同帧角点，各角色最大误差均小于 0.64 px；
|
||||
没有发现这条原始公共坐标变换的方向或图像身份错配。检查见
|
||||
`continuous_spatial.json`、`common_pose_projection_check.json`。
|
||||
|
||||
反馈输入、双映射、被动非线性与读取兼容检查 57 项通过；补充真实时间插值、四型号
|
||||
合成产物链、JSON 重建、独立角点和标准 URDF 检查 49 项通过，两组有交叉。
|
||||
SDK 和标定包最终构建通过，`git diff --check` 通过。
|
||||
合成验证不等于其他型号实机通过；本轮 O6 的最终空间精度问题仍未解决。
|
||||
|
||||
## 第十轮评估：保留跨运动的原始图像证据
|
||||
|
||||
此前原始角点随任务主机位的姿态采集写入，缺少顶部 ID7 在弯曲中、正面 ID2 在侧摆中
|
||||
的完整原始图像。这两块 Tag 已确认属于同一末节；只保存各自任务下的图像不足以独立
|
||||
核查共享运动链。公共采集器现独立保存所有机位的有效原始角点，保留原检测质量、
|
||||
窗口与会话身份，不依赖 PnP 是否成功。主采样门限、运动计划和稳态点数量均不变。
|
||||
同机位图像时间去重只在成功提交时推进；迟到或窗口已撤销的图像不能推进水位。
|
||||
|
||||
采集策略升级为 `unified_engine_v8_all_view_images`，四型号 Profile 及产品哈希同步更新。
|
||||
旧会话不修改哈希、不补造新策略记录。新增采集与公共并发检查 43 项通过，补充采集、
|
||||
四型号配置与断点身份检查 58 项通过，两组有交叉；标定包构建通过。
|
||||
|
||||
新会话 `20260915_213337` 完成 36/36，0 次人工暂停；小指第二轮反向扫描自动重试一次。
|
||||
三机位共保存 16,822 个原始图像记录。正式算法仍在 Tag 安装空间一致性处失败，
|
||||
未发布 manifest。主动零位与上一会话差均小于 0.03°，不能以重复性替代正确性。
|
||||
|
||||
保持当前源 URDF、相机参数和 Tag 尺寸，前三轮拟合共享基座、零位和每块 Tag 的固定安装，
|
||||
第四轮检查的新全视角对照仍未通过。39 参数固定曲线模型的最大像素误差约 3.52 px；
|
||||
359 参数双向单调曲线模型最大约 4.32 px。后者按 SDK 输入均匀选择训练图像，
|
||||
不再按停留时间重复加权；非负曲线增量和参考输入零约束仍不足以消除残差。
|
||||
|
||||
静止姿态也出现约 1%～3% 的投影边长偏差，因此另做一次受限的统一骨长尺度诊断。
|
||||
其训练最优尺度约 1.0327，第四轮最大误差仍约 4.37 px；没有采用该尺度，没有修改
|
||||
源 URDF 或相机文件。原始公共位姿可重投回其源角点，暂未发现转换方向错误。
|
||||
上述原型和数值结果均仅保存在 `20260915_213337/review/whole_chain/`，未接入生产求解器。
|
||||
|
||||
仅用稳态共同视野核查 ID2/ID7 刚体关系时，训练 6 个姿态、留出 7 个姿态全为侧摆，
|
||||
不能独立确定完整外参。连续扫描保存了两种动作的共同视野，可在原曝光时间上对齐后
|
||||
进行独立于 URDF 和曲线的外参一致性诊断；诊断结果不能自动覆盖已记录的相机参数。
|
||||
|
||||
## 第十一轮评估：尺度一致性与全机位最终图像验收
|
||||
|
||||
对 ID2/ID7 同末节约束做独立检查，不使用 URDF 尺寸、轴向或 SDK 查找表。
|
||||
连续图像按曝光时间对齐，线性/局部三次及留一插值的一致性误差不超过 0.1 px；
|
||||
训练前三轮 96 个姿态，第四轮 32 个姿态仅检查冻结的参数。每帧刚体位姿作为局部未知量，
|
||||
两个 Tag 之间始终只有一个固定安装变换。多初值只按训练平方误差选择。
|
||||
|
||||
| 诊断假设 | 第四轮 front/top 最大误差 | 结论 |
|
||||
| --- | --- | --- |
|
||||
| 记录的外参与两个 16 mm Tag | 1.28 / 1.69 px | 与原始图像仍有不一致。 |
|
||||
| 自由估计 front/top 外参，Tag 均 16 mm | 0.44 / 0.48 px | 外参改变量约 2.43°/23.72 mm；条件三倍标准差约 1.96°/22.24 mm，不能作为精确外参替换。 |
|
||||
| 记录外参，单独估计 ID7 尺度 | 0.84 / 0.49 px | 固定 ID2 为 16 mm 时 ID7 约 16.52 mm；不是两块 Tag 实测尺寸差的独立结论。 |
|
||||
| 记录外参，同时估计两个 Tag 尺度 | 0.42 / 0.52 px | 有效边长 ID2 约 16.38 mm、ID7 约 16.52 mm；依赖现有相机模型和刚体/正方形假设，尚不能替代物理尺寸。 |
|
||||
|
||||
自由外参不能解释此前保存的棋盘格图像：其最大误差约 12.93 px，原外参约 0.56 px。
|
||||
因此不能因为手指图像误差变小就认定相机移动或自动重写外参。只读采集当前静止画面后，
|
||||
正面背景匹配显示约 3 px 变化，侧面约 1 px 内；顶部旧图被棋盘格遮满，没有足够共同背景，
|
||||
不能判断顶部机位是否变化。该检查没有启动 SDK 或驱动手指。
|
||||
|
||||
另将整链中的所有相机基线或所有 Tag 尺寸分别只增加一个公共尺度参数：前三轮最优值
|
||||
约为 0.97226 和 1.03073。两者都明显降低拇指正面误差，但第四轮小指末节最大仍约
|
||||
4.29/4.27 px、顶部最大仍约 2.66/2.44 px；没有采用任何尺度修正。
|
||||
条件协方差未包含相机、打印尺寸和模型的系统误差，不能用其很小的数值宣称物理尺寸测准。
|
||||
已请求分别精测 ID2/ID7 黑框宽高及棋盘格跨五格长度，现有整数尺寸记录不足以区分尺度来源。
|
||||
结果在 `review/whole_chain/rigid_*_assessment.json`、`board_camera_comparison.json`、
|
||||
`static_background_comparison.json` 和 `all_view_*_scale.json`。
|
||||
|
||||
程序结构上，只有任务主机位参与最终图像验收会漏掉同一 Tag 在其他动作下的不一致。
|
||||
现将相机身份核验和原始图像索引集中在 `ImageEvidence`，反馈和稳态指令各自使用独立窗口。
|
||||
前三轮、未通过尝试和无同步反馈的原始图像不能成为最终验收测量；主观测必须在新原始记录
|
||||
中找到完全一致的角点、SDK 值和方向。其他动作中的原始角点也直接进入冻结 JSON/URDF 投影，
|
||||
不伪造 PnP 位姿，不参与训练，不增加运动或停点。历史主图像读取接口保留原证据语义。
|
||||
|
||||
实际 O6 输入核对：反馈原有 1,818 个主观测全部一致，增加 3,270 个其他观测；
|
||||
稳态指令原有 312 个主观测全部一致，增加 677 个观测。此结果仅证明证据绑定正确，
|
||||
不代表最终空间精度通过。专项和现有产物流程 36 项通过，补充最终检查、四型号合成产物、
|
||||
JSON 重建及非有限输入反例 34 项通过；两组有交叉，不相加为独立测试总数。
|
||||
标定包构建通过；实际数据证据核对和测试日志保存在
|
||||
`review/whole_chain/all_view_final_source_verification.json` 与 `final_image_validation/`。
|
||||
|
||||
### 剩余运动残差的解释边界
|
||||
|
||||
固定上述未启用的候选相机尺度与整链几何，仅逐帧调整活动关节角度进行定位。
|
||||
小指两关节最大改变量约 0.63°/0.49°，侧摆约 0.38°；图像最大残差分别可降至
|
||||
0.43/0.94 px 和 0.47 px。最大原始残差集中在 SDK 约 244 的运动区间；
|
||||
同一 Tag 静止、其他手指运动时的误差显著更小。
|
||||
|
||||
这项实验在每个被检查图像上估计局部角度，明确**不是独立第四轮验收**，也不证明
|
||||
其条件几何正确。它提示剩余像素误差中包含运动映射误差，不能据此继续修改骨长或零位。
|
||||
前三轮相同输入位置的角度改变量有约 0.1° 的轮间差，增加曲线节点也未必能消除。
|
||||
后续需分别评估反馈分辨率、允许的运动映射误差与图像测量误差；不能直接把 PnP 的
|
||||
像素测量门限等同于整条运动链的角度/位置精度要求。本轮没有修改任何验收门限。
|
||||
完整逐帧结果见 `review/whole_chain/local_angle_residuals.json`。
|
||||
|
||||
## 第十二轮评估：分离图像测量误差与运动精度
|
||||
|
||||
先用完全独立的已知真值验证规则：单关节运动存在恒定 0.5°、0.873 mm 偏差，
|
||||
满足原角度和位置要求。仅将相机焦距从 1000 改为 3500 px，最终 FK 的最大投影误差
|
||||
就从 1.10 变成 3.84 px；原规则因此对相同物理精度作出不同结论。测量位姿对原始角点
|
||||
的误差均近于零。这证明 PnP 的 1.5 px 图像测量门限被误用于含运动映射误差的总投影量。
|
||||
|
||||
公共验收现在分为以下职责：
|
||||
|
||||
1. 发布器原有的独立三维反馈、指令验收仍先执行，角度 MAE≤1°、P95≤2°、最大≤3°，
|
||||
位置 P95≤3 mm。非线性被动关节仍读取完整 JSON，不以 mimic 近似替代。
|
||||
2. `image_acceptance.py` 将独立位姿与原始角点、SDK 输入、方向和图像身份直接绑定。
|
||||
测量位姿重投影 RMS≤1.5 px；最终 FK 的总投影误差保留为单独统计，不据此改变物理精度要求。
|
||||
3. 额外视角无独立三维测量时，`image_consistency.py` 查找一个能解释原始角点的局部刚性位姿，
|
||||
直接核验其像素残差及相对最终 FK 的物理位移。数值见证仅说明图像与容差相容，
|
||||
**不是真实位姿测量，不证明最优，不替代独立三维精度**。求解器返回成功也不能单独使其通过。
|
||||
不回写或重新估计共享的相机、安装、基座、零位和曲线；按运动任务分别统计，防止静止帧稀释误差。
|
||||
4. manifest 明确记录 `independent_measurement_and_physical_consistency_v2`，
|
||||
独立精度与额外机位相容性分开报告。缺失独立位姿不能由数值见证补齐。
|
||||
|
||||
解析投影 Jacobian 已用独立有限差分检查。已知真值、不同焦距、物理超差、角点/相机篡改、
|
||||
证据身份和静止图像稀释等 42 项专项检查通过；四型号合成产物、非线性、JSON 重建及原物理
|
||||
验收共 36 项通过；最后的规则版本/任务身份补充检查 15 项通过。测试集合有交叉，不相加。
|
||||
标定包构建通过，`git diff --check` 通过。没有增加实机轮次或停点。
|
||||
|
||||
最新真实 O6 的 1,818 个第四轮及 312 个稳态主观测均核验了真实保存位姿与原始角点，
|
||||
最大误差分别为 0.613/0.616 px。此检查没有拟合任何参数，属于测量证据核验，不是产物 PASS。
|
||||
复算原训练安装检查仍得到拇指位置 P95 为 5.371/3.591/11.352 mm，仍失败，没有 manifest。
|
||||
尺度/相机输入尚待现场精测区分，未采用之前任何诊断尺度，也未直接替换整链求解器。
|
||||
已知真值脚本使用保存的旧规则源码复现反例，当前规则由生产测试验证；全部记录位于
|
||||
`20260915_213337/review/whole_chain/image_error_budget/`。
|
||||
|
||||
## 第十三轮评估：轴线参考点不能决定零位
|
||||
|
||||
一条转轴可用轴上任意一点表示；沿轴移动该点不会改变物理几何。当前 O6 的旧零位公式
|
||||
先把两轴参考点的差直接投到图像平面，没有消除沿轴任意坐标。相机斜看转轴时，这个
|
||||
任意坐标被误解释成弯曲零位。旧代码已为 O12 增加可选开关，但 O6/L6/G20 仍使用原公式。
|
||||
|
||||
在真实 O6 的相同轴线输入上，仅将被动轴参考点沿轴移动 ±20 mm：拇指弯曲零位从
|
||||
−9.8736° 变成 +1.0529° 或 −16.1650°，三个结果竟都通过原轴线角度检查。整个实验没有
|
||||
改变相机、Tag 尺寸、运动图像或真实轴线，足以证明程序中存在几何表示相关的错误。
|
||||
|
||||
公共 `ObservationGeometry.phase_error` 现始终先取两轴的垂直间距向量，再投影到相机平面。
|
||||
移除了 `project_axis_gauge_before_image` 型号开关及其编译参数；O12 删除已无必要的 YAML
|
||||
声明并同步产品哈希，其数学行为保持原先已启用的正确规则。O6 的采集配置与受保护输入
|
||||
不变，可以用原四轮数据离线复算，无须增加运动。
|
||||
|
||||
独立测试使用四型号的原始 URDF、自行生成已知 0.07 rad 零位，改变相机视角、共同坐标系，
|
||||
并任意移动预测和观测的轴上参考点；所有平行轴对均恢复相同真值。源码专项、四型号完整
|
||||
合成产物链、配置与 O12 安装/机位变化共 24 项检查通过；源代码的轴线/求解顺序 9 项通过。
|
||||
构建后另核对实际加载的安装包与源码字节一致，安装包的轴线、图像与物理门限检查共
|
||||
51 项通过，避免用旧安装文件检验新修改。各测试集合有交叉，不相加。
|
||||
|
||||
正式回放保留原始输入身份检查，输出到 `review/whole_chain/axis_gauge_production_replay/`。
|
||||
拇指弯曲零位约 −0.5384°、小指约 −1.8044°，不再依赖轴上参考点；侧摆仍约 14.5179°。
|
||||
JSON 已先保存并被读取生成候选 URDF；空间安装检查仍失败，位置 P95 如页首所列，
|
||||
未生成 manifest。这项数学修正没有消除后续空间不一致，也不能替代尚待核实的尺度输入。
|
||||
|
||||
对最近两次已保存的轴线输入使用同一修正公式，侧摆、拇指弯曲、小指零位差分别为
|
||||
0.02955°、0.01044°、0.02942°。这是条件零位的重复性对照，不是绝对精度通过；
|
||||
没有改写旧 v7 会话的采集策略或将其伪装成 v8 发布。结果见 `corrected_zero_repeatability.json`。
|
||||
|
||||
旧公式及配置已备份到 `axis_gauge_before/`，`assess_axis_line_gauge.py` 从备份源码复现
|
||||
历史反例；不将旧公式或型号条件保留在生产求解器中。数值对照见 `axis_line_gauge_assessment.json`，
|
||||
源码、安装包测试及构建日志均在同一 `review/whole_chain/` 目录。
|
||||
|
||||
## 第十四轮评估：独立双目尺度检查
|
||||
|
||||
先使用已冻结的 ID2/ID7 刚体关系，在第四轮 32 个共同观测姿态上仅估计逐帧刚体位姿，
|
||||
不使用 URDF 或 SDK 曲线。保持原相机与两个 16 mm Tag 时，front/top 图像最大误差仍为
|
||||
1.280/1.693 px；与原主机位三维测量的位置 P95 分别相差 26.84/19.09 mm。
|
||||
此前“调整外参”或“调整 Tag 尺寸”的条件模型均能把最大图像误差降至约 0.5 px，
|
||||
但得到不同的三维位置，且部分与原主测量相差约 20～33 mm。因此不能将其中任意一个
|
||||
条件重建当成独立真值，亦不能用小图像残差证明最终空间精度。共享参数均未在第四轮更新。
|
||||
对照见 `stereo_primary_measurement_comparison.json`。
|
||||
|
||||
只读保存的静止画面还发现：侧面相机可以同时看见正面 ID1、ID2。用两机位对同一个 Tag
|
||||
的四个对应角点直接三角测量，不预设 Tag 尺寸、刚体安装、URDF 或 LUT。两帧曝光相差
|
||||
6.55 ms，保存时没有 SDK 控制进程、机械手静止。尺度仅来自记录的 front/side 外参基线。
|
||||
|
||||
- ID1 两组对边平均长度为 **16.496 / 16.503 mm**；两机位重投影 RMS 为 **0.536 / 0.443 px**。
|
||||
这项独立对照进一步提示当前尺度与“16 mm”的记录不一致。
|
||||
- ID2 两组对边约 **16.593 / 16.713 mm**，但重投影 RMS 为 **1.583 / 1.412 px**，
|
||||
最大单角误差约 2.28 px，不能将该尺寸估计作为已通过质量检查的测量。
|
||||
|
||||
此检查仍依赖棋盘格确定的相机基线尺度，不能独立区分打印 Tag 偏大与棋盘格尺度偏差。
|
||||
没有把 ID1 的尺寸复制给其他 Tag，没有更改原相机或尺寸配置，没有增加整手运动。
|
||||
原始 PNG、相机元数据、逐角三维坐标及误差均可复查，见 `current_camera_images/`、
|
||||
`same_tag_stereo_assessment.json` 与相应同名诊断脚本。
|
||||
|
||||
下一项必要现场证据仍是已请求的精确黑框尺寸与棋盘格跨五格长度。现有图像能发现
|
||||
尺度不一致，但不足以无假设地决定应修改哪一项物理输入。尺寸证据到位后，应先核实输入、
|
||||
评估与原始数据的兼容性，再复算整链；不能把某个局部拟合最优值直接写入生产配置。
|
||||
|
||||
## 第十五轮评估：固定已确认尺寸，排除图像坐标处理与局部拟合误导
|
||||
|
||||
2026-09-16 用户再次明确：全部 Tag 黑色码区为 16×16 mm，棋盘格为 27×27 mm。
|
||||
这解除上一轮的尺寸信息等待；本轮按用户确认值作为固定物理输入，不把双目条件估计
|
||||
当作新的 Tag 尺寸。以下诊断记录在 `20260915_213337/review/whole_chain/confirmed_dimensions_audit/`。
|
||||
|
||||
### 1. 原始图像与去畸变图像的数值核验
|
||||
|
||||
仅启动相机，机械手不运动；保存同一曝光时间戳的原始和去畸变 PNG。按 CameraInfo 的
|
||||
K、D、R、P 独立执行 OpenCV remap，front/side/top 三机位与 image_proc 输出的最大、平均
|
||||
灰度差均为 **0**。程序以 P 解释 image_rect、棋盘物点间距为 0.027 m；没有发现单位错误、
|
||||
缩放裁剪、重复去畸变或原始 K 与去畸变图像混用。该证据只核验软件坐标契约，不证明
|
||||
K/D 本身就是准确物理内参。结果见 `raw_rect_consistency.json`、`raw_rect_capture/`。
|
||||
|
||||
新静止图像中 ID1 的两组对边平均为 **16.5057/16.5078 mm**，两机位重投影 RMS 为
|
||||
**0.830/0.687 px**。ID2 的重投影 RMS 为 1.836/1.635 px，继续不作为合格尺度测量。
|
||||
因此上一轮发现的矛盾可再次观测，并非一次检测结果；见 `fresh_same_tag_stereo.json`。
|
||||
|
||||
### 2. 棋盘低像素误差不能单独决定新相机参数
|
||||
|
||||
使用已有原始角点，保持 27 mm 间距，对正面/侧面及正面/顶部训练对分别联合估计内参。
|
||||
两组的留出误差均下降，但同一 front 的主点横坐标分别约 866/1078 px，明显不同。
|
||||
又将单机位训练集分为互不重叠子集：side 的部分子集可给出明显不同焦距/主点，同时
|
||||
在同一留出图像上仍保持约 0.45～0.48 px 的 RMS。不能凭这个像素指标选择物理参数。
|
||||
结果见 `joint_intrinsics.json`、`intrinsic_stability.json`；未修改相机配置。
|
||||
|
||||
进一步以 OpenCV object-releasing 仅在训练图像中估计一个固定棋盘角点形状，保持
|
||||
第一行两端 7×27 mm 间距。三个机位独立估计的形状均存在亚毫米偏差；其中 front、side
|
||||
估出的形状相近。冻结形状后，未参与拟合的单机位图像 RMS 分别从
|
||||
**0.276/0.465/0.317 px** 降为 **0.133/0.168/0.138 px**。
|
||||
这提示理想平面棋盘假设可能影响内参，但仍是条件模型证据，不等于已测准棋盘变形。
|
||||
|
||||
为避免“各相机自由拟合都能变好”的误判，另只采用 front 训练图像估出的同一形状，
|
||||
重新估计 side/top 内参，再用 19:25 棋盘原角点训练新的条件外参（每四组留一组验证）。
|
||||
side 的留出双目 RMS 最大约 0.395/0.383 px；随后检查从未参加相机拟合的新静止 Tag，
|
||||
ID1 两组对边仍为 **16.5038/16.5058 mm**。棋盘误差改善没有解决目标尺度矛盾，故明确
|
||||
拒绝启用这组候选参数。见 `board_shape.json`、`shape_camera_transfer.json`。
|
||||
|
||||
### 当前下一步
|
||||
|
||||
已请求保持手和相机不动，将棋盘置于手旁，使 front/side 同时看到完整棋盘及 ID1，
|
||||
采集同一时段的静止画面。这可直接核查已确认的两种尺度，减少先标棋盘、后拍手之间
|
||||
场景变化的影响。另询问棋盘载体是否刚性平整，作为几何假设的现场证据。
|
||||
本轮不增加整手运动、不启用候选相机参数、不改变 Tag 尺寸或精度门限；原数据和受保护
|
||||
配置保留。整手发布条件仍未达成，无新的合格 manifest。
|
||||
|
||||
### 当日现场反馈与贴平后检查
|
||||
|
||||
用户补充“棋盘没有完全弯曲变形，tag贴纸有弯曲不平整”，随后确认“已贴平固定”。
|
||||
因此不能把之前的平面 Tag 位姿当作已证实准确的物理测量，也不能把棋盘条件拟合
|
||||
直接解释成全部误差的原因。正式复标需使用处理后的新图像，而非沿用旧 Tag 安装结果。
|
||||
|
||||
10:09 的贴平后静止对照，front/side 曝光差为 1.763 ms。ID2 的双目重投影 RMS 为
|
||||
1.349/1.201 px，较处理前改善；ID1 为 2.254/1.872 px，仍不满足图像质量门限。
|
||||
两者的条件尺度仍约 16.5 mm，不能据此宣布测量链修复完成。数据保存在
|
||||
`confirmed_dimensions_audit/flattened_tags_20260916_100921/`。
|
||||
|
||||
用户表示棋盘已摆好后进行了实际拍摄，并持续开启无机械手控制的双目预览；至 10:10
|
||||
两路画面实际均未出现棋盘。已提供正面/侧面画面帮助摆放，没有将空缺的棋盘观测标成
|
||||
成功采集。新的同帧棋盘/Tag 核验脚本已用已知真值验证：正确相机下恢复 16 mm 各边，
|
||||
误差小于 1e−5 mm；该合成检查不是现场相机验证。下一步仍需让棋盘实际进入两个机位，
|
||||
目前未重新启动整手运动或输出新的合格产物。诊断汇总与受保护输入哈希见
|
||||
`confirmed_dimensions_audit/audit_summary.json`。
|
||||
|
||||
## 第十六轮评估：分时核验棋盘与 Tag，取消不必要的同框限制
|
||||
|
||||
固定相机外参时，棋盘负责估计相机之间的刚体变换；Tag 的位置和姿态在检查时独立
|
||||
估计。因此棋盘与机械手不必同时出现在画面中，手在两次拍摄间也不必保持原位置。
|
||||
此前把同框设为必要条件，给现场带来了不必要的摆放限制，现已取消。只需相机及镜头
|
||||
设置在两段采集中保持固定。这一修改属于诊断采集流程，不改变整手正式运动流程。
|
||||
|
||||
10:21～10:22 保存四组正面/侧面完整棋盘原图：
|
||||
`board_placement_preview_20260916_102131`、`102213`、`102215`、`102217`。
|
||||
原始双目曝光差 10.79～10.99 ms；每组以已确认的 27 mm 棋盘估计条件相机变换，
|
||||
棋盘重投影 RMS 为 front 0.281～0.364 px、side 0.438～0.571 px。
|
||||
四次相对首帧的变化最大为 0.0272°、0.458 mm。姿态相近的四组只说明条件重复性，
|
||||
不能充当完整多姿态外参标定，也没有写入活动相机配置。
|
||||
|
||||
临时预览的自动保存器还曾设置 20 ms 配对条件，而一次启动后的相机帧相位差约
|
||||
24.66 ms，造成没有进入检测。这是诊断脚本的问题，已采用原相机核验的 50 ms 配对
|
||||
范围，并保留真实曝光时间；正式图像与空间精度门限不变。另一个临时触发条件要求
|
||||
全部棋盘角点连续 2 秒变化均小于 0.4 px，未触发自动保存。上述四组是另行直接保存的
|
||||
原始画面，经离线图像核验后使用；不能把预览中的 `saved_count=0` 改记成自动触发成功。
|
||||
后续共面检查直接保存可见原图，在原图上评估几何质量，不先用这种触发条件丢弃证据。
|
||||
|
||||
用户移开棋盘、放回 O6 后，10:24 保存新的 Tag 图像，曝光差 11.75 ms，位于
|
||||
`joint_board_tag_20260916_102403/`。分别使用上述四组棋盘估计的相机变换，得到：
|
||||
|
||||
- ID1 两组对边平均约 **16.487～16.500 mm**；自由角点双目重投影 RMS 为
|
||||
front 1.040～1.107 px、side 0.878～0.935 px。
|
||||
- 强制 ID1 为 16 mm 平面方形时，front RMS 仍为 **1.629～1.677 px**，超过 1.5 px。
|
||||
- ID2 两组对边约 **16.499～16.585 mm**；自由角点的双目 RMS 约 0.41～0.51 px。
|
||||
|
||||
新旧手位姿分别估计，不参与棋盘相机参数的拟合。使用已知真值的另一个位置/姿态
|
||||
Tag 验证过这一分时检查方法，两种来源均恢复 16 mm，误差小于 1e−5 mm。
|
||||
结果记录在各棋盘目录的 `board_tag_consistency_joint_board_tag_20260916_102403.json`。
|
||||
这仍不足以单独判定应改 Tag 尺寸、棋盘尺寸还是相机模型；没有因此运行新一轮整手运动。
|
||||
|
||||
### 共面长度比例的独立检查
|
||||
|
||||
用户提供同批备用 Tag，并把 ID1 固定在棋盘同一块板上。10:27 的三个原始正面图像
|
||||
位于 `coplanar_capture_20260916_102710/frame_00` 至 `frame_02`。图中左侧的备用 ID1
|
||||
与棋盘共面；右侧手掌 ID0 不共面，不将其按棋盘平面换算的数值解释为尺寸。
|
||||
|
||||
使用棋盘格交错的 20 个角点拟合平面单应变换，另 20 个角点检查投影误差，不使用
|
||||
相机外参、URDF、SDK 或关节曲线。原始像素直接计算的 ID1 两组对边约
|
||||
**16.474～16.478 / 16.595～16.602 mm**，留出棋盘角点 RMS 约 0.186～0.191 px。
|
||||
用当前内参去畸变后约 **16.680～16.685 / 16.593～16.600 mm**；旧内参、未采用的
|
||||
棋盘形状内参仅作预先规定的敏感性对照，结果也记录在同一报告中，不按 Tag 尺寸选模型。
|
||||
|
||||
该 ID1 接近原图左边缘，畸变模型之间仍有约 0.2 mm 的差异，所以未据此改配置。
|
||||
随后请用户将备用 ID1 移到棋盘上方中间，再核验边缘畸变的影响。
|
||||
|
||||
### 中央位置复核与实测尺寸的矛盾
|
||||
|
||||
用户将备用 ID1 贴到棋盘中间上方后，10:43 保存三个新的原始正面图像，位于
|
||||
`coplanar_capture_20260916_104349/frame_00` 至 `frame_02`。图中 ID1 位于同一底板上方，
|
||||
纸张棋盘完整可见；手掌 ID0 仍不与棋盘共面，不解释其尺寸换算值。
|
||||
|
||||
沿用预先规定的棋盘交错训练/留出划分，三个图像的结果为：
|
||||
|
||||
- 原始像素直接计算的 ID1 两组对边为 **16.511～16.513 / 16.594～16.602 mm**；
|
||||
留出棋盘角点 RMS 为 **0.188～0.197 px**。
|
||||
- 当前内参去畸变后为 **16.521～16.523 / 16.636～16.645 mm**，相较原始像素仅变化约
|
||||
0.01 / 0.04 mm，不能解释相对 16 mm 的约 0.5 mm 差异。
|
||||
- 另用传统棋盘角点检测加亚像素细化，并分别使用全部角点、靠近 Tag 的三行或两行
|
||||
拟合平面变换；不同检测方法和网格子集的边长变化小于 0.1 mm，比例差异仍存在。
|
||||
这是同一图像上的方法敏感性检查，不是新增独立实物样本，也不是新的合格标定。
|
||||
|
||||
结果见各帧的 `coplanar_scale_assessment.json` 及汇总目录中的
|
||||
`central_coplanar_sensitivity.json`。上述长度全部以 27 mm 棋盘间距、平面棋盘及
|
||||
Tag 与棋盘共面为条件；图像本身不能无条件证明 Tag 的绝对毫米尺寸或两者物理共面。
|
||||
这项检查未使用外参、URDF 或关节拟合，不能通过增加关节拟合参数解决其比例矛盾。
|
||||
|
||||
用户随后确认 16 mm 和 27 mm 均为打印后实物测量值。保留这两个生产输入，不能将
|
||||
图像条件换算直接盖过实测值并断定打印错误。已请求测量工具、Tag 黑框宽高及棋盘连续
|
||||
五格的未取整总长度，以区分测量精度与几何假设的问题。此次仅进行静止图像与离线
|
||||
检查，没有新增整手运动，没有修改相机、尺寸或验收门限，没有生成合格发布 manifest。
|
||||
|
||||
## 第十七轮评估：依据实物复测修正 Tag 尺寸
|
||||
|
||||
用户再次量取后明确：“tag 是 16.5×16.5 mm，棋盘还是 27×27 mm”。这提供了独立的
|
||||
实物输入修正依据，与先前双目约 16.49 mm、中央共面约 16.51×16.60 mm 基本一致。
|
||||
不是从 URDF 拟合残差反推并选择一个易于通过的尺寸。
|
||||
|
||||
原先按 16 mm 计算 16.5 mm 的方形 Tag，会在同一单目姿态解下将平移距离缩小到
|
||||
16/16.5，约低估 3.03%；跨相机变换中的基线平移来自 27 mm 棋盘，并不会随之缩放。
|
||||
这构成明确的输入尺度不一致，不能通过增加运动曲线参数来合理消除。
|
||||
|
||||
修改范围仅为当前 O6 的以下配置及指纹：
|
||||
|
||||
- `profiles/o6_right_8.yaml`：8 个 Tag 均显式声明 `size_m: 0.0165`。
|
||||
- `o6_right_8_tags.yaml`：检测器默认尺寸和每个 ID 的尺寸均为 0.0165 m。
|
||||
- `o6_three_camera_calibration.yaml`：标定节点尺寸及全部 ID 覆盖值同步为 0.0165 m。
|
||||
- `o6_right_product.yaml`：更新以上三个受保护文件的 SHA-256。
|
||||
|
||||
程序已有 Profile、检测器和标定节点的尺寸一致性检查，本次无需增加型号特判或
|
||||
修改拟合算法。其他型号/打印批次没有新的实测依据，不自动替换其配置。棋盘尺寸未变，
|
||||
没有因此修改相机内外参、源 URDF、旧采集文件或验收门限。非线性关系仍由完整 JSON
|
||||
和修正 URDF 配合表达。改前配置与改后指纹见
|
||||
`20260915_213337/review/whole_chain/tag_size_correction_20260916/`。
|
||||
|
||||
28 项配置加载、实际 launch 参数和 PnP 参数测试通过；已重新构建安装，核实四个
|
||||
配置文件与源码逐字节一致，`--validate-only` 通过。用户移开棋盘及备用 ID1 并确认
|
||||
O6 可运动后，启动新会话 `20260916_105528`，显式 `--no-resume` 完整采集。
|
||||
最终是否合格须以该会话的独立验收及发布文件为准，不能仅凭尺寸改正确就声明成功。
|
||||
|
||||
### 新会话结果与收尾内存问题
|
||||
|
||||
36 个采集单元全部通过，小指第三轮 increasing 方向第一次存在反馈分箱空白 42，
|
||||
按原最大空白 16 的门限自动重扫一次后通过。完整采集约 6 分 47 秒；保存 15,167 个
|
||||
原始图像记录、6,828 个关节样本及 15,629 个 SDK 反馈样本。11:02:20 完成最后采集单元，
|
||||
随后开始安全回位与收尾。11:05:28 系统内核明确记录 OOM、4 GiB swap 耗尽，终止了
|
||||
一个 VS Code 进程；三相机同时出现长时间时钟读取中断，SDK 反馈超时，终端随后发生
|
||||
BrokenPipeError。不能把该现象归因于机械手硬件，也不能将此会话记为在线无暂停完成。
|
||||
|
||||
原始日志完整保留，正式 `--offline-raw` 流程验证了配置指纹、全部采集单元、运动来源、
|
||||
独立第四轮与稳态验证数据，生成 `offline_165mm/` 下的 JSON、URDF、报告和
|
||||
`release_manifest.json`，退出码 0。JSON/URDF 的 SHA-256 与清单相同；已从保存 JSON
|
||||
再独立调用重建 CLI，得到逐字节相同的 URDF。原来的角度 MAE/P95/max 1°/2°/3°、
|
||||
位置 P95 3 mm 及对应图像门限没有修改。
|
||||
|
||||
同时发现 `read_journal_prefix` 先整文件读取、再 `splitlines()`，在解码对象之外还保留
|
||||
两份原始文本。这份日志为 318,772,511 字节，额外副本会显著增加相机仍运行时的收尾
|
||||
峰值。已改为按冻结字节边界逐行解析，保留全部记录、顺序和不可越过边界的校验；
|
||||
不通过丢数据或修改安全超时规避问题。新增 16 MiB 日志的临时缓冲内存上限回归检查，
|
||||
并验证短文件与非对象记录拒绝；独立进程、取消、真实 finalizer 等共 9 项测试通过。
|
||||
已重新构建安装，进一步用同一实采数据检查独立收尾进程与已验收文件的一致性。
|
||||
|
||||
修改后的已安装独立进程以 `require_motion_evidence=True` 处理同一份完整原始数据，
|
||||
约 **55.21 秒**完成，子进程峰值 RSS 为 **1,344,164 KiB（约 1.28 GiB）**；
|
||||
JSON、URDF 两个文件的 SHA-256 均与正式离线验收版本相同。该检查没有启动相机或 SDK，
|
||||
也没有重新运动机械手，不能单凭它宣称修复后在线全过程已经完成第二次实机验证。
|
||||
报告为 `20260916_105528/isolated_finalization_verification.json`。
|
||||
|
||||
通过已验收文件的字节/哈希与 JSON→URDF 对应关系复核后，使用现有发布器将相同文件
|
||||
发布到产品根目录的 `20260916_105528_verified_165mm/`，并更新 `latest_partial_passed`。
|
||||
原始中断会话、离线验收目录与独立进程检查目录全部保留；没有把原始 PAUSED 状态改为
|
||||
COMPLETE。正式目录中的 `release_origin.json` 记录已验收来源；完整结论和实际采集范围
|
||||
见 `20260916_105528/verification_summary.json`。当前布局直接测量拇指和小指共 5 个关节，
|
||||
食指、中指、无名指共 6 个关节的运动曲线按原配置从小指迁移，不宣称这些关节独立实测。
|
||||
|
||||
## 第十八轮评估：被动关节由 JSON 提供角度，消除线性导出冲突
|
||||
|
||||
用户指出拇指 IP 在修正 URDF 中明显弯得更小。复核确认，原 URDF 的线性倍率为 1.86，
|
||||
旧修正产物的倍率为 1.16556243135602，而实测 JSON 的 IP/pitch 比例随行程约从 1.71
|
||||
变化到 1.94。父子下限分别为 -0.00172483145889436、-0.0020103987491093 rad，
|
||||
旧“共同零点＋整段不越界”约束把这两个小负端点的比值变成全行程倍率上限。
|
||||
这不是非线性 JSON 的拟合失败，而是将独立实测曲线再次约束为一条直线的导出设计问题。
|
||||
旧完整 JSON＋自定义 FK 验收虽通过，标准 ROS mimic 仍会覆盖被动角，因此显示与验收不一致。
|
||||
|
||||
评估后采用统一的 JSON 角度来源规则，不修补 O6 的某个倍率：
|
||||
|
||||
- 新 v3 修正 URDF 移除原被动关节的 `<mimic>`，全部关节角由 JSON 明确给出。
|
||||
父子链、转轴、连杆、零位与限位保持原有受检语义；原被动关节没有新增实机驱动通道。
|
||||
- `urdf_correction` 升为版本 2，保存 `joint_angle_source: calibration_json`、
|
||||
`passive_joint_sources` 和明确的删除操作。只有完整覆盖运动关节、来源与原 URDF 一致、
|
||||
被动关节与来源关节共享 SDK 通道时，才能授权移除；其他数值编辑仍使用 Profile 原授权。
|
||||
导出版本与采集配置分开，未修改 Profile 指纹或原始记录来绕过保护。
|
||||
- JSON 先保存，重新读取后重建 URDF；普通 FK、角度/位置验证及原始图像验收使用该最终文件。
|
||||
标准 `robot_state_publisher` 可直接接收全部关节状态，不需要忽略 mimic 的定制算法。
|
||||
- 线性拟合仅保留报告诊断，标记 `diagnostic_only_not_exported`，不再按限位限制拟合倍率，
|
||||
不参与导出和运行角度。旧 correction v1 的精确重建和旧读取方式继续兼容。
|
||||
|
||||
专项回归覆盖微小负限位、非线性行程、普通 FK、JSON 重建字节一致性、错通道、缺失曲线、
|
||||
非法来源、未经授权删除与拓扑改动拒绝。真实 ROS TF 测试在独立命名空间验证三个弯曲点，
|
||||
未连接 SDK；末节角度不再被覆盖。O6/G20/L6/O12 的合成完整收尾、独立图像验收、迁移来源、
|
||||
双向映射及旧格式检查通过；这不是其他三型号的新增实机精度证明。
|
||||
|
||||
已构建安装，并用 `20260916_105528/raw_samples.jsonl` 的完整 16.5 mm Tag 实采数据
|
||||
执行正式离线回放与发布，退出码 0。新目录为
|
||||
`calibration_output/O6_RIGHT_001/20260916_123347_json_driven_165mm/`，
|
||||
`latest_partial_passed` 已更新。原来的角度、位置和原始图像门限全部通过，没有放宽。
|
||||
|
||||
对新旧发布文件逐项比较：11 个关节的全部指令表、反馈映射、零位和限位不变；
|
||||
URDF 的结构差异仅为移除 5 个线性 mimic。重新读取新 JSON 重建得到逐字节相同的 URDF。
|
||||
相同输入的拇指结果如下（单位：度）：
|
||||
|
||||
| SDK 指令 | CMC pitch | 旧 URDF 线性 IP | 新 JSON＋URDF IP |
|
||||
| --- | ---: | ---: | ---: |
|
||||
| 192 | 9.3781 | 10.9307 | 16.0523 |
|
||||
| 128 | 18.8914 | 22.0191 | 33.0907 |
|
||||
| 64 | 27.2887 | 31.8067 | 49.6742 |
|
||||
| 0 | 34.2391 | 39.9078 | 66.3515 |
|
||||
|
||||
复核脚本与结果保存在原会话的 `verify_json_driven_export.py`、
|
||||
`json_driven_export_verification.json`，正式回放日志为 `json_driven_replay.log`。
|
||||
旧发布三文件的哈希未变。本次没有重新运动机械手，沿用原 5 个独立观测关节、6 个迁移关节
|
||||
及 CAD 零位假设的精度边界。使用新产物时须通过 JSON 同时驱动全部关节；单独拖动 URDF
|
||||
父关节滑条不再自动联动末节，外部仿真也须移除与 JSON 冲突的旧线性更新逻辑。
|
||||
|
||||
## 第十九轮评估：按用户要求保留标准 mimic 近似联动
|
||||
|
||||
用户明确使用 `https://viewer.robotsfan.com/` 直接预览 URDF，并进一步说明：
|
||||
非线性关节也必须保留 `<mimic>` 自动联动，单独 URDF 尽量接近实机；
|
||||
需要实测非线性结果时再由标定 JSON 查找表控制。上一轮删除 mimic 的正式导出方向
|
||||
不符合这一使用要求。已核对网站当前构建及官方 URDFAdapter：滑条直接设置 URDF 关节，
|
||||
没有本项目标定 JSON 的读取逻辑，不能期望它自动获得 JSON 中的非线性曲线。
|
||||
|
||||
### 方案评估与实现
|
||||
|
||||
标准 mimic 使用 `q_child = a*q_parent+b`。将 `b` 强制为 0，再约束父子全行程，
|
||||
使拇指的两个微小负端点决定全局倍率上限，这是旧版 1.16556 倍率失真的原因。
|
||||
仅恢复 `<mimic>`、手填倍率或扩大限位都不能解决模型与约束之间的矛盾。
|
||||
|
||||
现在使用两端子关节角度作为拟合变量：
|
||||
|
||||
- 父关节输出范围 `[p0,p1]` 保持不变，拟合 `c0=q_child(p0)`、`c1=q_child(p1)`。
|
||||
- 两个变量分别受子关节原输出限位约束；区间内的线性插值自然不会越界。
|
||||
- 由 `a=(c1-c0)/(p1-p0)`、`b=c0-a*p0` 导出标准 mimic 倍率和偏移。
|
||||
- 沿用前三轮成对视觉样本与稳健损失拟合，第四轮只验证；不改变 SDK→角度表、
|
||||
坐标零位、Tag 安装、相机参数或训练支持的限位。
|
||||
- `b` 是线性近似在零点的误差，不是新测出的机械零位。报告同时保存
|
||||
`baseline_error_rad`、`physical_zero_correction: false` 和实际残差。
|
||||
|
||||
生产 `urdf_correction` 使用版本 3，并声明
|
||||
`mimic_policy: range_bounded_affine_approximation`、`joint_angle_source: calibration_json`。
|
||||
仍先保存 JSON，再读取生成 URDF。所有原 mimic 来源与 XML 拓扑保留；
|
||||
数值修改继续使用已有 Profile 授权，没有更改采集指纹。版本 1、2 产物保留重建兼容，
|
||||
不原地覆盖。完整 JSON 回放显式使用非线性被动角，URDF 单独回放使用 mimic；
|
||||
两者分别报告精度，线性近似不能冒充 JSON 的实测精度。
|
||||
|
||||
专项检查覆盖微小负限位不压低全程倍率、正负运动方向、全行程限位、留出样本禁止参与拟合、
|
||||
重建字节一致性及缺失/降级导出规则拒绝。真实标准 ROS TF 检查只给父关节角度，
|
||||
验证被动关节按导出 mimic 联动；历史无 mimic 版本的独立关节测试也继续通过。
|
||||
|
||||
### O6 实采数据复验与发布结果
|
||||
|
||||
相关 121 项检查通过，覆盖四型号合成产物、迁移来源、双向表、历史格式及标准 ROS 联动。
|
||||
构建安装后，以同一份完整 `20260916_105528/raw_samples.jsonl` 正式离线回放，退出码 0，
|
||||
生成并发布 `20260916_124718_mimic_json_165mm/`，更新 `latest_partial_passed`。
|
||||
JSON＋URDF 的角度、位置及独立原始图像验收通过，原精度门限不变。
|
||||
|
||||
新旧三版的全部 11 关节指令/反馈曲线、几何零位、限位逐项一致；5 组 mimic 来源与
|
||||
原始 CAD 相同。拇指 IP 倍率为 **1.86604745134131**，偏移为
|
||||
**0.00120821859875384 rad（0.06923°）**。小指及三个迁移末节的倍率为
|
||||
**0.870364582419037**，偏移为 **0.0348981257618717 rad（1.99952°）**。
|
||||
这些偏移是近似关系在零点的残差，完整 JSON 的 baseline 仍为 0。
|
||||
|
||||
| SDK 指令 | CMC pitch | 旧 URDF IP | 新 mimic IP | JSON IP |
|
||||
| --- | ---: | ---: | ---: | ---: |
|
||||
| 255 | 0.0000° | 0.0000° | 0.0692° | 0.0000° |
|
||||
| 192 | 9.3781° | 10.9307° | 17.5691° | 16.0523° |
|
||||
| 128 | 18.8914° | 22.0191° | 35.3214° | 33.0907° |
|
||||
| 64 | 27.2887° | 31.8067° | 50.9912° | 49.6742° |
|
||||
| 0 | 34.2391° | 39.9078° | 63.9611° | 66.3515° |
|
||||
|
||||
相对 JSON 的 256 项指令网格，拇指线性近似最大差约 **2.3905°**，小指及对应迁移末节
|
||||
最大差约 **4.1965°**。这是两个表示之间的差异，不是新增的实机独立精度测量；
|
||||
独立留出结果分别保存在 manifest 的 `full_json_urdf_holdout` 和 `urdf_mimic_approximation`。
|
||||
因此网站中的联动预览仍是近似，需要实测非线性结果时必须使用 JSON。
|
||||
|
||||
另行读取新 JSON 重建得到逐字节相同的 URDF。所有旧发布文件哈希不变，
|
||||
没有重新运动机械手，也没有改写旧采集结果。复核脚本、结果及正式回放日志分别为
|
||||
原始会话下的 `verify_affine_mimic_export.py`、`affine_mimic_export_verification.json`、
|
||||
`affine_mimic_replay.log`。本次仍只独立观测拇指、小指 5 个关节，其他 6 个关节沿用原迁移关系。
|
||||
|
||||
## 第二十轮评估：全型号保留原始 mimic，实测非线性由 JSON 提供
|
||||
|
||||
用户确认统一采用原始 mimic 作为基础联动，暂时取消正式标定中的 mimic 参数优化。
|
||||
该规则替代第十九轮新产物的拟合策略；历史产物保持原样及原有读取语义。
|
||||
|
||||
### 全型号依据与边界
|
||||
|
||||
检查 L6、O6、O12、G20 的左右手原始 URDF，以及当前四套右手产品配置。
|
||||
L6/O6 部分曲线从小指迁移，O12 包含两级 mimic 链和无名指曲线迁移，G20 配置覆盖全部实测关节。
|
||||
左右手系数不能互相套用：O6 拇指左右分别为 2.29/1.86，G20 为 1.02/1.03。
|
||||
原始文件也不等于限位自洽：按原始右手父关节全行程，L6 拇指约超出子限位 3.6795°，
|
||||
O12 无名指中节约 0.7162°，G20 四个 DIP 约 0.4297°。这些是文件内冲突，不是硬件越限结论。
|
||||
O6 的已声明实测范围策略不能自动授予其他型号放宽 CAD 硬限位的权利。
|
||||
|
||||
### 统一实现
|
||||
|
||||
- 新 correction schema v4：`mimic_policy=preserve_source_mimic`，`joint_angle_source=calibration_json`。
|
||||
- measured 正式拟合及收尾不再调用 mimic 参数优化，原 mimic 来源、倍率、偏移及省略属性全部保留。
|
||||
- `source_mimic.py` 统一计算负倍率、非零偏移及串联关系的可达范围，不含型号分支。
|
||||
- `mimic_envelopes` 单独记录基础范围、联动可达范围和最小导出包络；不会改写实测范围证据。
|
||||
- 包络扩展只适用于已有实测范围策略；默认 CAD 硬限位和独立安全限位继续生效，冲突明确报告关节及范围。
|
||||
- 零位仍修正 origin。原始 mimic 只作为修正坐标中的预览近似，精确 FK 使用 JSON 提供的各关节角。
|
||||
- 新 JSON 禁止请求 mimic 参数修改或删除；最终验收对照受保护原文件核对 mimic 元素和可信范围证据。
|
||||
- correction v1/v2/v3 继续兼容重建与读取，既有原始记录和配置指纹不变。
|
||||
|
||||
### 验证与 O6 发布
|
||||
|
||||
两组相关检查分别 **68 项、72 项通过，共 140 项**。覆盖八份左右手源文件、负倍率、零倍率、
|
||||
非零偏移、多级链、独立安全界限、非法 JSON 修正、历史版本重建及标准 ROS 的父关节联动。
|
||||
四型号合成完整流水线同时验证：禁止调用 mimic 优化函数，仍能完成 JSON→URDF→独立角点验收→发布→读取。
|
||||
这些合成检查不代表其他型号已经完成实机标定。
|
||||
|
||||
构建后以完整 `20260916_105528/raw_samples.jsonl` 正式离线回放，退出码 0，发布：
|
||||
`20260916_134154_source_mimic_165mm/`，`latest_partial_passed` 指向该目录。
|
||||
5 个 mimic 元素与原始 O6 URDF 一致，拇指 1.86、其余四个末节 0.89，offset 全为 0。
|
||||
所有 11 关节指令/反馈查找表、几何零位、实测范围证据与前三版逐项一致，旧产物哈希不变。
|
||||
相对上一版 URDF，除 mimic 参数外仅 IP 下限改变：
|
||||
`−0.0020103987491093 → −0.00320818651354352 rad`，约增加 **0.068628°** 的联动兼容范围。
|
||||
|
||||
SDK 指令 0 时，拇指 pitch 为 34.2391°，原始 mimic 预览 IP 为 63.6848°，JSON IP 为 66.3515°。
|
||||
完整 JSON＋URDF 的拇指 IP 独立角度 P95 为 0.43466°、位置 P95 为 2.11692 mm；
|
||||
小指 DIP 分别为 1.87341°、1.94512 mm，原精度门限未改。
|
||||
独立图像验收的门限、样本数量和通过结论保持不变；重复数值优化的末位浮点值及诊断项可能变化。
|
||||
读取保存的 JSON 重建得到逐字节相同 URDF；本次没有重新运动机械手。
|
||||
|
||||
复核材料位于原始会话目录：`verify_source_mimic_export.py`、
|
||||
`source_mimic_export_verification.json`、`source_mimic_replay.log`。
|
||||
当前 8 Tag 布局仍只独立测量 5 个关节，其余 6 个按既有声明迁移；在线采集曾有 OOM 的边界保持原记录。
|
||||
@@ -0,0 +1,230 @@
|
||||
# 按关节零位与实测/复制 JSON v3 实施记录
|
||||
|
||||
更新：2026-09-15。范围：`linkerhand_calibration`。
|
||||
|
||||
## 当前收敛结果
|
||||
|
||||
- G20、L6、O6、O12 右手共用四轮连续扫描，随后执行独立的稳态指令映射训练与验证。原精度门限保留。
|
||||
- 正式入口在启动硬件前检查运动覆盖与绝对零位来源。O12 的 `thumb_mcp`、`pinky_pip`、`ring_pip`
|
||||
按用户确认沿用原始 URDF 零位，记录为 `assumed_source_cad_zero`；运动继续实测或按声明复制。
|
||||
- 收尾先保存 JSON，再由 `runtime/artifacts/urdf_from_json.py` 读取它重建 URDF。
|
||||
发布器检查修正元数据能否逐字节重建最终文件；可独立使用 `rebuild_calibrated_urdf` 命令。
|
||||
- 四型号的合成原始观测均已通过实际拟合、文件生成、验收、发布及读取器加载。
|
||||
合成夹具使用明确的几何真值与范围，这不代表四种实机均完成验证。
|
||||
- O6 会话 `20260915_134211` 实采 36/36 单元一次通过，无重扫、无采集中断，采集耗时 412.35 秒。
|
||||
同一原始日志已生成候选 JSON 与修正 URDF，独立 JSON 重建得到相同字节；整链几何验收仍失败，未发布。
|
||||
最大 Tag 位置 P95 为 15.32 mm，小指末节姿态 P95 为 7.76°，不能称为整手精度合格。
|
||||
- 随后用棋盘格重新测得 O6 三相机外参：两组各 15 个姿态，最大联合重投影 RMS 为 0.928 px，
|
||||
最大重复平移差 0.341 mm、旋转差 0.043°，通过原门限。新配置为
|
||||
`config/o6_three_camera_extrinsics_20260915.yaml`；旧数据保留原外参身份，不混用。
|
||||
- 新外参会话 `20260915_144514` 完成拇指 24/36 单元后，小指准备求解有两个候选耗尽 150 次迭代。
|
||||
同一批 120 帧离线复现确认是内部 LSMR 线性求解精度不足；调整 `atol/btol` 为 `1e-10` 后，
|
||||
四候选均在原 150 次预算内收敛。外层收敛条件、分支统计和物理精度门限未变。
|
||||
`test_recorded_pinky_alternatives_converge_within_original_budget` 保存该实采回归；相关 34 项测试通过。
|
||||
- 恢复会话 `20260915_145437` 暴露旧日志在运动回调中校验导致的反馈超时报错：离线复现读取约
|
||||
1.41 秒、完整凭据/单元校验约 13.88 秒。`PreparedResume` 将静态校验移到回调启动前；
|
||||
当前固定基准与逐关节安装仍在现场独立验证。后续 `20260915_145748` 暴露一次性导入大日志
|
||||
仍会阻塞；改为每个控制周期最多导入 64 条记录，全部持久化后才确认复用。
|
||||
恢复相关 73 项测试通过，包含导入期间持续处理反馈、接收中止及落盘前不得跳过单元；原 1 秒反馈保护未变。
|
||||
- 图像模型零位恢复还需同步导入该零位的 `pnp_candidate_frame`，不能只复制 `joint_zero_sample`。
|
||||
已修复该来源闭合问题；真实中断恢复会话离线核对发现恰好漏掉 10 帧零位图像，按新导入逻辑保留后
|
||||
原凭据检查通过。原始日志未修改。相关 74 项测试通过，包含连续两次恢复的零位图像回放。
|
||||
- 新外参完整会话 `20260915_150015` 已完成 36/36 单元，采集中无暂停，两次自动重扫均通过。
|
||||
生成的候选 JSON 可独立重建出字节相同的修正 URDF，并通过标准 ROS URDF 加载。
|
||||
整链训练阶段的刚性 Tag 安装检查仍失败：拇指 IP 位置 P95 为 9.73 mm，小指 DIP 姿态 P95 为 9.40°。
|
||||
这不是第四轮整链精度通过的结果，未生成发布 manifest，原始 URDF 未覆盖。
|
||||
用户已确认 ID4/ID5 安装在平整硬片上，Tag 黑框为 16×16 mm，棋盘格单格为 27 mm。
|
||||
尚不能据这些残差判定硬件故障。
|
||||
- 随后完成三相机原始棋盘格内参核验：正面 34、侧面 25、顶部 24 个不同姿态；
|
||||
每第 4 张预先留作独立验证。验证重投影 RMS 从旧内参的 0.880/0.955/0.484 px，
|
||||
降至 0.276/0.465/0.317 px。新内参保存在 `config/o6_camera_intrinsics_20260915/`。
|
||||
配套外参使用正面/侧面 25 组同步观测和补采的正面/顶部 15 组同步观测，均无剔除；
|
||||
最大 RMS 0.575 px、旋转重复差 0.024°、平移重复差 0.269 mm，通过原门限。
|
||||
O6 产品配置已指向 `config/o6_three_camera_extrinsics_intrinsics_20260915.yaml`，
|
||||
并通过无运动的配置及受保护输入校验。旧相机文件与旧整手记录保留原身份。
|
||||
原图、角点、训练/验证划分、拟合报告及导出哈希保存在
|
||||
`calibration_output/O6_RIGHT_001/camera_intrinsics_audit_20260915/`。
|
||||
相机核验通过后仍须新采整手数据，不能将旧整手会话改挂新相机参数发布。
|
||||
- 新相机完整重采 `20260915_154743` 的拇指 24/36 单元全部完成，无重扫；小指准备的四候选
|
||||
均在 26–36 次迭代内收敛，但候选区分证据不足，停在 `image_motion_families_not_distinguishable`。
|
||||
随后发现统一 launch 将四型号受保护 Tag 配置中的 `detector.decimate=1.0` 强制覆盖为 `1.5`。
|
||||
已移除这层未声明在产品配置内的覆盖，检测设置统一来自受保护 YAML；不修改候选区分或物理精度门限。
|
||||
新回归实际展开 ROS 组件参数，修复前四型号均复现 `1.5 != 1.0`；修复后启动及 runner 的 55 项测试通过。
|
||||
构建和 `git diff --check` 通过；旧降采样记录不混入新结果。
|
||||
- 原分辨率完整重采 `20260915_160040` 已从三台运行节点读回 `detector.decimate=1.0`。
|
||||
拇指 24/36 单元一次通过，无重扫；小指准备的 104 帧、四候选均在 21–35 次迭代内收敛，
|
||||
仍因 `image_motion_families_not_distinguishable` 暂停，未生成本次完整候选 JSON/URDF。
|
||||
离线回放中两个未分开的 ID4 姿态旋转差 P95 为 18.64°，不能当作同一解；
|
||||
ID4 法线与相机光轴夹角在两解下分别约 6–8°、10–11°，运动中一直接近正视。
|
||||
已请求保持相机、手掌和 ID3 不动，将 ID4 硬片垫斜后重新采集；这不是硬件故障结论。
|
||||
`review/all_view_detections/` 保留本次后 200.7 秒的三机位检测与 SDK 消息。
|
||||
其中侧摆期间同时看到 ID1/ID2 的匹配样本仅 9 帧、反馈范围 206–255，
|
||||
不足以判断整个侧摆行程是否引起被动关节联动。所有精度门限保持原值。
|
||||
- 上述修复后的四型号文件链、JSON 重建、恢复凭据和图像求解定向回归共 55 项通过（76.55 秒），
|
||||
构建通过，`git diff --check` 通过；142 个原始 URDF/mesh 文件仍与 HEAD 相同。
|
||||
|
||||
本次生成测试见 `test_all_model_artifact_pipeline.py` 和 `test_json_urdf_rebuild.py`。
|
||||
O6 最新采集及暂停证据在工作区 `calibration_output/O6_RIGHT_001/20260915_160040/review/`；
|
||||
最近一次完成采集并生成候选文件的证据在 `calibration_output/O6_RIGHT_001/20260915_150015/review/`；
|
||||
旧外参会话证据保留在 `calibration_output/O6_RIGHT_001/20260915_134211/review/`。
|
||||
下方分批测试数字为各次改造的历史记录。
|
||||
|
||||
2026-09-15 最后一次全包回归覆盖 1424 项：1408 项直接通过、15 项旧夹具兼容问题、1 项缺少实测记录跳过。
|
||||
这些失败项已修复，按失败清单重跑 15/15 通过，没有遗留失败；这是全量运行加失败项复测的结果。
|
||||
O12 使用当前 CAD 假设的完整生成/验收测试另行通过。构建与四型号安装入口检查通过，
|
||||
142 个原始 URDF/mesh 文件与 HEAD 内容一致。
|
||||
完整日志和复测日志保存在上述 O6 `review/` 目录的 `linkerhand_final_full_tests.log`、
|
||||
`linkerhand_final_rechecks.log`；`software_validation.json` 汇总了验证范围。
|
||||
|
||||
## 实现与坐标约定
|
||||
|
||||
`q_CAD = q_output + delta`。每个关节自己的实机 baseline 对应 `q_output=0`，
|
||||
不把某次局部参考的完整手姿态解释为全手零位。
|
||||
|
||||
生产流程仍为统一 runner、ROS 接线、协调器、任务执行器、公共拟合与发布器。
|
||||
新增型号通过 Profile、SDK Adapter、Tag 观测关系与避让动作接入，没有新增型号专用算法。
|
||||
|
||||
| 职责 | 实现 |
|
||||
| --- | --- |
|
||||
| 显式关节零位配置、已知几何依据 | `core/domain/profile.py`、`profiles/loader.py`、`profiles/validator.py` |
|
||||
| 不可变零位证据及持久化校验 | `core/domain/reference.py` |
|
||||
| 避让后按声明姿态采集零位 | `runtime/execution.py`、`runtime/motion_execution.py`、`runtime/capture.py` |
|
||||
| 会话/运动版本核对及冻结提交 | `runtime/coordinator.py` |
|
||||
| 原零位核验后逐任务恢复数据 | `runtime/joint_resume.py` |
|
||||
| 冻结参考下的相对运动与指令映射 | `core/fitting/motion_fit.py`、`command_mapping.py`、`anchored_mapping.py` |
|
||||
| 明确声明的运动复制、零位迁移及近似来源 | `core/fitting/motion_transfer.py` |
|
||||
| 独立线性 mimic 近似 | `core/fitting/mimic_approximation.py` |
|
||||
| 空间零偏及授权 URDF 修正 | `core/fitting/session.py`、`spatial_solver/`、`core/urdf/result_plan.py` |
|
||||
| v3 序列化、读取和最终文件验收 | `runtime/artifacts/`、`core/urdf/measured_acceptance.py`、`tag_acceptance.py` |
|
||||
|
||||
Start 清空预览后只锁固定参考。运动 Tag 轮到相应关节时才成为必需观测。
|
||||
Profile 必须声明完整零位指令和非零的最终到达动作；扫描起点不等于 baseline 时先访问零位。
|
||||
参考保存完整指令、反馈、到达方向、父子相对/公共位姿、图像身份、触发任务和会话/运动版本。
|
||||
其他手指的非零避让姿态保持在证据中。无法分离同通道多个关节影响的观测在启动检查时拒绝。
|
||||
|
||||
同一关节只冻结一次。重复任务、方向、轮次、重扫及恢复不替换参考。
|
||||
恢复先验证整场指纹，再在各关节声明姿态验证旧安装，保留原参考身份，通过后才导入已完成单元。
|
||||
原生 rad 输入的第一轮行程统计也随已验证单元恢复，避免跳过第一轮后丢失覆盖依据。
|
||||
迟到的预览、暂停前观测或旧运动结果不能进入当前参考。
|
||||
零位稳定性判断使用各机位最近的足量独立帧，短暂异常帧会随新帧退出判断窗口；
|
||||
原始记录保留，稳定性门限不变。
|
||||
|
||||
JSON 关节来自自己的视觉样本，或 Profile 明确声明的对应实测来源；同一 SDK 通道允许绑定多个关节。
|
||||
接收关节保留自己的 SDK 通道，复制来源的指令/反馈节点及角度，不再拟合。
|
||||
接收关节没有独立零位帧和第四轮指标，报告标记 `independently_measured=false` 并引用来源证据。
|
||||
运动曲线只用前三轮，第四轮分别验证两方向；独立稳态分区用于指令映射训练与验证，约束 baseline=0。
|
||||
O6 侧摆按已确认策略输出正反向指令曲线；其他关节沿用单值指令曲线。两方向共用同一参考。
|
||||
验证数据不能用于重新拟合曲线、零位、Tag 安装或基座。
|
||||
序列化只导出冻结结果；u8 必须实测支持 0–255 才能生成 256 项，rad 只在 `input_values` 范围内插值。
|
||||
|
||||
修正 URDF 保留各型号、本侧的原始 mimic，单独使用时提供近似联动；完整非线性角度由配套 JSON 提供。
|
||||
`urdf_correction` v4 明确 `preserve_source_mimic`，正式标定不再拟合倍率或偏移，
|
||||
不复制其他手指的 mimic 参数，也不因修正 origin 而重写原 mimic offset。准确 FK 显式使用 JSON 的各关节角。
|
||||
`mimic_envelopes` 与实测 `motion_range_evidence` 分开：只在明确声明的实测范围策略下，
|
||||
取实测范围与原始联动可达范围的最小包络；禁止突破独立安全界限及默认 CAD 硬界限。
|
||||
多级 mimic 使用父关节真正的联动可达范围递推，不将其更宽的 JSON 运动范围继续放大。
|
||||
JSON 重建与最终文件验收均核对原 mimic 元素、最小包络、通道一致性及非线性表。
|
||||
旧 correction v1、移除 mimic 的 v2、拟合近似的 v3 保持原有重建/读取语义。
|
||||
既无运动观测也无显式复制声明，或缺少绝对零位来源时，保留相对运动诊断并拒绝完整发布。
|
||||
旧 mimic 及旧迁移字段不能自动授权 v3 复制，不用默认零偏补全未知几何。
|
||||
接受原 CAD 末节零位的前提单独记录在 `cad_zero_assumptions`，不作为独立实测证据；
|
||||
仅明确列出的关节取 δ=0，保持其自身 `origin`；O12 的三个额外关节假设见上方当前结果。
|
||||
|
||||
## 验收与消费
|
||||
|
||||
发布器从最终文件回读并分别检验：
|
||||
|
||||
1. 最终 JSON 对第四轮原始视觉角度；逐实测关节、逐方向检查；复制表另核验来源与一致性。
|
||||
2. 最终 JSON＋修正 URDF 的完整 FK 对第四轮 Tag 位姿；显式使用所有被动角。
|
||||
3. 相同主动映射配合标准 mimic 的独立近似损失，单独报告。
|
||||
|
||||
前两项精度门限仍为角度 MAE ≤1°、P95 ≤2°、最大 ≤3°，Tag 位置 P95 ≤3 mm。
|
||||
三文件哈希、保护输入、观测覆盖和通道绑定一并检查；拟合失败不自动运动，取消后不能提交。
|
||||
混合产物的 manifest acceptance 为 `measured_and_transferred_json_urdf_v3`，
|
||||
明确记录观测关节、复制关节和 CAD 零位假设。未观测接收关节不具有独立精度结论。
|
||||
|
||||
公共读取器和 `calibrated_joint_state_bridge` 支持 v3 的完整关节映射及历史 v1/v2 原语义。
|
||||
完整 JSON 回放使用 `independent_mimic_angles=True`，防止线性近似覆盖实测被动角。
|
||||
标准 `robot_state_publisher` 和普通 URDF 网页查看器提供线性联动预览，不据此宣称实测非线性精度。
|
||||
本轮未修改 MuJoCo、相机时间同步和接触动力学；现有 MuJoCo 脚本尚不能视为支持 v3。
|
||||
|
||||
## 当前型号的发布边界
|
||||
|
||||
| 型号 | 独立运动观测 | 声明的运动复制 | 绝对几何边界 |
|
||||
| --- | --- | --- | --- |
|
||||
| G20 | 21 / 21 个运动关节 | 无 | 五个末节的原 CAD 零位为明确假设 |
|
||||
| L6 | 5 / 11 | 其余三指 MCP、DIP ← 小指对应关节 | MCP 零偏明确迁移；五个末节 CAD 零位为假设 |
|
||||
| O6 | 5 / 11 | 其余三指 MCP、DIP ← 小指对应关节 | 同上 |
|
||||
| O12 | 16 / 19 | 无名指 MCP/PIP/DIP ← 小指对应关节 | pinky_pip、ring_pip、thumb_mcp 及末节沿用已确认的 CAD 零位假设 |
|
||||
|
||||
`--validate-only` 的 `independent_measurement_coverage` 返回具体关节列表。
|
||||
配置检查通过表示合同可执行，不能解释为全关节精度通过。
|
||||
上述运动策略已经覆盖各型号全部运动关节；完整发布仍要求实测来源验收通过、零位有来源且范围合法。
|
||||
四个产品配置的零位来源均已明确；L6/O12/G20 的原 CAD mimic/限位冲突保留诊断，
|
||||
收尾保留限位约束内的线性近似；所有 JSON 查表角度和 URDF mimic 全行程都必须处于合法范围内。
|
||||
现有 Tag 布局没有被补写成未经验证的新布局，新增参考动作也没有标为实机已验证。
|
||||
|
||||
## 原按关节零位改造的软件验证记录
|
||||
|
||||
修改前在原工作区测试路径上加载备份代码：**582 passed、1 skipped**。
|
||||
固定基线保存在 `/tmp/calibration_v3_baseline_ck_q_c99/`，包含原包、输入哈希和运行日志。
|
||||
旧数值回归继续显式使用 v2 语义，未通过重写黄金输出或放宽验收门限消除差异。
|
||||
|
||||
最终代码全量回归：**614 passed、1 skipped，485.43 秒**。
|
||||
日志：`/tmp/calibration_v3_final_verified.log`。
|
||||
包含最后补入的零位异常帧恢复用例；未放宽原有空间求解或精度门限。
|
||||
|
||||
已完成的额外检查：
|
||||
|
||||
- `colcon build --packages-select linkerhand_calibration --symlink-install` 成功。
|
||||
- 安装后的 12 个 console entry point 均能导入；四个型号的安装入口 `--validate-only` 均以 0 退出。
|
||||
- 两种合成布局生成的 v3 修正 URDF 另经实际 ROS `robot_state_publisher` 隔离加载通过。
|
||||
- 对比修改前备份，142 个原始 URDF/mesh 文件与 16 个其他相机、Tag、设备配置文件内容不变。
|
||||
- 四份 Profile 新增参考动作、输出版本及采集策略，对应 product 仅更新 Profile 哈希。
|
||||
- `git diff --check` 通过；新实施记录随包安装，安装后的 README 文档链接检查通过。
|
||||
|
||||
主要回归场景：
|
||||
|
||||
| 场景 | 测试 |
|
||||
| --- | --- |
|
||||
| 四型号先锁固定参考、逐关节仅一次归零，食指在其他手指第四轮后开始 | `test_joint_zero_runtime.py` |
|
||||
| 起点与 baseline 不同、非零保持姿态、错误到达/禁用通道、跨任务复用、rad 断点行程恢复 | `test_joint_zero_runtime.py` |
|
||||
| 零位只采自身观测、短暂缺 Tag 丢帧、固定 Tag 缓存、姿态候选不重选冻结参考 | `test_observation_capture.py`、`test_o12_observation_resolution.py` |
|
||||
| 任意固定 Tag 安装旋转、冻结零位、非线性被动曲线、第四轮滑移、共零点约束 | `test_measured_zero_mapping.py` |
|
||||
| 最终非线性组合达标但 mimic 超差、文件篡改、通道错误、迁移/几何缺失禁止发布 | `test_measured_finalization.py` |
|
||||
| 更换通道顺序、机位名、任务顺序的虚拟型号走同一完整发布链 | `test_measured_finalization.py`、`measured_capture_fixture.py` |
|
||||
| 旧格式读取、标准 URDF 授权和结构、READY 后断流、迟到观测、后台反馈、中止与提交竞争 | 原有相关回归及扩展的 `test_calibration_coordinator.py` |
|
||||
|
||||
## 本轮观测策略扩展
|
||||
|
||||
全量回归：**700 passed、1 skipped，495.34 秒**，日志 `/tmp/calibration_transfer_full_verified.log`。
|
||||
全量收集后补入的两项反馈范围一致性用例另行通过（`/tmp/calibration_transfer_feedback_domain_verified.log`），
|
||||
本轮共覆盖 **702 项通过、1 项缺少实测数据而跳过**。未放宽任何空间求解或最终精度门限。
|
||||
构建成功；安装后 12 个 console entry point 均能导入，四型号 `--validate-only` 均以 0 退出。
|
||||
对照当前提交检查 142 个原始 URDF/mesh 文件，内容全部不变；四份产品配置仅更新对应 Profile 哈希。
|
||||
|
||||
新增回归见 `test_observation_strategy.py`、`test_motion_transfer.py` 和 `test_transfer_artifact_contract.py`。
|
||||
覆盖三种观测策略、虚拟型号改名/重排、接收通道、非线性被动表、来源与假设篡改拒绝、
|
||||
O12 PIP 不补零、末节自身 origin 和各指 CAD 几何保持、机械范围及混合产物读回。
|
||||
历史全实测合成夹具显式清空新的产品 CAD 假设，继续使用原非零合成真值；未修改其数值期望。
|
||||
`test_measured_transfer_finalization.py` 进一步覆盖 O6 及重排后的虚拟 O6：
|
||||
由 5 个关节的原始合成记录实际拟合，展开为 11 个关节，经过最终文件验收、发布及 manifest 读回；
|
||||
6 个复制关节保留自己的通道，且没有被制造为独立观测。完整组合通过时仍报告 mimic 近似损失。
|
||||
|
||||
合成数据的已知 baseline 几何明确标注 `synthetic_fixture_truth`，不写入产品 Profile。
|
||||
完整 v3 非线性回放覆盖合成 G20、O6 拓扑及重排后的虚拟型号；四型号执行器测试不等于四型号都有完整实测回放。
|
||||
原 G20 两种布局的合成回放均得到：最终关节映射最大误差约 0.079°，完整 FK 的 Tag 姿态最大误差约 0.078°、
|
||||
最差 Tag 位置 P95 约 0.025 mm；标准 mimic 的姿态最大误差约 5.465°，仍被如实报告。
|
||||
这些数字用于检验软件是否分开评价非线性映射与线性近似，不是相机或实机测量结果。
|
||||
O12 实测回放因未提供 `O12_REPLAY_RAW` 和 `O12_REPLAY_REFERENCE` 跳过。
|
||||
没有运行实机动作，没有整只手精度、USB 延迟或多关节接触工况的实测结论。
|
||||
|
||||
复现命令：
|
||||
|
||||
```bash
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
source install/setup.bash
|
||||
PYTHONPATH=src/linkerhand_calibration:$PYTHONPATH python3 -m pytest -q src/linkerhand_calibration/test
|
||||
colcon build --packages-select linkerhand_calibration --symlink-install
|
||||
ros2 run linkerhand_calibration calibrate_hand --config src/linkerhand_calibration/config/o6_right_product.yaml --validate-only
|
||||
```
|
||||
@@ -0,0 +1,740 @@
|
||||
# LinkerHand 多型号统一标定
|
||||
|
||||
G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器、拟合器、标准 URDF
|
||||
验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter，不再调用型号节点。
|
||||
|
||||
本包按观测关节冻结实机 baseline 零位，输出实测曲线及明确声明的复制曲线组成的 JSON v3，
|
||||
以及保留线性 mimic 联动的修正 URDF；报告区分独立实测、迁移和 CAD 零位假设。
|
||||
保留 CAD 连杆尺寸、轴位置、mesh、惯量。完整运动由 JSON 的全部关节角度配合修正 URDF 计算。
|
||||
软件仿真通过不等于实机精度通过；当前原始 G20/L6/O12 存在需要确认的 mimic/限位冲突。
|
||||
本轮实现与验证见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。
|
||||
|
||||
## 启动
|
||||
|
||||
```bash
|
||||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
colcon build --packages-select linkerhand_calibration --symlink-install
|
||||
source install/setup.bash
|
||||
|
||||
# 无运动、无 SDK 启动的配置检查
|
||||
ros2 run linkerhand_calibration calibrate_hand --config \
|
||||
src/linkerhand_calibration/config/o12_right_product.yaml --validate-only
|
||||
|
||||
# 正式启动：自动启动 SDK、相机、检测及标定，READY 后自动开始运动
|
||||
ros2 run linkerhand_calibration calibrate_hand --config \
|
||||
src/linkerhand_calibration/config/o12_right_product.yaml
|
||||
```
|
||||
|
||||
其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`。
|
||||
AprilTag 检测参数直接读取产品指定的受保护 Tag YAML，统一启动文件不再覆盖其中的采样分辨率。
|
||||
`calibrate_g20_right` 是同一 runner 的旧命令别名。不要同时运行 GUI、单独 SDK 或其他控制器。
|
||||
`--commands-disabled` 只预览，不发送运动；`--no-resume` 强制新采集。Ctrl+C 中止，不自动快速张手。
|
||||
重新开始时先安全回基准，仍必须确保现场没有障碍物。
|
||||
|
||||
### JSON 到修正 URDF
|
||||
|
||||
统一收尾顺序为：拟合与独立角度验证 → 保存标定 JSON → 重新读取 JSON 修正原始 URDF → 文件回读与整链验证 → 发布。
|
||||
JSON 的 `urdf_correction` 保存原始 URDF SHA256 和必要的关节字段修正，重建不依赖内存拟合对象。
|
||||
原始 URDF 不被覆盖；关节拓扑、字段授权、完整 mimic 范围和 JSON 查表范围在文件安装前检查。
|
||||
新 v3 使用 `urdf_correction.schema_version: 4`、`joint_angle_source: calibration_json`，
|
||||
并记录 `mimic_policy: preserve_source_mimic`：所有型号保留本侧原始 `<mimic>` 元素，
|
||||
不改来源、倍率、偏移或省略的默认属性；正式标定不再拟合 mimic 参数。JSON 保留各关节
|
||||
非线性曲线及方向分支，精度验收使用 JSON 提供的全部关节角。父子链、轴和连杆不变，
|
||||
被动关节与驱动关节仍绑定同一 SDK 通道，所有数值编辑仍按 Profile 授权。
|
||||
`mimic_envelopes` 单独记录基础范围、原始联动可达范围和导出范围；在明确的实测范围策略下，
|
||||
导出范围取二者的最小包络，实测范围证据与查找表不变。未声明该策略的 CAD 限位，以及
|
||||
另行声明的硬件安全限位，仍是硬边界；冲突时报告具体关节和所需范围，不压低倍率或放宽容差。
|
||||
关节 origin 按标定零位修正后，原始 mimic 作为输出坐标中的近似预览，不作为准确物理约束。
|
||||
旧 correction v1、移除 mimic 的 v2、拟合线性近似的 v3 产物继续兼容读取与重建。
|
||||
导出规则单独版本化，不改变采集配置指纹或旧原始记录，已发布产物不能原地覆盖。
|
||||
|
||||
已生成的 JSON 可单独重建对应 URDF，无需相机或 SDK：
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration rebuild_calibrated_urdf \
|
||||
--config src/linkerhand_calibration/config/o6_right_product.yaml \
|
||||
--calibration-json /path/to/calibration.json \
|
||||
--output /path/to/new_corrected.urdf
|
||||
```
|
||||
|
||||
输出路径必须尚不存在。没有 `urdf_correction` 的历史 JSON 不会被猜测或自动补全。
|
||||
同一 JSON 和源 URDF 可逐字节重建相同修正文件；发布器会核对此项一致性，读取器要求相应验收记录。
|
||||
历史无修正元数据的已发布文件继续沿用原有读取规则。
|
||||
完整文件验收失败时保留候选 JSON／URDF 和诊断，便于复核；只有 `release_manifest.json`
|
||||
和成功发布指针才代表通过，运行时读取器拒绝把未发布候选当作合格标定。
|
||||
|
||||
| Profile | SDK 位置通道 | 扫描任务 | Tag | 输出格式 |
|
||||
| --- | ---: | ---: | ---: | --- |
|
||||
| G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v3，256 项指令查表 |
|
||||
| L6/right/l6_right_8/v1 | 6 | 3 | 8 | unified v3，256 项指令查表 |
|
||||
| O6/right/o6_right_8/v1 | 6 | 3 | 8 | unified v3，256 项指令查表 |
|
||||
| O12/right/o12_right_16/v1 | 12 | 11 | 16 | unified v3，rad 输入节点查表 |
|
||||
|
||||
任务数、主动关节数和实测关节数不必相等。一条运动可观测多个主动/被动关节。
|
||||
观测策略由 `measurement.transferred_motion_sources` 声明，目标关节指向直接实测的来源关节：
|
||||
|
||||
| 型号 | 独立采集 | 复制的运动映射 |
|
||||
| --- | --- | --- |
|
||||
| O6/L6 | 拇指、小指 | 食指/中指/无名指的 MCP、DIP 分别复制小指对应关节 |
|
||||
| O12 | 拇指、小指、中指、食指 | 无名指 MCP/PIP/DIP 分别复制小指对应关节 |
|
||||
| G20 / 全关节模式 | 每个关节 | 不声明复制关系 |
|
||||
|
||||
复制保留接收关节自己的 SDK 通道、URDF 名称与 CAD 几何，不要求接收关节另贴 Tag 或采集零位帧。
|
||||
复制参数不等于接收关节独立实测；接收关节仍须有明确的绝对零位来源及合法机械范围。
|
||||
`zero.cad_zero_assumptions` 显式记录“实机 baseline 与原 CAD 末节零位一致”的使用前提，
|
||||
相应 DIP（及拇指末节 IP）δ=0、自身 `origin` 不改。这是假设，不是视觉测得的绝对 CAD 朝向。
|
||||
按用户确认，O12 的 `pinky_pip`、`ring_pip` 和主动 `thumb_mcp` 也采用原始 URDF 零位作为 baseline 假设。
|
||||
运动曲线仍按实际观测／明确复制关系生成，报告标记 `assumed_source_cad_zero`，不称为独立实测零偏。
|
||||
正式入口在驱动硬件前检查运动覆盖及零位来源，缺失依据时直接指出缺项，不采完才发现无法发布。
|
||||
启动检查分别列出 `unmeasured_joints`（未独立观测）、`unresolved_motion_joints`（也未声明复制）
|
||||
及 `missing_baseline_geometry`。旧迁移字段单独存在不会自动授权新的 v3 曲线复制。
|
||||
本次沿用现有 Tag 和避让布局声明延后零位动作，没有把新增动作或缺失 Tag 布局标为实机验证通过。
|
||||
每任务的运动模型采集四个往返，共八趟单程：前三轮训练，第四轮独立双向验证。
|
||||
四个产品配置统一使用 `command_capture_mode: separate`：这八趟均为完整连续扫描，随后另做一组稳态映射训练往返和一组独立验证往返。
|
||||
主扫描不逐点停顿；稳态点直接执行移动、稳定反馈、新图像采样，不重复等待两次到位。
|
||||
稳态训练点由前三轮连续数据生成 `command_sampling_plan`：在原节点网格上寻找最少的
|
||||
保留节点，各关节、各方向的预测曲线插值变化均不超过 0.25°；端点、基准、中间支撑点及
|
||||
显式增加的非线性节点保留。缺少完整训练数据或曲线噪声过大时使用原网格。
|
||||
这份计划只决定停在哪里，每个保留点的静态角度仍由新图像独立测量，不用连续运动角度
|
||||
冒充静态角度。独立验证网格保持原样，不参与节点选择。计划及训练来源哈希随原始记录保存，
|
||||
拟合、离线回放和断点恢复均重新核验；缺点、篡改或最终误差超限仍禁止发布。
|
||||
上一段已经完成到位/稳定检查且下一准备目标完全相同时，可复用一秒内、反馈未变化的
|
||||
完整保持姿态，省去重复准备等待；有实际位移、反馈变化或记录过期时执行原准备流程。
|
||||
原训练网格每方向通常为 9 点，并保留行程内的 baseline；O6 小指额外包含 239，共 10 点。
|
||||
新流程在这份网格上按上述规则精简训练停点。
|
||||
验证使用交错点加端点（O6 小指包含 247），两类数据不混用。
|
||||
日志中的运动轮编号为 0–3，独立指令映射训练和验证编号为 4、5；界面分别显示“稳态指令映射训练／独立验证”。
|
||||
其他采用 `interleaved` 的配置仍在四轮各方向中穿插稳态点，并使用三轮稳态训练、第四轮稳态验证。
|
||||
行进图像用于旋转/几何拟合，停稳后的新图像用于 SDK 指令映射；同一张图像不会复制成两类样本。
|
||||
每点至少 3 个稳定同步图像，轨迹结束后最多等待 2 秒，不能用精确指令/反馈相等判断稳定。
|
||||
关节零位冻结另需至少 10 张独立有效图像。`acquisition.joint_zero_timeout_seconds`
|
||||
单独限定首张合格零位图像后的采集窗口，四个产品配置均为 5 秒；几何求解后的跟踪恢复仍最多等待 2 秒。
|
||||
反馈不稳定会清空当前零位候选，但不会重置总采集期限；图像数量、1° 稳定性及分支证据要求不变。
|
||||
稳定窗口使用每条新收到的 SDK 反馈，按消息时间戳去重，不再只取控制定时器看到的最新一条。
|
||||
定时器延迟不能制造“独立反馈不足”；期间真实运动仍会使窗口失效。
|
||||
候选清空时的 `steady_capture_reset` 保存反馈窗口和丢弃样本数量，便于区分抖动、断流和调度问题。
|
||||
零位样本的 `capture_timing` 记录接收、历史快照和提交时间，用于检查处理延迟。
|
||||
图像数量未齐时先返回缺样结果，不在控制定时器中反复校验全部姿态。
|
||||
四轮及稳态采集完成、数据落盘后关闭图像采集；拟合只使用已保存的相机和参考证据。
|
||||
之后的新图像和相机消息不再改变拟合输入，也不触发采集暂停；SDK 反馈、故障保护及操作者中止持续生效。
|
||||
拟合或文件验收失败结束为 `FAILED`，保存 `failure_diagnostic.json` 中的失败阶段、原始数据路径和异常堆栈。
|
||||
这类失败应先离线复算，不能仅因状态失败就重复四轮运动。运动采集期间的固定参考检查保持生效。
|
||||
|
||||
O6 的顶部 ID7 与正面 ID2 实际固定在同一末节，Profile 将 ID7 明确绑定到 `rh_thumb_distal`。
|
||||
`thumb_yaw` 是它承担的侧摆观测角色，不是其安装连杆名称。整链回放包含该 Tag 上游的侧摆、弯曲和末节角度，
|
||||
保持关节沿用记录的到达方向。新绑定改变了 Profile 哈希，旧会话不会被自动复用为新配置的通过结果。
|
||||
|
||||
固定 Tag 的 ID 解码成功不代表边框完整。独立的视觉边框检查进程按完全相同的图像时间戳配对整流图像与检测结果，
|
||||
检查当前检测四边的黑白边界支持；部分遮挡产生的畸变四边形会被记为 `detection_quality_rejected`，
|
||||
不能进入 PnP、关节样本或固定基准移动计数。配对缓存有界，不用邻近时间的图像替代。
|
||||
检查使用当前检测四边形，清晰 Tag 发生真实位移仍进入原有 5 px／连续 10 帧保护。
|
||||
控制进程只订阅 `detections_checked` 小体积观测；三路大图像的传输、配对和边框处理不占用 SDK 控制节点的回调队列。
|
||||
ROS 宿主将准备几何求解和逐帧图像角度求解交给独立计算进程，避免与 SDK 反馈争用 Python 执行资源。
|
||||
准备请求仍限时 120 秒；逐帧请求最多三个在途，超时或工作进程不可用的图像不生成有效样本。
|
||||
工作进程只接收不可变图像/模型输入，没有 ROS 或 SDK 控制接口。返回后仍检查当前图像、跟踪版本和会话版本，
|
||||
不能将迟到结果提交到下一运动段。`motion_solver_diagnostic.jsonl` 记录请求、计算耗时、线程 CPU 时间及提交结果。
|
||||
当前任务需要的固定 Tag 无有效观测时，仍按原采样规则等待或暂停；其他机位的遮挡不误报为移动。
|
||||
稳态点缺失与连续扫描不足统一在方向结束处理，最多同速重扫一次。
|
||||
首次关节零位准备、避让及收尾属于独立的必要动作，不计入四个正式往返；不会每轮重新锁定零位。
|
||||
分段加减速和停点仍有耗时，八趟行程不等于耗时恰好降为原来的三分之一。
|
||||
新产品会话记录 `capture_schedule_version=unified_schedule_v3_separate_mapping`；旧 `interleaved` 配置仍记录 v2。
|
||||
采集模式或保护配置不同的旧断点不与新流程拼接，
|
||||
历史原始数据仍保留原语义供对应配置离线读取和分析。训练／验证身份与精度阈值保持不变。
|
||||
字节拟合不再改写原始 command/feedback，也不将接近端点的读数伪装成 0/255。
|
||||
|
||||
## O6 右手拇指短程诊断
|
||||
|
||||
当前 O6 实物在 2026-09-16 复测的 Tag 黑色码区边长为 **16.5 mm**，棋盘格边长仍为
|
||||
**27 mm**。O6 Profile、检测器配置和标定节点配置已同步采用 `0.0165 m`，产品配置记录
|
||||
对应文件指纹。该数值属于这套实物标记，不是所有型号或所有打印批次的统一尺寸。
|
||||
更换标记时应实测黑框，并同步维护上述配置；旧 16 mm 会话保留原尺寸记录，不作为
|
||||
新尺寸的恢复断点。Tag 尺寸修正不改变 JSON 中非线性运动曲线与修正 URDF 的配合方式。
|
||||
|
||||
本套 O6 的 2026-09-16 实采数据已生成通过验收的产物，目录为
|
||||
`calibration_output/O6_RIGHT_001/20260916_134154_source_mimic_165mm/`，
|
||||
`latest_partial_passed` 已指向该目录。JSON 和修正 URDF 应配套使用；当前 8 Tag 布局
|
||||
直接测量拇指、小指，其余三指沿用配置中的曲线迁移关系。采集完成后的在线收尾曾发生
|
||||
系统 OOM，最终产物由完整原始数据的正式离线流程验收；日志读取内存优化另经独立进程
|
||||
验证，不能将此记录描述为在线全过程无中断。详细证据见 `CALIBRATION_PIPELINE_REVIEW.md`。
|
||||
此目录用同一份原始数据重新验收，5 组 mimic 的完整元素与原始 URDF 一致：
|
||||
拇指倍率 **1.86**、其他四个末节 **0.89**，偏移均为 **0**。
|
||||
11 个关节的指令/反馈查找表及几何零位保持不变；仅拇指 IP 的 URDF 下限从
|
||||
−0.115187° 调整到 −0.183816°，覆盖原始联动所需范围，实测范围证据仍为原值。
|
||||
拇指 SDK 指令 0 时，IP 的 URDF 预览为 **63.68°**，JSON 为 **66.35°**；
|
||||
这是线性预览与实测非线性的区别。完整 JSON＋URDF 和独立原始图像继续满足原精度门限。
|
||||
`20260916_123347_json_driven_165mm/`（移除 mimic）及
|
||||
`20260916_124718_mimic_json_165mm/`（拟合倍率和偏移）为历史版本，已被本次发布替代。
|
||||
旧目录均保留；始终使用同一 manifest 指定的 JSON、报告和 URDF。
|
||||
|
||||
定位 Tag 姿态跳变时，可沿用相同产品配置添加诊断选项：
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration calibrate_hand \
|
||||
--config src/linkerhand_calibration/config/o6_right_product.yaml \
|
||||
--diagnostic-thumb
|
||||
```
|
||||
|
||||
诊断选择原 Profile 中的 `thumb_yaw_top` 和 `thumb_pitch_ip_front`，各采一次往返，
|
||||
观测拇指 CMC yaw、CMC pitch、IP 三个关节。保留原速度、完整扫描行程、准备/避让、
|
||||
固定基准锁定、按关节尝试零位确认和安全收尾；其余手指不执行扫描任务，但开始时仍会安全回到 baseline。
|
||||
本次诊断只启动正面和顶部机位：开始时一次锁定正面 ID0、顶部 ID6；运动观测使用顶部 ID6/ID7、正面 ID0/ID1/ID2。
|
||||
侧面不参与本次诊断，ID3 缺失或双解、侧面相机未连接均不会阻塞；因此无需为拇指诊断调整 ID3 或改动相机外参。
|
||||
范围由本次全部任务的主/辅观测依赖统一确定，启动、ROS 订阅、设备检查、基准锁定、指纹和进度使用同一范围。
|
||||
它不会随当前任务切换而重新锁定；正面和顶部的配置变化、固定基准漂移及所需 Tag 数据不足仍按原策略处理。
|
||||
正式全手标定继续使用 Profile 声明的全部机位和固定基准，外参文件及保护输入的完整校验保持不变。
|
||||
“短程”缩短的是任务和轮次数，不截断需要复现异常的运动行程。
|
||||
|
||||
本模式跳过稳态映射采样与三轮训练/第四轮验收，收尾后显示 `DIAGNOSTIC_COMPLETE`
|
||||
并正常退出。诊断按当前运动 Tag 的原始图像覆盖推进，持续姿态歧义不阻断原始候选采集；
|
||||
缺少当前运动 Tag 角点或同步反馈导致方向覆盖不足时，仍最多同速重扫一次，严重故障和中止行为不变。
|
||||
归零准备证据为 `unresolved` 时，仅诊断模式允许继续收集原始观测：必须先到达 Profile 声明的零位指令，
|
||||
SDK 反馈稳定，当前运动 Tag 具有足量、带会话和运动版本的图像，才保存独立的 `diagnostic_joint_zero_attempt`。
|
||||
它表示访问并观察了声明姿态，不表示已建立关节零位；不写入 `zero_references`，方向切换及重扫不重新归零。
|
||||
该组关节后续仅保存原始角点和候选，即使普通跟踪恢复，也不能生成 `joint_sample` 或伪造有效角度。
|
||||
缺少零位原始图像、SDK 不稳定、固定基准移动及已冻结参考冲突仍按现有策略暂停；正式标定仍须先确认分支和冻结零位。
|
||||
会话目录保存 `raw_samples.jsonl`（含角点和 IPPE 候选）、实际成功冻结的关节零位与 `diagnostic_capture.json`。
|
||||
`diagnostic_observation_frame` 在零位尝试和扫描阶段单独记录各帧候选，包括未接受的姿态，不能当作关节角度样本。
|
||||
报告 `unit_quality` 分别记录原始观测覆盖与 `accepted_pose_quality` 可信姿态覆盖；
|
||||
`joint_zero_status`、`missing_joint_zero_references` 列出各关节真实零位状态，`motion_initialization_summary` 保留各候选的关节残差。
|
||||
进度显示原始观测覆盖，并单列可信姿态数量和未确认关节；暂停时保持“已暂停”，不会被零位动作名称覆盖。
|
||||
可信姿态不足会在退出时明确提示。相邻帧姿态跳变和图像身份用于定位问题，诊断完成不代表标定精度通过。
|
||||
每方向的 `observation_timing` 分别统计原始/可见/同步图像数、缺命令与缺反馈图像数、最大图像及同步观测间隔；
|
||||
缺命令与缺反馈可以发生在同一帧，不能相加当作总丢帧数。少于两帧的间隔记为 `null`。
|
||||
方向失败时这些信息同时进入暂停诊断。反馈跨度 100% 仅表示两端都有观测，不代表中间没有缺口。
|
||||
原始帧的 `timing` 保存回调接收、历史快照及最新命令/反馈时间戳，用于区分消息延迟与状态匹配问题；
|
||||
接收到快照的时间包含线程调度和锁等待，不能单独解释为写盘耗时。未匹配反馈时同步误差为 `null`，不会显示为 0 ms。
|
||||
可额外加 `--record-bag` 保存现有录制话题；仅检查入口与配置可加 `--validate-only`，不会启动硬件。
|
||||
|
||||
诊断始终完整新采，不复用断点，不拟合或发布标定 JSON/URDF，也不更新发布指针。
|
||||
原始会话带有诊断标记，正式离线拟合和断点发现会拒绝将其用作正式标定数据。
|
||||
当前选项只开放给 O6 右手；其他型号仍走原来的正式流程。原始 URDF、Profile 和设备配置无需修改。
|
||||
诊断任务预设在 `profiles/diagnostics.py` 声明，运行时只选择既有 Profile 任务，复用公共调度与采集组件。
|
||||
|
||||
诊断采集完成后，runner 先关闭相机和 SDK 进程，再自动运行公共图像几何分析，生成独立的
|
||||
`diagnostic_analysis.json` 和可直接阅读的 `diagnostic_analysis.md`。耗时优化不进入实时状态锁或相机回调。
|
||||
分析按任务、机位、方向和尝试分别保留原始图像身份，比较所有候选族的三维 PnP 残差、深度误差比例、
|
||||
图像域关节链拟合以及隔帧和前后区间验证。验证时几何参数冻结，只从当前图像反解几何角度，不使用 SDK 角度映射。
|
||||
图像误差小不等于毫米精度通过，优化收敛也不代表分支唯一；分析结果不能写回跟踪器、零位或正式标定产物。
|
||||
未采到的方向和旧会话缺少的归零前证据会明确列出，不拼接不同会话，不使用第四轮重新拟合。
|
||||
图像分析失败或被取消时，已完成的采集及原始文件仍保留，不会改标成 SDK 故障。
|
||||
|
||||
已保存的数据可直接离线复算，无需启动相机或重复运动：
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration analyze_calibration_capture \
|
||||
--config src/linkerhand_calibration/config/o6_right_product.yaml \
|
||||
--raw calibration_output/O6_RIGHT_001/20260914_192637/raw_samples.jsonl \
|
||||
--output calibration_output/O6_RIGHT_001/20260914_192637/image_review
|
||||
```
|
||||
|
||||
这些修改解决诊断采集被未确认零位阻塞及报告不完整的问题。现有真实 IP 数据仍有分段几何不一致，
|
||||
正式分支门槛、拟合器、URDF 修正和独立精度验收保持原要求；软件回归不能替代实机精度结论。
|
||||
|
||||
### O6 右手 yaw 方向差保持诊断
|
||||
|
||||
需要区分“到达指令后仍在缓慢稳定”和“保持后仍存在方向差”时，使用独立选项：
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration calibrate_hand \
|
||||
--config src/linkerhand_calibration/config/o6_right_product.yaml \
|
||||
--diagnostic-yaw-hold
|
||||
```
|
||||
|
||||
此模式仅选择 `thumb_yaw_top`，只启动顶部机位，使用固定基准 ID6 和运动 Tag ID7。
|
||||
每次 Start 都重新采集会话基准并尝试确认 yaw 零位，不复用旧会话；同一会话的两个方向共用首次冻结零位，
|
||||
不会分别归零。未能确认零位时按诊断规则保留原始观测，并明确标记无法生成可信角度。
|
||||
|
||||
在原速度和行程下执行一次往返。每个方向扫描后先回到该方向的起点，再沿同一方向访问三个保持点：
|
||||
递减方向为 `255 → 192 → 128 → 64`，递增方向为 `0 → 64 → 128 → 192`。
|
||||
每个保持点在指令轨迹完成后至少保持 10 秒，两方向合计六次保持；反馈确认可能使实际等待稍长。
|
||||
保持过程继续记录图像角点、候选、实际指令和同步反馈，用于比较到达后的角度随时间变化及双向最终差异。
|
||||
保持阶段的原始观测单独标记，不计入连续扫描的覆盖统计；遮挡、设备故障、中止及安全收尾仍沿用公共策略。
|
||||
|
||||
`--diagnostic-yaw-hold` 与 `--diagnostic-thumb` 互斥，不能同时使用离线拟合、发布或禁用运动选项。
|
||||
可加 `--validate-only` 只检查入口与配置，或加 `--record-bag` 留存现有录制话题。
|
||||
结果保存在 `raw_samples.jsonl` 和 `diagnostic_capture.json`，记录保持指令及时间；
|
||||
不拟合或发布标定 JSON/URDF，不形成正式断点，也不算精度验收。
|
||||
本诊断用于查清实机稳定时间和方向差，不改变最终单值 `angle_rad` 的验收要求。
|
||||
|
||||
## 启动前相机检查
|
||||
|
||||
`--validate-only` 检查相机配置文件、序列号、内外参指纹和已有外参质量记录，不采集现场图像。
|
||||
正式启动还检查实时 `CameraInfo` 的宽高及 K/D/R/P 是否与受保护外参中的记录一致；
|
||||
参数不匹配、无效或消息过期时不能开始，并显示具体机位和原因。
|
||||
|
||||
**配置匹配不等于现场内外参有效。** 当前只有手上的 Tag，各机位使用不同 Tag，
|
||||
没有声明跨机位共视或已知的 Tag 间几何关系；手掌和贴纸又允许在开始前移动、重贴。
|
||||
因此光学内参与相对外参的现场有效性记录为 `unverified`，不能用重新锁定掌心 Tag、
|
||||
较小的 PnP 残差或未变化的配置哈希代替实测，也不能由 Tag 位置变化断定相机被碰。
|
||||
|
||||
`READY` 只表示设备与消息满足现有开始条件。显示上述能力提示后沿用现有启动行为，
|
||||
不要求各手指 Tag 可见，不增加人工确认或新的暂停条件；`READY` 不表示物理相机参数已验证通过。
|
||||
会话目录中的 `camera_preflight.json` 和 `raw_samples.jsonl` 保存检查结果与依据，
|
||||
开始前仅在结果变化时记录，接受 Start 前再检查并冻结一次；后续状态中的检查结果指这次启动检查。
|
||||
单凭这些记录不能判定是否需要重新标定。`expected_cameras` 是配置期望身份，不是现场实测结果。
|
||||
|
||||
镜头、焦距或成像设置变化后应重新核验内参；相机之间相对位置变化后应重新核验外参。
|
||||
未来同一个物理 Tag 被多个机位共同看到时，可以检查相对外参一致性，但当前尚未实现该能力。
|
||||
本次检查不修改 Profile、SDK 或原始 URDF。
|
||||
|
||||
## 基准、避让与恢复
|
||||
|
||||
- 开始前可以调整手掌、相机和 Tag；预览数据不会成为正式参考。
|
||||
- 开始后清空预览缓存，先安全回基准，再为每机位固定 Tag 收集至少 10 个有效帧。
|
||||
缺少基准时保持并显示缺少的 ID，不因为等待时间长暂停。
|
||||
- 锁定后禁止人工移动手掌、相机、支架、Tag 粘贴位置；程序驱动关节和避让仍是允许的。
|
||||
- 全手 baseline 时只要求固定参考可见，不要求四指运动 Tag 同时可见。
|
||||
每关节在 `motion.joint_zero_references` 指定任务前，按声明的到达姿态和避让分组回到自身 baseline，
|
||||
反馈稳定后收集至少 10 个稳定视觉帧，冻结 `JointZeroReference`。此后方向、轮次、重扫均不重新归零。
|
||||
例如 G20 食指开始前，已完成四轮的其他手指可以保持非零避让；这些完整指令和反馈写入参考。
|
||||
task.start 不代表零位，例如侧摆 baseline=127、扫描起点=255，会分别执行两种姿态。
|
||||
- 相机相对位置改变时须重新确认/标定外参；仅重新锁定掌心 Tag 不能替代外参标定。
|
||||
- 固定 Tag 被避让遮住时可用已锁定参考继续采集；软件不能保证检测完全不可见对象的移动。
|
||||
运动 Tag 的刚性和几何一致性仍须通过最终独立验证。
|
||||
- 采集断点版本为 `unified_engine_v8_all_view_images`；另以 `pose_tracking_policy_version=confirmed_branch_v8_training_model_selection`
|
||||
记录姿态分支策略。缺少此字段或版本不同的旧断点不能自动或手工恢复到新会话，历史文件保持原样供离线诊断。
|
||||
首先检查配置哈希与整场固定参考；各关节轮到时，在同一完整保持姿态核对原零位与父子 Tag 位姿
|
||||
（5 px、2°、5 mm），通过后才导入对应已完成扫描单元，并保留原零位记录。
|
||||
整场参考不匹配则放弃旧数据；逐关节安装验证失败则暂停，禁止在旧样本上新建零位拼接。
|
||||
排除原因后用 `--no-resume` 开始新会话。旧会话文件不修改。
|
||||
旧日志的读取、凭据回放与单元质量检查在实时回调启动前完成，不占用运动/反馈回调。
|
||||
现场验证通过后，每个控制周期最多导入 64 条历史记录；全部落盘后才跳过对应单元。
|
||||
导入期间持续处理反馈、固定基准监测和中止，原 1 秒反馈超时保护不变。
|
||||
|
||||
连续图像、候选和样本按图像批量追加并刷新到系统缓存，避免持有控制状态锁逐条 `fsync` 阻塞反馈和运动定时器。
|
||||
固定参考、关节零位、方向完成和暂停记录仍强制同步到磁盘；拟合、报告哈希、发布和关闭前另有同步屏障。
|
||||
断点只复用通过且持久化的完整方向，意外断电时未完成方向不能拼入旧数据；同步失败不得发布。
|
||||
此存储策略不改变 50 ms 图像/状态配对门限、方向覆盖门限、拟合公式或精度验收条件。
|
||||
|
||||
O6/L6/G20 标定读取 SDK 的 `*_hand_state_timed` 反馈接口；位置与每个通道的 CAN 接收时间
|
||||
一同保存。G20 五指的分帧反馈分别插值到图像时间，重复发布的缓存不能刷新反馈超时或增加
|
||||
稳态独立样本。标准 `JointState` 仍可供其他程序使用，其时间戳保留测量时间。
|
||||
`sdk_feedback_sample` 原始记录支持复查时间配对；旧版本只保存发布时间，不能事后补成真实
|
||||
CAN 测量时间，也不能与新版断点混用。O12 继续使用独立 HCAN 同步读取接口。
|
||||
|
||||
共同坐标约定：`q_CAD = q_output + delta`；每个关节在自身冻结 baseline 时 `q_output=0`。
|
||||
局部零位通过采样时的完整保持姿态和运动链进入同一掌部参考。固定 Tag 不提供未知末端连杆的 CAD 朝向。
|
||||
`zero.known_baseline_geometry` 可声明有独立依据的 CAD 角度、误差界与来源；不能填入猜测的 0、旧 mimic
|
||||
或 Tag 安装偏角冒充几何证据。相应 `origin.rpy` 和限位修正仍须在 Profile 中明确授权。
|
||||
|
||||
O12 避让：小指和无名指到 Profile 的最大弯曲指令；标定食指前，中指 MCP/PIP 到最大弯曲指令，
|
||||
中指侧摆为 0 rad。到位采用完成轨迹、正确方向、至少 80% 请求位移和短时稳定，
|
||||
不要求反馈数值精确等于命令。SDK 指令最大值与实测 URDF 角度不是同一个量。
|
||||
|
||||
O12 使用 HCAN device 0/channel 0 的已锁定 vendor Python wheel，不检查 `can0`。
|
||||
连接参数由本包的 `config/o12_sdk.yaml` 保存，并通过产品配置中的 SHA256 校验；
|
||||
不依赖第三方 SDK 子仓库中未提交的示例 YAML 修改。
|
||||
只读反馈桥可在第一次运动命令之前取得状态，不会发送零位命令来“激活回读”。
|
||||
POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 错误位过热保护，不伪造温度。
|
||||
字节 SDK 当前没有独立硬件故障遥测，状态会明确注明这一能力边界。
|
||||
|
||||
## 哪些情况暂停
|
||||
|
||||
实时仅因人工中止、竞争控制器、活动硬件故障/模式错误、真实失联、反馈超过 1 秒未更新、
|
||||
物理越限、明显运动要求下连续 2 秒无推进、已锁固定基准连续 10 帧漂移超过 5 px 而暂停。
|
||||
开始后相机投影参数变化也会拒绝继续使用混合坐标数据。
|
||||
当前关节持续缺失零位观测会暂停并列出关节、机位和所需 Tag；尚未轮到的手指遮挡不阻塞当前任务。
|
||||
零位和稳态点的反馈稳定窗口以最新独立反馈为终点，并保留跨越窗口起点的一条反馈。
|
||||
控制定时器的调度抖动或重复读取同一反馈不会清空稳定观测；真实反馈波动或反馈过期仍会使当前窗口失效。
|
||||
零位仍需至少 10 帧且姿态波动不超过 1°；拟合验收门限不变。
|
||||
|
||||
短时 Tag 丢失、PnP/同步失败、低检测率/低反馈 Hz、正常跟随滞后、固有耦合和非目标小幅运动
|
||||
不会实时中断轨迹。无效视觉帧直接丢弃。
|
||||
|
||||
所有型号的独立 Tag 姿态筛选共用 `core/geometry/pnp.py` 中的近似同误差阈值，默认 `0.03 px`。
|
||||
ROS 和离线采集使用相同默认值，型号 YAML 不重复声明该值。`1.5 px` 是图像质量拒绝阈值，
|
||||
不能用它判断两种姿态是否同样可信；仅在误差差值不超过近似同误差阈值时使用时间连续性。
|
||||
分支容差覆盖值必须有限、非负且小于图像质量拒绝阈值。
|
||||
|
||||
独立 Tag 首次接受姿态前，需要默认至少 5 帧、跨越至少 0.1 秒的图像证据确认。
|
||||
近似同误差的候选只有旋转相差不超过 1°、平移相差不超过 1 mm 时才视为同一姿态；这些参数统一定义于
|
||||
`core/geometry/tag_pose/parameters.py`，ROS 通过 `pnp_branch_confirmation_frames`、
|
||||
`pnp_branch_confirmation_minimum_seconds`、`pnp_branch_equivalence_deg` 加载，离线采集使用相同默认值。
|
||||
这只是图像分支的一致性确认，不能把多帧低重投影误差视为物理姿态真值。
|
||||
|
||||
以下独立图像跟踪规则保留用于预览和历史兼容；新会话的运动 Tag 首次零位还必须通过后述公共运动几何确认。
|
||||
参考冻结前，明显更优但与历史姿态发生大跳变的新分支也必须经过确认，过渡帧不进入零位或拟合样本。
|
||||
运动 Tag 参考冻结后，若原分支仍有连续、图像质量合格的候选，但另一候选的重投影误差明显更低，
|
||||
该帧标为 `pose_branch_ambiguous` 并丢弃，内部保留原分支轨迹等待图像证据恢复；
|
||||
不能仅凭另一候选连续数帧得分更高，就判定冻结参考损坏。不会切换分支、重建零位或把歧义帧送入拟合。
|
||||
如果持续不存在符合原分支连续性约束的候选，仍在确认后暂停；长期失联后无法确认分支一致时也会明确拒绝。
|
||||
正式标定方向结束仍检查可信样本、覆盖与稳态点，最多同速重扫一次；诊断模式另报原始观测与可信姿态覆盖。
|
||||
需要更换分支或重新安装 Tag 时，必须重新开始新会话，
|
||||
不能把旧冻结参考与新分支数据拼接。原有 0.03 px 容差、35° 跳变、75° 倾角及 1.5 px 重投影质量阈值保持不变。
|
||||
真实角点噪声、采集时序和最终运动精度仍须通过独立实测验证。
|
||||
|
||||
### 首次零位前的公共运动几何确认
|
||||
|
||||
新生产会话复用 Profile 已声明的**最后一段归零准备运动**，在 `zero_approach` 中保存父子 Tag 的原始候选，
|
||||
到 `joint_zero` 后先确认分支，再采集原有的稳定零位帧。没有增加扫描轮次、额外试探动作或型号专用求解器。
|
||||
几何确认与 SDK 反馈稳定分别检查；几何通过不会提前开启零位采集窗口。
|
||||
计算在状态锁外进行，期间持续接收 SDK 反馈并保持声明姿态；计算本身有 120 秒上限。
|
||||
几何确认后，先在原有 2 秒观察窗口内等待恢复后的首个有效零位样本，再用
|
||||
`joint_zero_timeout_seconds` 限定的独立窗口采齐稳定帧（字段默认 2 秒，四型号产品配置均为 5 秒）。
|
||||
窗口不会无限延长。正式采集中，每组尚未冻结零位的关节最多自动恢复一次：可恢复的图像
|
||||
准备失败重新执行该组原有的归零准备路径并采集全新图像；几何已经冻结但零位图像不足时，
|
||||
保持原姿态和模型，只重新开启一次零位图像窗口。原始失败和恢复动作写入 `joint_zero_recovery`。
|
||||
已冻结的零位、部分已经提交的多视角几何、保持姿态变化、设备故障不重新拟合；恢复后仍
|
||||
不足时明确暂停，保留原有反馈、固定基准保护及用户中止能力。
|
||||
中止、暂停或运动版本改变后，迟到的计算结果不能提交。
|
||||
前面的避让段、预览、训练扫描和第四轮不参与首次确认。准备段同时核对其他通道的完整保持指令及反馈；
|
||||
开始后不能改变相机、固定基准或 Tag 安装关系。
|
||||
|
||||
生产入口 `core/geometry/tag_pose/production_image_motion.py` 枚举 IPPE 候选家族，
|
||||
使用父子转动副 `R = exp(theta × axis) R_ref`、`t = pivot + R × mount` 拟合原始角点的重投影残差。
|
||||
稀疏线性子问题使用 `atol=btol=1e-10`，避免近似计算误差导致外层耗尽迭代预算；
|
||||
外层收敛条件、默认 150 次迭代预算和各项精度门限保持不变，记录每个候选的求解状态及迭代次数。
|
||||
父 Tag 本身运动时联合求解整条已声明的观测链；已冻结的上游几何不会重新拟合。
|
||||
角度来自当前图像，不使用 SDK 到角度的先验、旧 mimic 或 CAD 零位假设选择分支。
|
||||
单帧 PnP 深度噪声较大时，不再将其三维平移估计直接作为关节运动的真值。
|
||||
|
||||
当源 URDF 中两个转轴相邻且平行，并且观测 Tag 实际固定在对应连杆上时，
|
||||
`profiles/observations.py` 提取零位修正后仍不变的平行关系和垂直轴距。
|
||||
`CadImageHingeBundle` 消去三个多余几何变量，使准备模型遵守最终 URDF 保留的几何；
|
||||
不修改原骨长/轴向、不增加权重或 CAD 零位假设。不满足条件的观测链使用原模型。
|
||||
新冻结模型保存源 URDF 哈希和约束，最终处理从源文件重新推导核验；旧模型保持可读。
|
||||
修改前的原始图像对照实验及适用边界见 [流程评估](CALIBRATION_PIPELINE_REVIEW.md)。
|
||||
|
||||
准备段至少需要 24 帧，训练与留出图像身份分离，默认各最多 72 帧。
|
||||
训练帧取交错序列，未进入训练的图像才可留出；不因训练帧增加而复用验证图像。
|
||||
局部几何可观测性检查计入逐帧角度的不确定性，轴线与转心的不确定度界分别不超过 3° 和 3 mm。
|
||||
候选只按训练图像的平方误差选择；留出数据不用于选优，所选候选验证失败不能改选另一候选。
|
||||
候选家族还须在准备段留出图像上区分：按运动角度分组，比较与拟合一致的平方误差损失，
|
||||
差值为 `alternative_rms² - (candidate_rms + 0.03 px)²`，保留原像素容差。
|
||||
同时通过单侧均值和符号秩检验，再对完整比较集采用 Holm 逐步校正，名义族错误水平保持 0.01。
|
||||
`image_model_selection.py` 统一负责候选选择、同帧角色对齐、损失比较和多重校正；
|
||||
诊断记录原始/校正后概率、选择策略和实际源 URDF 约束，失败时也保留这些证据。
|
||||
缺失、未收敛或无法评估的替代家族不能被当作已排除；几何等价的家族才可合并。
|
||||
这些检查是图像几何的条件性证据，不是独立毫米精度证明，不能替代第四轮 JSON／URDF 验收。
|
||||
|
||||
确认成功后冻结轴线、参考旋转、转心、安装关系及其来源图像。
|
||||
`image_motion_projection.py` 后续每帧只反解当前图像角度，不重新拟合这些几何参数。
|
||||
保留 1.5 px 重投影、75° 倾角及单帧角度不确定度检查；信息不足或角度仍有歧义时丢弃该帧，
|
||||
按现有方向覆盖和最多一次同速重扫处理。35° 连续性和失联后的确认仍由公共 tracker 检查。
|
||||
Tag 中心接近转轴本身不再是必然失败条件，是否可解取决于全部角点提供的几何信息。
|
||||
准备段暂时缺帧或候选不确定时，每组尚未冻结零位的关节最多沿原归零路径补采一次；
|
||||
已确认几何但零位图像短缺时，保持几何和姿态，只重新打开一次图像窗口。
|
||||
两个恢复方式共享一次额度，保存 `joint_zero_recovery`，不延长成无限重试。
|
||||
多机位已有部分几何冻结、保持姿态改变、设备故障和诊断采集不会触发重新拟合。
|
||||
恢复后仍无法确认时不冻结零位、不发布；已有零位和旧数据不能被重新解释后拼接。
|
||||
|
||||
`motion_branch_observation` 保存准备段图像、角点、候选及 SDK 状态；`motion_branch_initialization` 保存各假设残差、
|
||||
冻结模型、来源图像和内容哈希。新姿态来源为 `image_constrained_current_image`，保留原始 IPPE 候选，
|
||||
同时记录 `frozen_image_model` 与 `image_model_sha256`。每个运动 Tag 的 `motion_evidence_id` 绑定首次证据，
|
||||
`JointZeroReference.branch_references` 将它与零位、机位和分支版本关联。
|
||||
正式读取与收尾校验这些关联，缺少证据、篡改来源或删除表头均不能降级为旧图像确认；
|
||||
`motion_branch_provenance.json` 只证明证据链完整，不是精度证书。
|
||||
恢复时共享来源只导入一次，关节零位及历史验证记录在该关节实际验证后导入，支持恢复期间再次中止。
|
||||
正式回放用同一冻结模型复算同帧角点，核对父链、相机、Tag 尺寸及实际提交姿态，不重新拟合模型。
|
||||
旧 `confirmed_branch_v1/v2/v3` 及 v4 三维候选约束记录仍按原语义读取，不能自动升级或恢复成新 v5 证据。
|
||||
`motion_evidence.py` 的旧三维筛选入口保留供历史兼容和诊断。这里 v5 仅指姿态策略，最终标定 JSON 仍为 schema v3。
|
||||
|
||||
真实 O6 `20260914_175815` 的旧三维筛选只读回放中，yaw 与 pitch 分别存在唯一通过几何条件的家族；
|
||||
但 pitch→IP 的四种父子组合全部超差，IP 转心 RMS 约 4.9–11.4 mm，完整拇指链仍为 `unresolved`。
|
||||
这批图像来自旧零位冻结之后的单轮诊断扫描，只能用于反事实回归，不能代替新准备段证据、
|
||||
覆盖旧零位或证明 JSON＋URDF 精度。
|
||||
可复算摘要见[本次分支修复回放](../../calibration_output/O6_RIGHT_001/20260914_175815/motion_branch_fix_review/README.md)，
|
||||
测试夹具同时保留“yaw/pitch 可分”和“IP 不得强行通过”两类断言。
|
||||
后续 `20260914_200044` 的图像求解回放使用其真实归零准备段冻结模型，后续 838 帧可复算通过当前图像检查；
|
||||
这仍是单轮诊断回放，不是完整四轮精度验收，也不会生成正式 JSON 或 URDF。
|
||||
|
||||
`20260915_104446` 正式实采中，拇指三个关节各冻结 10 帧零位并完成四轮，
|
||||
pitch／IP 的旋转及指令单表子项均通过；yaw 旋转通过，但指令单表 MAE 为 1.0454°，超过 1°。
|
||||
两场独立会话均观察到稳定方向差，该批观测即使允许每个指令任取一个输出角，MAE 下界仍超过 1°。
|
||||
这不是通过重新拟合单表能够消除的误差；方向分支的诊断通过也不能冒充单表通过。
|
||||
详见[拇指实测子项报告](../../calibration_output/O6_RIGHT_001/20260915_104446/motion_holdout_review/README.md)。
|
||||
后续 `20260915_111401` yaw 保持实测在 64／128／192 各双向保持 10 秒；末 3 秒的方向差
|
||||
分别为 2.2965°／2.7885°／2.1248°，每个窗口均有 90 帧可靠观测，同一目标的 SDK 反馈相同。
|
||||
0.5–1 秒至末段的角度变化最多约 0.038°，延长等待没有消除方向差。
|
||||
本轮仅启动顶部机位，不能据此确定具体机械原因或证明整手精度；继续保留单值 JSON 与原验收门槛。
|
||||
复现脚本、时间曲线与来源哈希见[双向保持实测](../../calibration_output/O6_RIGHT_001/20260915_111401/yaw_hold_review/README.md)。
|
||||
`20260915_104446` 会话在小指准备求解超时后停止，未完成整手空间／FK 验收、未发布 JSON／URDF。
|
||||
小指同源准备段离线求解只需约 3.42 秒，但候选仍未达到统计区分要求；运行时超时来源尚待定位，
|
||||
详见[小指准备段核查](../../calibration_output/O6_RIGHT_001/20260915_104446/side_preparation_review/README.md)。
|
||||
后续会话另存 `motion_solver_diagnostic.jsonl`，记录请求来源哈希、版本、求解耗时和提交接受／拒绝，
|
||||
以区分求解未返回与迟到结果被丢弃；该文件不参与测量授权，也不修改已冻结的原始记录。
|
||||
此诊断接线在上述实采之后添加，不能补证该次超时原因。
|
||||
|
||||
固定基准另有明确的静止约束：锁定后始终使用首次冻结的相机局部姿态，将其投影到当前角点验证，
|
||||
不因 IPPE 两支重投影误差排名反转而更换姿态或暂停。仍执行原有 1.5 px 重投影和 75° 倾角限制，
|
||||
并保留当前 IPPE 候选与原有角点漂移监测。当前图像无法验证冻结姿态时丢弃依赖该基准的观测，
|
||||
记录 `fixed_reference_pose_unverified`，不将几帧图像质量不合格判为不可恢复的姿态分支冲突。
|
||||
恢复须默认至少 5 个合格帧且跨越至少 0.1 秒，始终核验同一冻结姿态，恢复完成前也禁止缓存兜底。
|
||||
尚未使用该机位的动作可以继续；当前任务持续缺少有效数据时，仍按原有零位超时、稳态采样和最多一次同速重扫处理。
|
||||
固定基准实际连续 10 帧漂移超过 5 px 仍使整场暂停，包括当前任务未使用的机位。
|
||||
此约束仅用于 Profile 声明的 `fixed_reference`，
|
||||
不用于随关节运动的 Tag，也不证明初始分支物理正确或当前外参已经实测验证。
|
||||
|
||||
方向结束才检查 ≥40 个有效同步样本、≥32 分箱、内部空白 ≤行程的 1/16、规定的有效行程覆盖，
|
||||
以及 Profile 声明的端点/双视角观测。数据不足只同速重扫一次；第二次失败暂停。
|
||||
拟合或最终文件验收失败不重新自动运动、不更新发布指针。
|
||||
`independent_rotation_holdout_failed` 表示前三轮冻结旋转模型未通过第四轮检查，发生在空间求解前。
|
||||
错误原因包含失败关节、机位和 MAE/P95/最大误差；`fit_diagnostics.json.rotation_holdout`
|
||||
另存完整 SO(3)、轴向、偏轴统计及最坏图像身份。验收使用完整旋转误差，不能仅凭投影角度曲线重复判定通过。
|
||||
新采样行的 `pnp_observation_evidence` 保存父子 Tag 的当前角点、尺寸、已选姿态和候选诊断，
|
||||
相机矩阵引用同会话的 `rectified_camera_model`。缓存固定基准明确标注来源，不伪造当前帧的角点或候选。
|
||||
可见且通过冻结姿态投影验证的基准标为 `verified_fixed_reference`，保存当前角点、IPPE 候选和实际投影误差；
|
||||
完全未检测到时的缓存仍标为 `cached_fixed_reference`。候选回放保持同一冻结基准并复核前者的当前角点证据。
|
||||
短程诊断报告以 `fixed_reference_summary` 单独统计基准的当前图像投影误差，
|
||||
不会把固定输出姿态的零跳变误当作独立测量结果。
|
||||
不合格姿态事件另外记录当时的动作阶段、指令与同步反馈，便于区分准备动作期间的异常与正式采样异常。
|
||||
角点与候选记录用于诊断；其中分支状态、版本及实际选中姿态也用于核验冻结参考的一致性，
|
||||
不使历史采集自动获得缺失的观测证据。O12 的轨迹几何评分仍只使用训练轮，
|
||||
但正式收尾不得用评分更好的另一支替换已冻结的训练或第四轮姿态；超过同一公共等价界时明确拒绝发布。
|
||||
历史记录仍可在离线诊断入口分析候选，不能借旧格式标记绕过正式产物的冻结分支检查。
|
||||
|
||||
所有型号显示相同进度：阶段、任务/轮次/方向、分机位 Tag ID、命令/反馈/单位/速度、
|
||||
有效数据、基准、断点来源，以及中文原因、建议和原始诊断。
|
||||
ROS 收尾拟合在独立进程执行：只读取采集结束时已同步的日志前缀，后续图像不进入拟合；
|
||||
主进程继续接收反馈并检查固定基准，阶段事件按原顺序回传，最终发布仍由主进程持锁完成。
|
||||
中止时终止计算子进程，不允许子进程更新发布指针。`finalization_timing.json` 保存实际耗时。
|
||||
标题明确显示型号、左右手和序列号。开始前等待状态列出缺少或过期的相机内参、检测消息和 SDK 条件；
|
||||
空 Tag 检测消息可以证明视觉链路工作，不要求开始前已经识别全部 Tag。
|
||||
收到开始请求时重新检查设备状态；实时 CameraInfo 参数必须与受保护外参中的指纹一致。
|
||||
开始前相机消息的新鲜度窗口为 2 秒，该窗口不用于扫描中的实时暂停。
|
||||
|
||||
## 唯一生产代码链
|
||||
|
||||
```text
|
||||
linkerhand_calibration/
|
||||
├── config/profiles/ 唯一型号定义（受保护 YAML）
|
||||
├── config/*_product.yaml 设备、序列号、相机、路径与哈希
|
||||
├── urdf/ 原始 CAD/mesh，不覆盖
|
||||
└── linkerhand_calibration/
|
||||
├── profiles/ 加载、静态验证、Tag/CAD 观测关系
|
||||
├── core/domain/ Profile、样本、CalibrationResult、状态
|
||||
├── core/geometry/ 相机、变换、PnP 兼容导出
|
||||
│ └── tag_pose/ IPPE、连续跟踪、冻结前运动证据、刚性组和轨迹选择
|
||||
├── core/fitting/ 运动曲线、线性耦合、固定安装
|
||||
│ └── spatial_solver/ 空间观测、基座、零位、统计与独立验收
|
||||
├── core/urdf/ 授权修正、FK、范围与最终文件验收
|
||||
├── runtime/runner.py 唯一产品启动器
|
||||
├── runtime/coordinator.py 会话协调、采集提交、运动和收尾接线
|
||||
├── runtime/parameters.py 已解析的运行参数；无 ROS 类型
|
||||
├── runtime/inputs.py 相机/检测输入、时钟与发布接口
|
||||
├── runtime/cameras.py 受保护相机身份与消息新鲜度
|
||||
├── runtime/camera_preflight.py 启动前配置检查结果与内外参实测能力边界
|
||||
├── runtime/session.py 唯一在线状态机
|
||||
├── runtime/execution.py Profile 任务与运动效果调度
|
||||
├── runtime/motion_execution.py 平滑轨迹与位移到位判定
|
||||
├── runtime/capture.py 多机位视觉/反馈采集
|
||||
├── runtime/branch_initialization.py 最后归零准备段证据与一次性几何确认
|
||||
├── runtime/branch_tracking.py 冻结约束选候选，原子提交同帧姿态与证据
|
||||
├── runtime/motion_provenance.py 读取和收尾核验来源内容、零位与样本关联
|
||||
├── runtime/scan_quality.py 唯一方向数据门
|
||||
├── runtime/reference_lock.py 开始后固定基准
|
||||
├── runtime/resume.py 整场断点证据检查
|
||||
├── runtime/joint_resume.py 逐关节原零位验证后导入旧样本
|
||||
├── runtime/safety.py 最小实时保护
|
||||
├── runtime/status.py 统一状态与中文显示
|
||||
├── runtime/snapshot.py 类型化状态快照与旧状态映射
|
||||
├── runtime/adapters/ SDK 协议，不包含标定业务
|
||||
├── runtime/ros/ ROS 参数加载、消息转换、订阅/服务/定时器
|
||||
├── runtime/artifacts/ 收尾控制、后台 finalizer、serializer、原子发布
|
||||
└── compat/ 历史格式、布局与离线诊断；不提供旧在线流程
|
||||
```
|
||||
|
||||
`calibrate_hand → runner → runtime/ros/entrypoint → UnifiedCalibrationNode → CalibrationCoordinator → SessionExecution`
|
||||
是所有型号的生产调用链。节点直接继承 ROS `Node`，组合 `RosCalibrationIO` 和协调器，
|
||||
不通过父子类业务回调执行标定。SDK 绑定接收明确的反馈新鲜度、时钟、健康订阅和发布接口。
|
||||
|
||||
空间入口 `core/fitting/spatial.py` 保留显式兼容导出，计算顺序在
|
||||
`spatial_solver/solve.py`：输入准备 → 仅训练求解 → 可观测性/轮间统计 → 冻结结果的独立验证 → 结果装配。
|
||||
`TrainingProblem` 不包含第四轮观测，训练几何中的掌部姿态也按训练轮筛选。
|
||||
基座策略由 Profile 的几何约束选择；曲线、零位坐标约定、权重、阈值和原始 URDF 授权修正规则不变。
|
||||
|
||||
PnP 的原导入路径继续可用。`tag_pose/parameters.py` 集中定义跟踪默认值，
|
||||
ROS 加载器将角度参数由 deg 转为 rad，离线采集直接使用同一套默认值。
|
||||
公共分支容差仍为 `0.03 px`，重投影质量和倾角限制保持一致；首次确认、参考冻结与失联恢复
|
||||
使用同一公共分支策略，ROS 和离线链路均不能绕过它。
|
||||
|
||||
`CalibrationSession.phase` 是流程状态来源。协调器在状态锁内开始、中止、推进和提交观测；
|
||||
耗时姿态计算在锁外进行，提交时核对会话版本、采集对象、运动对象及稳态采样边界。
|
||||
Start 更换预览采集对象并重新锁定基准，暂停/中止使尚未完成的旧观测失效。
|
||||
收尾由 `FinalizationController` 组合 worker 和发布器，worker 只上报阶段事件、准备候选文件；
|
||||
协调器核实全部验收阶段后才能提交。锁顺序固定为状态锁 → worker 锁，
|
||||
拟合失败维持当前姿态，中止后禁止提交，成功产物只提交一次。
|
||||
|
||||
老的单相机拇指/CMC 在线节点和 launch 已退出安装入口；需要局部测量时应增加经过验证的 Profile，
|
||||
不能重新启用旧状态机。老版本离线诊断不是新的标准 URDF 发布证据。
|
||||
|
||||
## 新增型号
|
||||
|
||||
必需文件：
|
||||
|
||||
```text
|
||||
config/profiles/new_hand.yaml
|
||||
config/new_hand_product.yaml
|
||||
urdf/new_hand/raw.urdf
|
||||
urdf/new_hand/meshes/*
|
||||
```
|
||||
|
||||
Profile 声明：
|
||||
|
||||
1. Adapter、SDK 通道顺序、单位、正负方向、命令/反馈范围、基准值、独立速度槽布局。
|
||||
2. Tag ID/尺寸/机位/安装 link，以及每次运动的父子观测关系；不明确的 link 不能靠名称猜。
|
||||
3. 扫描任务、速度、准备/避让/收尾 waypoint；每关节首次零位任务、完整保持指令及固定到达姿态。
|
||||
4. 声明空间可观测约束、已知 baseline 几何或明确接受的 CAD 零位假设；每个运动关节须独立观测或显式指定对应实测来源。
|
||||
5. 每关节允许修正的 `origin.rpy`、`limit.lower/upper`、`mimic.multiplier/offset`。
|
||||
|
||||
这仍需要正确的可观测性设计：单轴加一对未知安装角的 Tag 不能自动辨识绝对 CAD 零位。
|
||||
不能为了减少配置，默认把 SDK 零点或最大指令当成 CAD 零位。可参考最简单的 L6/O6 Profile，
|
||||
删除不适用关系后重新声明；不要盲目复制另一型号的空间约束。
|
||||
|
||||
复用 SDK 不增加 Python；新协议只增加 `runtime/adapters/` 实现及协议工厂注册。
|
||||
没有型号节点、runner、状态机、质量/断点/发布器；新数学只能增加通用拟合原语。
|
||||
新型号使用 `artifacts.output_schema_version: 3`；产物为
|
||||
`format: unified_calibration_v3, schema_version: 3`。详细双映射保留在独立标定报告中。
|
||||
旧 generic v1、v4/v6/v7 只保留历史兼容工具，不把反馈表改名为指令表来伪造兼容。
|
||||
v1/v2 按原语义读取，不会自动升级为独立实测；保留的 v2 数学入口用于历史回归，不能证明 v3 精度。
|
||||
配置错字、错误绑定、未知修正字段在启动前失败；修改受保护文件后应审查并更新对应 SHA256。
|
||||
|
||||
## 产物与离线验证
|
||||
|
||||
会话目录保存原始样本、冻结 Tag 安装、拟合诊断、标准 URDF 验收、JSON、URDF。
|
||||
有效发布由 `release_manifest.json` 及产品发布指针共同证明；单独的 `PASS` 摘要或存在 URDF 文件不等于已发布。
|
||||
manifest 记录 JSON、URDF、标定报告三文件 SHA256、受保护输入和独立 holdout 证据。任一验收失败都不会替换原发布指针。
|
||||
`measurement_contract.json` 提前列出 Tag/link、零位观测关系、CAD 保留/迁移与原始 mimic 限位冲突。
|
||||
主 JSON 以原始 URDF 关节名为键，单值关节包含 `sdk_channel` 和 `angle_rad`；
|
||||
O12 等 rad 输入还包含一一对应的 `input_values`（在有效节点间线性插值，不外推）。
|
||||
字节型号每条曲线固定 256 项：SDK 指令 `v` 对应所选曲线的第 `v` 项，只能在训练指令实际覆盖 0–255 时导出。
|
||||
输出均是修正 URDF 的 q_output，不再加零偏。顶层记录身份、单位、完整 `baseline_command` 和共同零位约定。
|
||||
独立观测的主动和被动关节直接导出各自视觉曲线，同一驱动通道可绑定多个关节。
|
||||
显式复制的关节使用来源关节的同一组节点与角度，不重新拟合、归零或由 mimic 推导；接收通道保持不变。
|
||||
默认单值曲线在指定的稳态训练数据中约束 baseline=0，正反向诊断分支共用冻结参考、保留回差。
|
||||
独立稳态验证分别检验两个方向；超差时禁止发布，不隐藏切换分支。
|
||||
|
||||
O6 侧摆 `rh_thumb_cmc_yaw` 通过 `artifacts.directional_command_joints` 显式启用双向指令曲线。
|
||||
v3 JSON 顶层保存同名声明，所列关节额外包含 `increasing_rad` / `decreasing_rad`；
|
||||
方向指原始 SDK 数值增加/减少，与关节角度正负无关。其他关节继续使用单值表。
|
||||
两条曲线分别只用所声明的稳态训练数据拟合，共用已冻结零位、各自满足 baseline=0；
|
||||
独立稳态验证按实采方向读取最终序列化曲线，仍要求 MAE≤1°、P95≤2°、最大≤3°。
|
||||
报告分别保存运动与指令映射的训练／验证分区及来源图像，限位推导仍不得使用任一验证分区。
|
||||
`angle_rad` 保留为均值参考；双向关节在非 baseline 输入且方向未知时禁止用均值代替。
|
||||
读取器须从 baseline 初始化，停住时保持到达方向，允许端点换向；行程中途反转尚无小回环标定证据，会明确拒绝。
|
||||
字节指令的方向判断与查表共用 `floor(v+0.5)` 量化规则；连续小数步进不能绕过方向切换检查。
|
||||
两条主回环曲线不构成任意反转、负载或多轴工况的精度证明。旧读取器会拒绝新增关节字段，须同步更新。
|
||||
修正 URDF 保留原被动关节 `<mimic>`，单独拖动父关节滑条时，末节按
|
||||
`q_child ≈ multiplier × q_parent + offset` 联动。标准 URDF 不能将查找表表达为 mimic。
|
||||
前三轮成对视觉角度用于拟合线性近似，优化变量为父关节两端对应的子关节角度；
|
||||
两端都限制在子关节原有输出范围内，因此整个联动行程合法，无需放大限位或裁剪小负角。
|
||||
偏移是近似直线在 baseline 的误差，不能当作新的物理零位；报告明确保存
|
||||
`baseline_error_rad`、`physical_zero_correction: false` 和独立留出误差。
|
||||
JSON 指令/反馈曲线、几何零位不因近似拟合改变。需要实测非线性运动时使用完整 JSON，
|
||||
消费程序显式设置全部关节角，并避免 URDF mimic 再次覆盖末节角度。
|
||||
|
||||
原始 CAD 限位默认仍是硬边界。Profile 可逐关节显式声明 `calibrated_motion_envelope`，
|
||||
将该 CAD 限位视为本次要测定的运动范围；只有已授权的 `limit.lower/upper` 可据此修改。
|
||||
O6 右手采用此策略，范围由前三轮运动反馈映射、独立稳态训练映射的实测包络及共同零位确定，
|
||||
不增加安全余量，不用第四轮扩展范围，也不为容纳 mimic 越界而放大范围。
|
||||
SDK 安全指令域不变，IP／DIP 的 CAD 零位不变。复制关节的范围证据仍属于来源关节。
|
||||
报告和 manifest 的验收数据保存 `motion_range_evidence`，包含来源限位、零偏、训练轮次、
|
||||
样本身份与新范围，并在收尾重新验证。O6 尚无独立机械安全角度界的实测证据，
|
||||
该策略不代表对碰撞、过载或任意多轴姿态作出安全认证。
|
||||
|
||||
验收分别记录：最终 JSON 对独立稳态验证角度；完整 JSON＋URDF FK 对独立验证 Tag 位姿；
|
||||
仅使用 URDF mimic 的近似损失。前两项决定实测精度通过，第三项如实报告，不冒充非线性精度。
|
||||
manifest 同时保存 `urdf_joint_angle_source: calibration_json`、`urdf_mimic_policy` 和两种回放结果。
|
||||
门限保持角度 MAE≤1°、P95≤2°、最大≤3°，Tag 位置 P95≤3 mm。
|
||||
平行转轴的零位计算先消除轴上参考点的任意坐标，再比较相机平面中的轴间方向；
|
||||
改变同一轴线的表示点不能改变标定零位。这是所有型号共用的几何规则。
|
||||
独立精度结论只覆盖实际观测的关节和 Tag；复制关节另检查来源、查表一致性、通道与机械范围，
|
||||
不把来源关节的第四轮误差称为接收关节精度。混合产物 manifest 使用
|
||||
`measured_and_transferred_json_urdf_v3`，列出实测和复制覆盖。
|
||||
FK 使用训练冻结的安装与基座；第四轮不拟合零位、曲线、Tag 安装或基座。
|
||||
正式图像采集还要求核验第四轮及独立稳态验证的原始 Tag 四角，沿用冻结的安装、基座
|
||||
和受哈希保护的相机参数。主观测的独立三维位姿必须解释同帧原始角点，四角重投影 RMS
|
||||
不得超过原 1.5 px；最终落盘 JSON＋修正 URDF 与独立位姿的差异仍按上述角度和位置门限检查。
|
||||
最终 FK 的总像素误差单独报告，不能将包含允许运动误差的总量等同于图像测量噪声。
|
||||
额外机位没有独立三维测量时，仅检查是否存在满足原物理误差范围、且重投影误差不超过
|
||||
1.5 px 的局部刚性位姿。这个数值见证不是真实位姿测量，不写回安装、零位、曲线或相机；
|
||||
各运动任务分别检查，不能用其他任务中的静止图像稀释误差。它不能替代必要的独立三维验收。
|
||||
缺少独立测量或原始图像、相机身份不一致、测量像素或物理误差超限均不能发布。
|
||||
失败时保留 `final_image_diagnostics.json`；通过后在 manifest 记录 `final_file_image_holdout`
|
||||
及规则版本 `independent_measurement_and_physical_consistency_v2`，分别列出独立精度与额外视角相容性。
|
||||
训练阶段若 Tag 与整条运动链不满足刚性一致性，`tag_installation_diagnostics.json`
|
||||
一次列出全部 Tag 的角度／位置 P95、门限和通过状态，不只显示遇到的第一个失败 Tag。
|
||||
该失败不能直接归因于硬件松动，也不能作为自动重做整场采集的依据。
|
||||
|
||||
`calibration_report.json` 保存完整的 `command_to_rad` / `feedback_to_rad`、输入域、方向分支、
|
||||
实测关节的零位原始参考帧、完整指令/反馈/到达方向、来源和训练/holdout 指标。
|
||||
复制关节记录 `independently_measured: false`、`transferred_from_joint` 与来源参考链接，
|
||||
不生成自己的伪零位样本或独立精度指标。绝对零位来源另由 `zero_method`、
|
||||
`cad_zero_assumption` 或 `zero_transferred_from_joint` 表达。报告中的 `applicability` 记录保持姿态；
|
||||
未采集多姿态证据不能宣称任意多轴工况已验证。主 JSON 用于指令查表，不能误作反馈转换。
|
||||
旧 unified v1 会话继续只读兼容，不自动覆盖或转换历史产物；新输出单表必须重新通过最终文件验收。
|
||||
|
||||
统一消费桥可直接读取通过验证的 manifest（纯消息转换，不会发送实机命令）：
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration calibrated_joint_state_bridge --ros-args \
|
||||
-p calibration_file:=<session/release_manifest.json> -p input_kind:=command \
|
||||
-p input_topic:=<SDK原始指令话题> -p output_topic:=/calibration/verification/joint_states
|
||||
```
|
||||
|
||||
需要反馈角度显示时显式选择 `input_kind:=feedback`，桥读取配套报告中的反馈映射。
|
||||
桥检查三文件哈希、拒绝越域输入，
|
||||
v3 全部关节角由 JSON 显式计算，不加第二次零偏。完整 FK 使用
|
||||
`independent_mimic_angles=True`，以 JSON 给出的末节角度覆盖线性近似。
|
||||
标准 `robot_state_publisher` 和普通 URDF 网页查看器会执行 mimic，适合检查近似联动；
|
||||
仅把完整 JointState 发给标准 RSP，不能让它停止覆盖被动角。
|
||||
消费实测非线性曲线的仿真程序必须明确采用 JSON 角度，关闭重复的线性被动更新。
|
||||
仿真输出不能与实机反馈话题同名。
|
||||
MuJoCo 是用户后续使用的外部验收项目，本轮不改动它、不增加 MuJoCo 依赖，自动标定不要求打开 GUI。
|
||||
外部对比须确认加载的模型来自这份 URDF，指令转换使用配套 JSON；不能用实机反馈直接驱动仿真来证明指令映射。
|
||||
现有 MuJoCo 脚本尚不能视为支持 v3：后续须消费完整关节映射并停止冲突的线性被动更新。
|
||||
|
||||
```bash
|
||||
# 同一算法离线回放新策略的完整采集；不启动硬件，默认不发布
|
||||
ros2 run linkerhand_calibration calibrate_hand --config <product.yaml> \
|
||||
--offline-raw <raw_samples.jsonl> --offline-output <新的输出目录>
|
||||
|
||||
# 只比较两份 URDF，不修改，不控制机械手
|
||||
ros2 run linkerhand_calibration compare_calibration_urdfs \
|
||||
--reference <参考.urdf> --candidate <本次.urdf> --output <新报告.json>
|
||||
```
|
||||
|
||||
离线回放也检查原始会话策略、SDK 输入域、配置/相机证据、每方向完整数据及最终文件。
|
||||
旧会话不能自动获得当前哈希或新版本认证。`--publish-offline` 必须显式给出，且不会绕过验证。
|
||||
当前坐标规则、生产职责与验收范围见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。
|
||||
|
||||
早期需求审查见 [2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)；
|
||||
当前修改依据、实机结果与未完成的精度验收见 [流程评估](CALIBRATION_PIPELINE_REVIEW.md)。
|
||||
相机和反馈目前均采用下述测量时间；时间配对正确仍须经过独立空间验收。
|
||||
|
||||
|
||||
### 实时调度边界
|
||||
|
||||
公共 ROS 节点顺序处理控制和反馈消息；耗时图像估计由每视角有界工作队列执行。
|
||||
图像最多在原同步容差内等候真实反馈时间覆盖，队列不反向持锁查询会话。
|
||||
增加执行器线程数不能提高共享状态的处理能力；反馈超时仍按真实测量判断。
|
||||
暂停或验收失败的会话仅提供诊断/候选文件，不能当作已发布的标定结果。
|
||||
调度对照依据与验证范围见 `CALIBRATION_PIPELINE_REVIEW.md` 第七轮评估。
|
||||
|
||||
|
||||
### 相机与反馈时间
|
||||
|
||||
连续扫描将 CAN 实测反馈与相机曝光中点配对。MVS 取帧/图像发布时间不参与替代曝光时间；
|
||||
设备时钟由独立锁存换算到 ROS 时钟，换算依据保存在会话的 `camera_timing_<view>.jsonl`。
|
||||
运行中的时钟校验失败会暂停出图并重新执行独立锁存校验，通过后恢复采集；校验失败期间不会继续使用旧映射，也不会永久退出采集线程。恢复前后的图像时间戳仍须严格递增；若主机时钟向后跳变，早于已发布图像的帧继续丢弃。
|
||||
四型号共用 `unified_engine_v8_all_view_images`，保留设备曝光时间策略；旧采集只能按其原策略诊断，不能混用。
|
||||
报告中的 `feedback_u8` 表示来自字节通道的测量值：插值到曝光时间后可以是小数，
|
||||
拟合、内存映射和报告读取均按声明的分段线性曲线计算，不再次取整。
|
||||
`command_u8` 仍按实际发送的整数字节查表；旧版整数表保留原读取规则。
|
||||
相机时钟修正不会将被动关节的非线性曲线转成 URDF mimic；仿真仍须读取 JSON 查找表。
|
||||
|
||||
### 同一运动中的所有机位原始角点
|
||||
|
||||
四轮扫描、必要稳态点和关节基准采样期间，所有机位均保存 `image_observation_frame`。
|
||||
记录包含曝光时间、完整指令/反馈及方向、轮次与尝试编号、相机矩阵和可见 Tag 的二维角点。
|
||||
它经过原检测质量与采集窗口检查，但不依赖 PnP 分支或任务的主机位。
|
||||
缺少同步反馈时保留明确的空值，不能将该帧当成有效角度测量。
|
||||
|
||||
例如 O6 顶部 ID7 与正面 ID2 属于同一末节：弯曲和侧摆时两机位的可见角点都应保存，
|
||||
以便检查整条运动链。其他机位的记录不会增加停点或运动次数，也不会替代主机位的采样覆盖要求。
|
||||
同机位重复帧不重复入库；暂停、切换运动和结束采集时仍按同一会话身份撤销迟到数据。
|
||||
最终 JSON/URDF 的图像验收使用通过采集检查的第四轮和独立稳态验证窗口内全部机位角点。
|
||||
主观测必须能与原始图像记录逐项对应，同一图像不能重复计数；其他动作下的有效角点也直接
|
||||
投影验收，不先拟合成三维位姿。缺少主观测的原始记录或二者 SDK 条件不一致时拒绝发布。
|
||||
这不表示联合求解或外参自校准已经通过验证。
|
||||
+3
-6
@@ -44,7 +44,7 @@ g20_thumb_calibration:
|
||||
minimum_detection_hz: 15.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
# Trial threshold for the current 10 mm tags (observed at 32-38 px).
|
||||
# Trial threshold for small/far tags (historically observed at 32-38 px).
|
||||
# Final acceptance is still guarded by static RMS and random validation.
|
||||
minimum_edge_pixels: 30.0
|
||||
# Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.
|
||||
@@ -60,11 +60,8 @@ g20_thumb_calibration:
|
||||
# 30-38 px tags are usable, but only if IPPE gives a tight image fit and
|
||||
# a pose continuous with the preceding frame.
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
# All four tags keep a temporally continuous IPPE solution throughout the
|
||||
# complete session. With 30 px planar tags, tiny reprojection differences
|
||||
# do not reliably identify the physical branch and previously caused
|
||||
# stationary T0 to flip by about 25 deg between scan and validation.
|
||||
pnp_reprojection_tie_px: 1.5
|
||||
# Independent IPPE selection inherits the shared near-tie tolerance from
|
||||
# core/geometry/pnp.py; continuity cannot override a clear image-fit lead.
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
pnp_maximum_translation_jump_m: 0.04
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
@@ -0,0 +1,40 @@
|
||||
schema_version: 3
|
||||
profile_id: G20/right/g20_right_19/v1
|
||||
profile_config: package://linkerhand_calibration/config/profiles/g20_right_19.yaml
|
||||
profile_config_sha256: 958f74c476730636d2dadb58a30296a7cdf0b25ad00345252754f9c1b6b50c7f
|
||||
model: G20
|
||||
side: right
|
||||
tag_layout: g20_right_19
|
||||
serial_number: G20_RIGHT_001
|
||||
can_interface: can0
|
||||
output_root: calibration_output
|
||||
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
camera_name: hikrobot_front_DB2163742
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
camera_name: hikrobot_side_DB2163749
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
camera_name: hikrobot_top_DB2163739
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||
|
||||
artifacts:
|
||||
source_urdf: package://linkerhand_calibration/urdf/g20_right/linkerhand_g20_right.urdf
|
||||
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
||||
calibration_config_sha256: 4927506d787d665c16f0209ee4d052654e648bb25beaa1913ed0d416f449d523
|
||||
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
||||
|
||||
release:
|
||||
# Each task already contains three training cycles plus an isolated fourth
|
||||
# holdout, so a second complete hardware session duplicates hours of motion.
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -0,0 +1,59 @@
|
||||
/l6_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2]
|
||||
frames: [front_base, thumb_pitch, thumb_dip]
|
||||
sizes: [0.016, 0.016, 0.016]
|
||||
|
||||
/l6_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [3, 4, 5]
|
||||
frames: [side_base, pinky_pitch, pinky_dip]
|
||||
sizes: [0.016, 0.016, 0.016]
|
||||
|
||||
/l6_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [6, 7]
|
||||
frames: [top_base, thumb_roll]
|
||||
sizes: [0.016, 0.016]
|
||||
@@ -0,0 +1,39 @@
|
||||
schema_version: 3
|
||||
profile_id: L6/right/l6_right_8/v1
|
||||
profile_config: package://linkerhand_calibration/config/profiles/l6_right_8.yaml
|
||||
profile_config_sha256: d241e87ded3a29d416a876b27f1a251f8daf2f98e3676ab277264f1bdfaf5c60
|
||||
model: L6
|
||||
side: right
|
||||
tag_layout: l6_right_8
|
||||
namespace: /l6_calibration
|
||||
serial_number: L6_RIGHT_001
|
||||
can_interface: can0
|
||||
output_root: calibration_output
|
||||
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
camera_name: hikrobot_front_DB2163742
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
camera_name: hikrobot_side_DB2163749
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
camera_name: hikrobot_top_DB2163739
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||
|
||||
artifacts:
|
||||
source_urdf: package://linkerhand_calibration/urdf/l6_right/linkerhand_l6v3.1_right.urdf
|
||||
source_urdf_sha256: 298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
|
||||
calibration_config_sha256: 090d82a5609e8b981c1b8c6ebeaf4c2f477323209ae3db3b9d8713ac6d6169b5
|
||||
tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml
|
||||
tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
|
||||
|
||||
release:
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -0,0 +1,66 @@
|
||||
l6_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /l6/cb_right_hand_control_cmd
|
||||
state_topic: /l6/cb_right_hand_state
|
||||
setting_topic: /l6/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /l6_calibration/front/camera/camera_info
|
||||
front_detections_topic: /l6_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /l6_calibration/side/camera/camera_info
|
||||
side_detections_topic: /l6_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /l6_calibration/top/camera/camera_info
|
||||
top_detections_topic: /l6_calibration/top/apriltag/detections
|
||||
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255]
|
||||
# L6_RIGHT_001 measured a 250->5 travel of only ~0.9 s at speed 10,
|
||||
# which left fewer than 32 useful feedback bins. Speed 1 is still only a
|
||||
# firmware ceiling: different L6 motors complete a full stroke in 0.7-1.3 s.
|
||||
# A 100 Hz cosine trajectory therefore sets the actual, model-level pace.
|
||||
preflight_speed_u8: 1
|
||||
formal_speed_u8: 1
|
||||
speed_settle_seconds: 0.2
|
||||
command_trajectory_full_range_seconds: 6.0
|
||||
torque_u8: 80
|
||||
repetitions: 4
|
||||
preflight_checkpoints_u8: [255, 127, 0]
|
||||
tag_size_m: 0.016
|
||||
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
|
||||
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
minimum_detection_rate: 0.95
|
||||
# Per-Tag quality remains >=95%. With three independently detected Tags,
|
||||
# the fully joined frame rate may be 0.95^3 ~= 85.7%.
|
||||
minimum_joint_frame_rate: 0.85
|
||||
minimum_feedback_hz: 25.0
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
pnp_maximum_translation_jump_m: 0.04
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
pnp_tracker_reset_seconds: 5.0
|
||||
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
maximum_validation_error_deg: 3.0
|
||||
mimic_minimum_multiplier: 0.5
|
||||
mimic_maximum_multiplier: 1.5
|
||||
mimic_maximum_cycle_range: 0.03
|
||||
mimic_maximum_residual_p95_deg: 2.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
endpoint_hold_seconds: 1.0
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
automatic_sweep_retry_limit: 1
|
||||
non_target_motion_tolerance_u8: 3.0
|
||||
fixed_base_maximum_corner_drift_px: 5.0
|
||||
fixed_base_movement_confirmation_frames: 10
|
||||
@@ -0,0 +1,41 @@
|
||||
/o12_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2, 3, 12, 13]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_dip, middle_roll, index_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/o12_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 7, 8, 9, 10, 11]
|
||||
frames: [side_base, pinky_mcp, pinky_pip, pinky_dip, middle_pip, middle_dip, index_pip, index_dip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/o12_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [14, 15]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.016, 0.016]
|
||||
@@ -0,0 +1,47 @@
|
||||
schema_version: 3
|
||||
profile_id: O12/right/o12_right_16/v1
|
||||
profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml
|
||||
profile_config_sha256: 652c2a82dfc7532b4f62c42cebdad57b169435cf950c5f2ee910cc67c7972621
|
||||
model: O12
|
||||
side: right
|
||||
tag_layout: o12_right_16
|
||||
namespace: /o12_calibration
|
||||
serial_number: O12_RIGHT_001
|
||||
output_root: calibration_output
|
||||
|
||||
sdk:
|
||||
driver: o12_sdk_bridge
|
||||
python_package: src/agillink_omnihand_sdk/linux/x64/python/omnihand-1.1.8-cp312-cp312-linux_x86_64.whl
|
||||
package_sha256: cae7a0d5bce7e7c9d72cc90a0dd15e152f11e2a1ecf04cb6761e8171399ff170
|
||||
transport: hcan
|
||||
setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
|
||||
config: package://linkerhand_calibration/config/o12_sdk.yaml
|
||||
config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c
|
||||
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
camera_name: hikrobot_front_DB2163742
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
camera_name: hikrobot_side_DB2163749
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
camera_name: hikrobot_top_DB2163739
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||
|
||||
artifacts:
|
||||
source_urdf: package://linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703.urdf
|
||||
source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb
|
||||
camera_extrinsics: config/o12_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: 5a515d0706f4e67d5e26bfddb348b519817bd72e885ea9e43997e41016176e53
|
||||
calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml
|
||||
calibration_config_sha256: c2fcdd532e22c15013413e655311de2ca87107c23a96d5c043e8f340f232eebd
|
||||
tag_config: package://linkerhand_calibration/config/o12_right_16_tags.yaml
|
||||
tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab
|
||||
|
||||
release:
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -0,0 +1,45 @@
|
||||
# OmniHand Pro 2025 (O12) Configuration
|
||||
# connection_type: "zlgcan" | "hcan" | "socketcan" | "rs485" | "usb"
|
||||
# request_interval_ms: min gap between requests, SDK default 0 = no throttling, range 0..100
|
||||
# frame_recv_timeout_ms: single-frame reply wait, SDK default 50, range 10..1000
|
||||
|
||||
/**:
|
||||
ros__parameters:
|
||||
# This workstation uses one O12 right hand through HCAN device 0/channel 0.
|
||||
# Leave left_hand undefined so the node does not try to open a second hand.
|
||||
right_hand:
|
||||
hand_device_id: 1
|
||||
connection_type: "hcan"
|
||||
canfd_device_id: 0
|
||||
canfd_channel_id: 0
|
||||
# O12 POSITION streams set-points and has no G20-style firmware speed
|
||||
# interpolation, so do not throttle the 50 Hz calibration command stream.
|
||||
request_interval_ms: 0
|
||||
frame_recv_timeout_ms: 100
|
||||
show_data_details: false
|
||||
|
||||
# Alternative ZLG CANFD example:
|
||||
# right_hand:
|
||||
# hand_device_id: 1
|
||||
# connection_type: "zlgcan"
|
||||
# canfd_device_id: 0
|
||||
# canfd_channel_id: 0
|
||||
# request_interval_ms: 0
|
||||
# frame_recv_timeout_ms: 50
|
||||
# show_data_details: false
|
||||
# socketcan example
|
||||
# left_hand:
|
||||
# hand_device_id: 1
|
||||
# connection_type: "socketcan"
|
||||
# can_interface: "can0"
|
||||
# request_interval_ms: 0
|
||||
# frame_recv_timeout_ms: 50
|
||||
# show_data_details: true
|
||||
|
||||
# right_hand:
|
||||
# hand_device_id: 1
|
||||
# connection_type: "socketcan"
|
||||
# can_interface: "can1"
|
||||
# request_interval_ms: 0
|
||||
# frame_recv_timeout_ms: 50
|
||||
# show_data_details: true
|
||||
@@ -0,0 +1,53 @@
|
||||
o12_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /o12/right/joint_cmd
|
||||
state_topic: /o12/right/joint_states
|
||||
front_camera_info_topic: /o12_calibration/front/camera/camera_info
|
||||
front_detections_topic: /o12_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /o12_calibration/side/camera/camera_info
|
||||
side_detections_topic: /o12_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /o12_calibration/top/camera/camera_info
|
||||
top_detections_topic: /o12_calibration/top/apriltag/detections
|
||||
|
||||
# O12 standard interface is radians. Scan the complete reviewed SDK range;
|
||||
# the source CAD limits are outputs to correct, not acquisition limits.
|
||||
# Raw 0..2000 mixed control is forbidden.
|
||||
command_rate_hz: 50.0
|
||||
repetitions: 4
|
||||
tag_size_m: 0.016
|
||||
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
|
||||
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_joint_frame_rate: 0.85
|
||||
minimum_feedback_hz: 35.0
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_fraction: 0.90
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 2
|
||||
endpoint_tolerance_rad: 0.01
|
||||
endpoint_hold_seconds: 0.5
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
position_timeout_seconds: 60.0
|
||||
sweep_timeout_seconds: 180.0
|
||||
automatic_sweep_retry_limit: 1
|
||||
non_target_motion_tolerance_rad: 0.015
|
||||
maximum_temperature_c: 70
|
||||
# 当前 O12 固件返回空温度报告。仍发起查询；5秒无结果后依赖已验证的
|
||||
# joint_error_states bit1 过热保护，并在会话记录中明确标注降级。
|
||||
temperature_report_required: false
|
||||
temperature_fallback_after_seconds: 5.0
|
||||
# 快速标定档：各任务按4倍请求，但拇指pitch、侧摆、屈曲及避让均有独立硬上限。
|
||||
motion_speed_scale: 4.0
|
||||
# 正弦速度加减速时间；中段保持关节限速，避免位置余弦全程低速。
|
||||
trajectory_ramp_seconds: 0.4
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
fixed_base_maximum_corner_drift_px: 5.0
|
||||
fixed_base_movement_confirmation_frames: 10
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
pnp_maximum_translation_jump_m: 0.04
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
pnp_tracker_reset_seconds: 5.0
|
||||
@@ -0,0 +1,59 @@
|
||||
/o6_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.0165
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2]
|
||||
frames: [front_base, thumb_pitch, thumb_ip]
|
||||
sizes: [0.0165, 0.0165, 0.0165]
|
||||
|
||||
/o6_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.0165
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [3, 4, 5]
|
||||
frames: [side_base, pinky_pitch, pinky_dip]
|
||||
sizes: [0.0165, 0.0165, 0.0165]
|
||||
|
||||
/o6_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.0165
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [6, 7]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.0165, 0.0165]
|
||||
@@ -0,0 +1,39 @@
|
||||
schema_version: 3
|
||||
profile_id: O6/right/o6_right_8/v1
|
||||
profile_config: package://linkerhand_calibration/config/profiles/o6_right_8.yaml
|
||||
profile_config_sha256: 72bf56b13eb2a0d4d14a5be42838b2ffc176722f470fe1a3ae5fd84604e1df4d
|
||||
model: O6
|
||||
side: right
|
||||
tag_layout: o6_right_8
|
||||
namespace: /o6_calibration
|
||||
serial_number: O6_RIGHT_001
|
||||
can_interface: can0
|
||||
output_root: calibration_output
|
||||
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
camera_name: hikrobot_front_DB2163742
|
||||
camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163742.yaml
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
camera_name: hikrobot_side_DB2163749
|
||||
camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163749.yaml
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
camera_name: hikrobot_top_DB2163739
|
||||
camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163739.yaml
|
||||
|
||||
artifacts:
|
||||
source_urdf: package://linkerhand_calibration/urdf/o6_right/linkerhand_o6_right.urdf
|
||||
source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62
|
||||
camera_extrinsics: config/o6_three_camera_extrinsics_20260915_192517.yaml
|
||||
camera_extrinsics_sha256: d057183eaba592149ec4c63c4ceab8a70e1a0c9d7cf2460a1c8b7e86a6ca6638
|
||||
calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
|
||||
calibration_config_sha256: 49ce8a0d317695994c5b306e3a3a28d0470b8769c77f1173ca935178430587db
|
||||
tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml
|
||||
tag_config_sha256: 219b8aa906fc9e2a13f3bbede46f542ff176ef2c7f60190f0dd2508993f04593
|
||||
|
||||
release:
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -0,0 +1,63 @@
|
||||
o6_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /o6/cb_right_hand_control_cmd
|
||||
state_topic: /o6/cb_right_hand_state
|
||||
setting_topic: /o6/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /o6_calibration/front/camera/camera_info
|
||||
front_detections_topic: /o6_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /o6_calibration/side/camera/camera_info
|
||||
side_detections_topic: /o6_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /o6_calibration/top/camera/camera_info
|
||||
top_detections_topic: /o6_calibration/top/apriltag/detections
|
||||
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255]
|
||||
# O6 has a different speed scale from L6. Motion is still bounded by the
|
||||
# six-second cosine command trajectory; these values are firmware limits.
|
||||
baseline_speed_u8: 80
|
||||
preflight_speed_u8: 60
|
||||
formal_speed_u8: 40
|
||||
speed_settle_seconds: 0.2
|
||||
command_trajectory_full_range_seconds: 6.0
|
||||
torque_u8: 80
|
||||
repetitions: 4
|
||||
preflight_checkpoints_u8: [255, 127, 0]
|
||||
tag_size_m: 0.0165
|
||||
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
|
||||
tag_size_overrides_m: [0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165]
|
||||
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_joint_frame_rate: 0.85
|
||||
minimum_feedback_hz: 25.0
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
pnp_maximum_translation_jump_m: 0.04
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
pnp_tracker_reset_seconds: 5.0
|
||||
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
maximum_validation_error_deg: 3.0
|
||||
mimic_minimum_multiplier: 0.5
|
||||
mimic_maximum_multiplier: 2.2
|
||||
mimic_maximum_cycle_range: 0.03
|
||||
mimic_maximum_residual_p95_deg: 2.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
endpoint_hold_seconds: 1.0
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
automatic_sweep_retry_limit: 1
|
||||
non_target_motion_tolerance_u8: 3.0
|
||||
fixed_base_maximum_corner_drift_px: 5.0
|
||||
fixed_base_movement_confirmation_frames: 10
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,418 @@
|
||||
schema_version: 1
|
||||
profile_id: L6/right/l6_right_8/v1
|
||||
namespace: /l6_calibration
|
||||
sdk_adapter: legacy_byte_sdk
|
||||
command:
|
||||
sdk_to_joint_direction: [-1, -1, -1, -1, -1, -1]
|
||||
names:
|
||||
- thumb_cmc_pitch
|
||||
- thumb_cmc_roll
|
||||
- index_mcp_pitch
|
||||
- middle_mcp_pitch
|
||||
- ring_mcp_pitch
|
||||
- pinky_mcp_pitch
|
||||
baseline_u8:
|
||||
- 255
|
||||
- 255
|
||||
- 255
|
||||
- 255
|
||||
- 255
|
||||
- 255
|
||||
command_index_by_joint:
|
||||
rh_thumb_cmc_pitch: 0
|
||||
rh_thumb_cmc_roll: 1
|
||||
rh_index_mcp_pitch: 2
|
||||
rh_middle_mcp_pitch: 3
|
||||
rh_ring_mcp_pitch: 4
|
||||
rh_pinky_mcp_pitch: 5
|
||||
disabled_indices: []
|
||||
urdf_joint_by_joint:
|
||||
rh_thumb_cmc_pitch: rh_thumb_cmc_pitch
|
||||
rh_thumb_cmc_roll: rh_thumb_cmc_roll
|
||||
rh_index_mcp_pitch: rh_index_mcp_pitch
|
||||
rh_middle_mcp_pitch: rh_middle_mcp_pitch
|
||||
rh_ring_mcp_pitch: rh_ring_mcp_pitch
|
||||
rh_pinky_mcp_pitch: rh_pinky_mcp_pitch
|
||||
feedback_name_aliases:
|
||||
thumb_cmc_yaw: thumb_cmc_roll
|
||||
speed_slot_by_command_index:
|
||||
'0': 0
|
||||
'1': 1
|
||||
'2': 2
|
||||
'3': 3
|
||||
'4': 4
|
||||
'5': 5
|
||||
unit: u8
|
||||
baseline: []
|
||||
lower_bounds: []
|
||||
upper_bounds: []
|
||||
feedback_lower_bounds: []
|
||||
feedback_upper_bounds: []
|
||||
feedback_by_index: false
|
||||
vision:
|
||||
views:
|
||||
- name: front
|
||||
tags:
|
||||
- role: front_base
|
||||
fixed_reference: true
|
||||
id: 0
|
||||
- role: thumb_pitch
|
||||
fixed_reference: false
|
||||
id: 1
|
||||
- role: thumb_dip
|
||||
fixed_reference: false
|
||||
id: 2
|
||||
- name: side
|
||||
tags:
|
||||
- role: side_base
|
||||
fixed_reference: true
|
||||
id: 3
|
||||
- role: pinky_pitch
|
||||
fixed_reference: false
|
||||
id: 4
|
||||
- role: pinky_dip
|
||||
fixed_reference: false
|
||||
id: 5
|
||||
- name: top
|
||||
tags:
|
||||
- role: top_base
|
||||
fixed_reference: true
|
||||
id: 6
|
||||
- role: thumb_roll
|
||||
fixed_reference: false
|
||||
id: 7
|
||||
common_frame: calibration_common
|
||||
extrinsic_reference_view: front
|
||||
extrinsics_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
|
||||
motion:
|
||||
tasks:
|
||||
- key: thumb_roll_top
|
||||
view: top
|
||||
command_index: 1
|
||||
joints:
|
||||
- rh_thumb_cmc_roll
|
||||
auxiliary_commands:
|
||||
- - 0
|
||||
- 255
|
||||
validation_only: false
|
||||
start_u8: 255
|
||||
end_u8: 0
|
||||
preflight_speed_u8: 1
|
||||
formal_speed_u8: 1
|
||||
start: null
|
||||
end: null
|
||||
preflight_speed: null
|
||||
formal_speed: null
|
||||
- key: thumb_pitch_dip_front
|
||||
view: front
|
||||
command_index: 0
|
||||
joints:
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_dip
|
||||
auxiliary_commands:
|
||||
- - 1
|
||||
- 255
|
||||
validation_only: false
|
||||
start_u8: 255
|
||||
end_u8: 0
|
||||
preflight_speed_u8: 1
|
||||
formal_speed_u8: 1
|
||||
start: null
|
||||
end: null
|
||||
preflight_speed: null
|
||||
formal_speed: null
|
||||
- key: pinky_pitch_dip_side
|
||||
view: side
|
||||
command_index: 5
|
||||
joints:
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_pinky_dip
|
||||
auxiliary_commands: []
|
||||
validation_only: false
|
||||
start_u8: 255
|
||||
end_u8: 0
|
||||
preflight_speed_u8: 1
|
||||
formal_speed_u8: 1
|
||||
start: null
|
||||
end: null
|
||||
preflight_speed: null
|
||||
formal_speed: null
|
||||
preparation_waypoints_u8: []
|
||||
safe_return_waypoints_u8: []
|
||||
speed_parameters:
|
||||
preflight_u8: 1
|
||||
formal_u8: 1
|
||||
speed_settle_seconds: 0.2
|
||||
command_trajectory_full_range_seconds: 6.0
|
||||
torque_u8: 80
|
||||
endpoint_hold_seconds: 1.0
|
||||
stall_timeout_seconds: 2.0
|
||||
precheck_sweeps: false
|
||||
steady_command_checkpoints: false
|
||||
joint_zero_references:
|
||||
rh_thumb_cmc_roll:
|
||||
task_key: thumb_roll_top
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [255, 0, 255, 255, 255, 255]
|
||||
rh_thumb_cmc_pitch:
|
||||
task_key: thumb_pitch_dip_front
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [0, 255, 255, 255, 255, 255]
|
||||
rh_thumb_dip:
|
||||
task_key: thumb_pitch_dip_front
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [0, 255, 255, 255, 255, 255]
|
||||
rh_pinky_mcp_pitch:
|
||||
task_key: pinky_pitch_dip_side
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [255, 255, 255, 255, 255, 0]
|
||||
rh_pinky_dip:
|
||||
task_key: pinky_pitch_dip_side
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [255, 255, 255, 255, 255, 0]
|
||||
measurement:
|
||||
transferred_motion_sources:
|
||||
rh_index_mcp_pitch: rh_pinky_mcp_pitch
|
||||
rh_index_dip: rh_pinky_dip
|
||||
rh_middle_mcp_pitch: rh_pinky_mcp_pitch
|
||||
rh_middle_dip: rh_pinky_dip
|
||||
rh_ring_mcp_pitch: rh_pinky_mcp_pitch
|
||||
rh_ring_dip: rh_pinky_dip
|
||||
measurements:
|
||||
rh_thumb_cmc_roll:
|
||||
joint: rh_thumb_cmc_roll
|
||||
kind: relative_rotation
|
||||
view: top
|
||||
parent_role: top_base
|
||||
child_role: thumb_roll
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
rh_thumb_cmc_pitch:
|
||||
joint: rh_thumb_cmc_pitch
|
||||
kind: relative_rotation
|
||||
view: front
|
||||
parent_role: front_base
|
||||
child_role: thumb_pitch
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
rh_thumb_dip:
|
||||
joint: rh_thumb_dip
|
||||
kind: relative_rotation
|
||||
view: front
|
||||
parent_role: thumb_pitch
|
||||
child_role: thumb_dip
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
rh_pinky_mcp_pitch:
|
||||
joint: rh_pinky_mcp_pitch
|
||||
kind: relative_rotation
|
||||
view: side
|
||||
parent_role: side_base
|
||||
child_role: pinky_pitch
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
rh_pinky_dip:
|
||||
joint: rh_pinky_dip
|
||||
kind: relative_rotation
|
||||
view: side
|
||||
parent_role: pinky_pitch
|
||||
child_role: pinky_dip
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
cross_view_sources: {}
|
||||
image_curve_joints: []
|
||||
directional_zero: true
|
||||
cross_view_roll_curve: false
|
||||
stable_cross_view_cone_bias: false
|
||||
zero:
|
||||
cad_zero_assumptions:
|
||||
rh_index_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
rh_middle_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
rh_pinky_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
rh_ring_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
rh_thumb_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
active_joints:
|
||||
- rh_index_mcp_pitch
|
||||
- rh_middle_mcp_pitch
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_ring_mcp_pitch
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_cmc_roll
|
||||
passive_joints:
|
||||
- rh_index_dip
|
||||
- rh_middle_dip
|
||||
- rh_pinky_dip
|
||||
- rh_ring_dip
|
||||
- rh_thumb_dip
|
||||
direct_zero_joints:
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_cmc_roll
|
||||
axis_joints:
|
||||
- rh_pinky_dip
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_cmc_roll
|
||||
- rh_thumb_dip
|
||||
mechanical_endpoint_joints: []
|
||||
post_solve_endpoint_joints: []
|
||||
mimic_source_by_joint:
|
||||
rh_thumb_dip: rh_thumb_cmc_pitch
|
||||
rh_index_dip: rh_index_mcp_pitch
|
||||
rh_middle_dip: rh_middle_mcp_pitch
|
||||
rh_ring_dip: rh_ring_mcp_pitch
|
||||
rh_pinky_dip: rh_pinky_mcp_pitch
|
||||
cad_frozen_joints:
|
||||
- rh_index_dip
|
||||
- rh_middle_dip
|
||||
- rh_pinky_dip
|
||||
- rh_ring_dip
|
||||
- rh_thumb_dip
|
||||
endpoint_anchor_by_joint: {}
|
||||
fitted_mimic_joints:
|
||||
- rh_pinky_dip
|
||||
- rh_thumb_dip
|
||||
coupling_model_by_joint:
|
||||
rh_thumb_dip: linear_mimic
|
||||
rh_pinky_dip: linear_mimic
|
||||
rh_index_dip: linear_mimic
|
||||
rh_middle_dip: linear_mimic
|
||||
rh_ring_dip: linear_mimic
|
||||
transferred_zero_sources: {rh_index_mcp_pitch: rh_pinky_mcp_pitch, rh_middle_mcp_pitch: rh_pinky_mcp_pitch, rh_ring_mcp_pitch: rh_pinky_mcp_pitch}
|
||||
transferred_mimic_sources: {rh_index_dip: rh_pinky_dip, rh_middle_dip: rh_pinky_dip, rh_ring_dip: rh_pinky_dip}
|
||||
spatial:
|
||||
base_pose_strategy: thumb_serial
|
||||
root_anchor_joints: [rh_thumb_cmc_roll]
|
||||
orientation_anchor_joint: rh_pinky_mcp_pitch
|
||||
directed_base_axis_joints: [rh_thumb_cmc_roll, rh_pinky_mcp_pitch]
|
||||
depth_free_axis_projection: true
|
||||
axis_order: [rh_thumb_cmc_roll, rh_thumb_cmc_pitch, rh_thumb_dip, rh_pinky_mcp_pitch, rh_pinky_dip]
|
||||
axis_parent_joint: {rh_thumb_cmc_pitch: rh_thumb_cmc_roll}
|
||||
phase_parent_joint: {rh_thumb_dip: rh_thumb_cmc_pitch, rh_pinky_dip: rh_pinky_mcp_pitch}
|
||||
offset_observer_joint: {rh_thumb_cmc_roll: rh_thumb_cmc_pitch, rh_thumb_cmc_pitch: rh_thumb_dip, rh_pinky_mcp_pitch: rh_pinky_dip}
|
||||
quality:
|
||||
training_cycles:
|
||||
- 0
|
||||
- 1
|
||||
- 2
|
||||
holdout_cycle: 3
|
||||
hard_threshold_keys:
|
||||
- maximum_mimic_residual_rad
|
||||
- maximum_state_image_skew_ms
|
||||
- maximum_validation_error_rad
|
||||
- minimum_detection_rate
|
||||
retry_metric_scope: {}
|
||||
isolated_holdout: true
|
||||
scope:
|
||||
calibrate_joints:
|
||||
partial:
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_cmc_roll
|
||||
frozen_joints:
|
||||
partial:
|
||||
- rh_index_mcp_pitch
|
||||
- rh_middle_mcp_pitch
|
||||
- rh_ring_mcp_pitch
|
||||
default_scope: partial
|
||||
artifacts:
|
||||
output_schema_version: 3
|
||||
calibration_filename: l6_right_{serial_number}_partial_calibration.json
|
||||
corrected_urdf_filename: linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf
|
||||
protected_input_fields:
|
||||
- calibration_config_sha256
|
||||
- camera_extrinsics_sha256
|
||||
- profile_config_sha256
|
||||
- source_urdf_sha256
|
||||
- tag_config_sha256
|
||||
publication_pointer: latest_partial_passed
|
||||
session_compatibility_tokens:
|
||||
- feedback_curves_v6
|
||||
- l6_partial_v1
|
||||
publish_corrected_urdf: true
|
||||
acquisition:
|
||||
command_capture_mode: separate
|
||||
joint_zero_timeout_seconds: 5.0
|
||||
policy_version: unified_engine_v8_all_view_images
|
||||
mapping_probe_maximum_rad: 0.0
|
||||
automatic_rescan_limit: 1
|
||||
minimum_valid_samples: 40
|
||||
minimum_bins: 32
|
||||
maximum_unobserved_fraction: 0.0625
|
||||
legacy_minimum_span_01: 0.9411764705882353
|
||||
physical_first_cycle_minimum_span_01: 0.85
|
||||
physical_repeat_minimum_fraction: 0.9
|
||||
stall_timeout_seconds: 2.0
|
||||
feedback_stale_seconds: 1.0
|
||||
fixed_reference_minimum_frames: 10
|
||||
fixed_reference_maximum_drift_px: 5.0
|
||||
fixed_reference_confirmation_frames: 10
|
||||
urdf:
|
||||
authorized_fields:
|
||||
rh_pinky_mcp_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_thumb_cmc_roll:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_index_mcp_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_thumb_cmc_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_middle_mcp_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_ring_mcp_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_ring_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
rh_thumb_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
rh_index_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
rh_pinky_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
rh_middle_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
joint_coverage:
|
||||
rh_pinky_mcp_pitch: measured_static_dynamic
|
||||
rh_thumb_cmc_roll: measured_static_dynamic
|
||||
rh_thumb_cmc_pitch: measured_static_dynamic
|
||||
rh_middle_mcp_pitch: transferred_static_dynamic
|
||||
rh_index_mcp_pitch: transferred_static_dynamic
|
||||
rh_ring_mcp_pitch: transferred_static_dynamic
|
||||
rh_pinky_dip: measured_dynamic_cad_static
|
||||
rh_middle_dip: transferred_dynamic_cad_static
|
||||
rh_ring_dip: transferred_dynamic_cad_static
|
||||
rh_thumb_dip: measured_dynamic_cad_static
|
||||
rh_index_dip: transferred_dynamic_cad_static
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,472 @@
|
||||
schema_version: 1
|
||||
profile_id: O6/right/o6_right_8/v1
|
||||
namespace: /o6_calibration
|
||||
sdk_adapter: legacy_byte_sdk
|
||||
command:
|
||||
sdk_to_joint_direction: [-1, -1, -1, -1, -1, -1]
|
||||
names:
|
||||
- thumb_cmc_pitch
|
||||
- thumb_cmc_yaw
|
||||
- index_mcp_pitch
|
||||
- middle_mcp_pitch
|
||||
- ring_mcp_pitch
|
||||
- pinky_mcp_pitch
|
||||
baseline_u8:
|
||||
- 255
|
||||
- 255
|
||||
- 255
|
||||
- 255
|
||||
- 255
|
||||
- 255
|
||||
command_index_by_joint:
|
||||
rh_thumb_cmc_pitch: 0
|
||||
rh_thumb_cmc_yaw: 1
|
||||
rh_index_mcp_pitch: 2
|
||||
rh_middle_mcp_pitch: 3
|
||||
rh_ring_mcp_pitch: 4
|
||||
rh_pinky_mcp_pitch: 5
|
||||
disabled_indices: []
|
||||
urdf_joint_by_joint:
|
||||
rh_thumb_cmc_pitch: rh_thumb_cmc_pitch
|
||||
rh_thumb_cmc_yaw: rh_thumb_cmc_yaw
|
||||
rh_index_mcp_pitch: rh_index_mcp_pitch
|
||||
rh_middle_mcp_pitch: rh_middle_mcp_pitch
|
||||
rh_ring_mcp_pitch: rh_ring_mcp_pitch
|
||||
rh_pinky_mcp_pitch: rh_pinky_mcp_pitch
|
||||
feedback_name_aliases: {}
|
||||
speed_slot_by_command_index:
|
||||
'0': 0
|
||||
'1': 1
|
||||
'2': 2
|
||||
'3': 3
|
||||
'4': 4
|
||||
'5': 5
|
||||
unit: u8
|
||||
baseline: []
|
||||
lower_bounds: []
|
||||
upper_bounds: []
|
||||
feedback_lower_bounds: []
|
||||
feedback_upper_bounds: []
|
||||
feedback_by_index: false
|
||||
vision:
|
||||
views:
|
||||
- name: front
|
||||
tags:
|
||||
- role: front_base
|
||||
fixed_reference: true
|
||||
id: 0
|
||||
size_m: 0.0165
|
||||
- role: thumb_pitch
|
||||
fixed_reference: false
|
||||
id: 1
|
||||
size_m: 0.0165
|
||||
- role: thumb_ip
|
||||
fixed_reference: false
|
||||
id: 2
|
||||
size_m: 0.0165
|
||||
- name: side
|
||||
tags:
|
||||
- role: side_base
|
||||
fixed_reference: true
|
||||
id: 3
|
||||
size_m: 0.0165
|
||||
- role: pinky_pitch
|
||||
fixed_reference: false
|
||||
id: 4
|
||||
size_m: 0.0165
|
||||
- role: pinky_dip
|
||||
fixed_reference: false
|
||||
id: 5
|
||||
size_m: 0.0165
|
||||
- name: top
|
||||
tags:
|
||||
- role: top_base
|
||||
fixed_reference: true
|
||||
id: 6
|
||||
size_m: 0.0165
|
||||
- role: thumb_yaw
|
||||
fixed_reference: false
|
||||
id: 7
|
||||
size_m: 0.0165
|
||||
link: rh_thumb_distal
|
||||
common_frame: calibration_common
|
||||
extrinsic_reference_view: front
|
||||
extrinsics_quality_limits:
|
||||
reprojection_rms_px: 1.5
|
||||
maximum_rotation_repeatability_deg: 0.3
|
||||
maximum_translation_repeatability_m: 0.0015
|
||||
minimum_capture_counts:
|
||||
front_side_captures: 15
|
||||
front_top_captures: 15
|
||||
motion:
|
||||
tasks:
|
||||
- key: thumb_yaw_top
|
||||
view: top
|
||||
command_index: 1
|
||||
joints:
|
||||
- rh_thumb_cmc_yaw
|
||||
auxiliary_commands: []
|
||||
validation_only: false
|
||||
start_u8: 255
|
||||
end_u8: 0
|
||||
preflight_speed_u8: 60
|
||||
formal_speed_u8: 40
|
||||
start: null
|
||||
end: null
|
||||
preflight_speed: null
|
||||
formal_speed: null
|
||||
- key: thumb_pitch_ip_front
|
||||
view: front
|
||||
command_index: 0
|
||||
joints:
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_ip
|
||||
auxiliary_commands: []
|
||||
validation_only: false
|
||||
start_u8: 255
|
||||
end_u8: 0
|
||||
preflight_speed_u8: 60
|
||||
formal_speed_u8: 40
|
||||
start: null
|
||||
end: null
|
||||
preflight_speed: null
|
||||
formal_speed: null
|
||||
- key: pinky_pitch_dip_side
|
||||
view: side
|
||||
command_index: 5
|
||||
joints:
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_pinky_dip
|
||||
auxiliary_commands: []
|
||||
validation_only: false
|
||||
start_u8: 255
|
||||
end_u8: 0
|
||||
preflight_speed_u8: 60
|
||||
formal_speed_u8: 40
|
||||
start: null
|
||||
end: null
|
||||
preflight_speed: null
|
||||
formal_speed: null
|
||||
preparation_waypoints_u8: []
|
||||
safe_return_waypoints_u8: []
|
||||
speed_parameters:
|
||||
baseline_u8: 80
|
||||
preflight_u8: 60
|
||||
formal_u8: 40
|
||||
speed_settle_seconds: 0.2
|
||||
command_trajectory_full_range_seconds: 6.0
|
||||
torque_u8: 80
|
||||
endpoint_hold_seconds: 1.0
|
||||
stall_timeout_seconds: 2.0
|
||||
precheck_sweeps: false
|
||||
steady_command_checkpoints: false
|
||||
joint_zero_references:
|
||||
rh_thumb_cmc_yaw:
|
||||
task_key: thumb_yaw_top
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [255, 0, 255, 255, 255, 255]
|
||||
rh_thumb_cmc_pitch:
|
||||
task_key: thumb_pitch_ip_front
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [0, 255, 255, 255, 255, 255]
|
||||
rh_thumb_ip:
|
||||
task_key: thumb_pitch_ip_front
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [0, 255, 255, 255, 255, 255]
|
||||
rh_pinky_mcp_pitch:
|
||||
task_key: pinky_pitch_dip_side
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [255, 255, 255, 255, 255, 0]
|
||||
rh_pinky_dip:
|
||||
task_key: pinky_pitch_dip_side
|
||||
command: [255, 255, 255, 255, 255, 255]
|
||||
approach_commands:
|
||||
- [255, 255, 255, 255, 255, 0]
|
||||
measurement:
|
||||
transferred_motion_sources:
|
||||
rh_index_mcp_pitch: rh_pinky_mcp_pitch
|
||||
rh_index_dip: rh_pinky_dip
|
||||
rh_middle_mcp_pitch: rh_pinky_mcp_pitch
|
||||
rh_middle_dip: rh_pinky_dip
|
||||
rh_ring_mcp_pitch: rh_pinky_mcp_pitch
|
||||
rh_ring_dip: rh_pinky_dip
|
||||
measurements:
|
||||
rh_thumb_cmc_yaw:
|
||||
joint: rh_thumb_cmc_yaw
|
||||
kind: relative_rotation
|
||||
view: top
|
||||
parent_role: top_base
|
||||
child_role: thumb_yaw
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
rh_thumb_cmc_pitch:
|
||||
joint: rh_thumb_cmc_pitch
|
||||
kind: relative_rotation
|
||||
view: front
|
||||
parent_role: front_base
|
||||
child_role: thumb_pitch
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
rh_thumb_ip:
|
||||
joint: rh_thumb_ip
|
||||
kind: relative_rotation
|
||||
view: front
|
||||
parent_role: thumb_pitch
|
||||
child_role: thumb_ip
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
rh_pinky_mcp_pitch:
|
||||
joint: rh_pinky_mcp_pitch
|
||||
kind: relative_rotation
|
||||
view: side
|
||||
parent_role: side_base
|
||||
child_role: pinky_pitch
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
rh_pinky_dip:
|
||||
joint: rh_pinky_dip
|
||||
kind: relative_rotation
|
||||
view: side
|
||||
parent_role: pinky_pitch
|
||||
child_role: pinky_dip
|
||||
validation_source: null
|
||||
pose_axis_line_required: true
|
||||
cross_view_sources: {}
|
||||
image_curve_joints: []
|
||||
directional_zero: true
|
||||
cross_view_roll_curve: false
|
||||
stable_cross_view_cone_bias: false
|
||||
zero:
|
||||
cad_zero_assumptions:
|
||||
rh_index_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
rh_middle_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
rh_pinky_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
rh_ring_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
rh_thumb_ip: User accepts original CAD joint zero as the physical baseline; not
|
||||
independently measured.
|
||||
active_joints:
|
||||
- rh_index_mcp_pitch
|
||||
- rh_middle_mcp_pitch
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_ring_mcp_pitch
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_cmc_yaw
|
||||
passive_joints:
|
||||
- rh_index_dip
|
||||
- rh_middle_dip
|
||||
- rh_pinky_dip
|
||||
- rh_ring_dip
|
||||
- rh_thumb_ip
|
||||
direct_zero_joints:
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_cmc_yaw
|
||||
axis_joints:
|
||||
- rh_pinky_dip
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_cmc_yaw
|
||||
- rh_thumb_ip
|
||||
mechanical_endpoint_joints: []
|
||||
post_solve_endpoint_joints: []
|
||||
mimic_source_by_joint:
|
||||
rh_thumb_ip: rh_thumb_cmc_pitch
|
||||
rh_index_dip: rh_index_mcp_pitch
|
||||
rh_middle_dip: rh_middle_mcp_pitch
|
||||
rh_ring_dip: rh_ring_mcp_pitch
|
||||
rh_pinky_dip: rh_pinky_mcp_pitch
|
||||
cad_frozen_joints:
|
||||
- rh_index_dip
|
||||
- rh_middle_dip
|
||||
- rh_pinky_dip
|
||||
- rh_ring_dip
|
||||
- rh_thumb_ip
|
||||
endpoint_anchor_by_joint: {}
|
||||
fitted_mimic_joints:
|
||||
- rh_pinky_dip
|
||||
- rh_thumb_ip
|
||||
coupling_model_by_joint:
|
||||
rh_thumb_ip: linear_mimic
|
||||
rh_index_dip: linear_mimic
|
||||
rh_middle_dip: linear_mimic
|
||||
rh_ring_dip: linear_mimic
|
||||
rh_pinky_dip: linear_mimic
|
||||
transferred_zero_sources: {rh_index_mcp_pitch: rh_pinky_mcp_pitch, rh_middle_mcp_pitch: rh_pinky_mcp_pitch, rh_ring_mcp_pitch: rh_pinky_mcp_pitch}
|
||||
transferred_mimic_sources: {rh_index_dip: rh_pinky_dip, rh_middle_dip: rh_pinky_dip, rh_ring_dip: rh_pinky_dip}
|
||||
spatial:
|
||||
base_pose_strategy: thumb_serial
|
||||
root_anchor_joints: [rh_thumb_cmc_yaw]
|
||||
orientation_anchor_joint: rh_pinky_mcp_pitch
|
||||
directed_base_axis_joints: [rh_thumb_cmc_yaw, rh_pinky_mcp_pitch]
|
||||
depth_free_axis_projection: true
|
||||
axis_order: [rh_thumb_cmc_yaw, rh_thumb_cmc_pitch, rh_thumb_ip, rh_pinky_mcp_pitch, rh_pinky_dip]
|
||||
axis_parent_joint: {rh_thumb_cmc_pitch: rh_thumb_cmc_yaw}
|
||||
phase_parent_joint: {rh_thumb_ip: rh_thumb_cmc_pitch, rh_pinky_dip: rh_pinky_mcp_pitch}
|
||||
offset_observer_joint: {rh_thumb_cmc_yaw: rh_thumb_cmc_pitch, rh_thumb_cmc_pitch: rh_thumb_ip, rh_pinky_mcp_pitch: rh_pinky_dip}
|
||||
quality:
|
||||
training_cycles:
|
||||
- 0
|
||||
- 1
|
||||
- 2
|
||||
holdout_cycle: 3
|
||||
hard_threshold_keys:
|
||||
- maximum_mimic_residual_rad
|
||||
- maximum_state_image_skew_ms
|
||||
- maximum_validation_error_rad
|
||||
- minimum_detection_rate
|
||||
retry_metric_scope: {}
|
||||
isolated_holdout: true
|
||||
scope:
|
||||
calibrate_joints:
|
||||
partial:
|
||||
- rh_pinky_mcp_pitch
|
||||
- rh_thumb_cmc_pitch
|
||||
- rh_thumb_cmc_yaw
|
||||
frozen_joints:
|
||||
partial:
|
||||
- rh_index_mcp_pitch
|
||||
- rh_middle_mcp_pitch
|
||||
- rh_ring_mcp_pitch
|
||||
default_scope: partial
|
||||
artifacts:
|
||||
output_schema_version: 3
|
||||
directional_command_joints:
|
||||
- rh_thumb_cmc_yaw
|
||||
calibration_filename: o6_right_{serial_number}_partial_calibration.json
|
||||
corrected_urdf_filename: linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf
|
||||
protected_input_fields:
|
||||
- calibration_config_sha256
|
||||
- camera_extrinsics_sha256
|
||||
- profile_config_sha256
|
||||
- source_urdf_sha256
|
||||
- tag_config_sha256
|
||||
publication_pointer: latest_partial_passed
|
||||
session_compatibility_tokens:
|
||||
- feedback_curves_v6
|
||||
- o6_partial_v1
|
||||
publish_corrected_urdf: true
|
||||
acquisition:
|
||||
joint_zero_timeout_seconds: 5.0
|
||||
command_capture_mode: separate
|
||||
steady_training_nodes: 9
|
||||
steady_extra_training_nodes:
|
||||
pinky_pitch_dip_side: [239.0]
|
||||
policy_version: unified_engine_v8_all_view_images
|
||||
mapping_probe_maximum_rad: 0.0
|
||||
automatic_rescan_limit: 1
|
||||
minimum_valid_samples: 40
|
||||
minimum_bins: 32
|
||||
maximum_unobserved_fraction: 0.0625
|
||||
legacy_minimum_span_01: 0.9411764705882353
|
||||
physical_first_cycle_minimum_span_01: 0.85
|
||||
physical_repeat_minimum_fraction: 0.9
|
||||
stall_timeout_seconds: 2.0
|
||||
feedback_stale_seconds: 1.0
|
||||
fixed_reference_minimum_frames: 10
|
||||
fixed_reference_maximum_drift_px: 5.0
|
||||
fixed_reference_confirmation_frames: 10
|
||||
urdf:
|
||||
limit_policies:
|
||||
rh_pinky_mcp_pitch:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_thumb_cmc_pitch:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_middle_mcp_pitch:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_index_mcp_pitch:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_ring_mcp_pitch:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_thumb_cmc_yaw:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_pinky_dip:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_thumb_ip:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_middle_dip:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_ring_dip:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
rh_index_dip:
|
||||
source_limits: calibrated_motion_envelope
|
||||
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
|
||||
authorized_fields:
|
||||
rh_pinky_mcp_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_index_mcp_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_thumb_cmc_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_middle_mcp_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_ring_mcp_pitch:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_thumb_cmc_yaw:
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
- origin.rpy
|
||||
rh_ring_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
rh_index_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
rh_pinky_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
rh_thumb_ip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
rh_middle_dip:
|
||||
- mimic.multiplier
|
||||
- mimic.offset
|
||||
- limit.lower
|
||||
- limit.upper
|
||||
joint_coverage:
|
||||
rh_pinky_mcp_pitch: measured_static_dynamic
|
||||
rh_thumb_cmc_pitch: measured_static_dynamic
|
||||
rh_middle_mcp_pitch: transferred_static_dynamic
|
||||
rh_index_mcp_pitch: transferred_static_dynamic
|
||||
rh_ring_mcp_pitch: transferred_static_dynamic
|
||||
rh_thumb_cmc_yaw: measured_static_dynamic
|
||||
rh_pinky_dip: measured_dynamic_cad_static
|
||||
rh_thumb_ip: measured_dynamic_cad_static
|
||||
rh_middle_dip: transferred_dynamic_cad_static
|
||||
rh_ring_dip: transferred_dynamic_cad_static
|
||||
rh_index_dip: transferred_dynamic_cad_static
|
||||
@@ -0,0 +1,44 @@
|
||||
$schema: https://json-schema.org/draft/2020-12/schema
|
||||
title: LinkerHand calibration profile
|
||||
type: object
|
||||
additionalProperties: false
|
||||
required:
|
||||
- schema_version
|
||||
- profile_id
|
||||
- namespace
|
||||
- sdk_adapter
|
||||
- command
|
||||
- vision
|
||||
- motion
|
||||
- measurement
|
||||
- zero
|
||||
- quality
|
||||
- scope
|
||||
- artifacts
|
||||
- urdf
|
||||
properties:
|
||||
schema_version: {const: 1}
|
||||
profile_id: {type: string, pattern: "^[A-Za-z0-9_-]+/(left|right)/[a-z0-9_-]+/v[1-9][0-9]*$"}
|
||||
namespace: {type: string, pattern: "^/[^/].*[^/]$"}
|
||||
sdk_adapter: {type: string, minLength: 1}
|
||||
command: {type: object}
|
||||
vision: {type: object}
|
||||
motion: {type: object}
|
||||
measurement: {type: object}
|
||||
zero: {type: object}
|
||||
quality: {type: object}
|
||||
scope: {type: object}
|
||||
artifacts: {type: object}
|
||||
acquisition: {type: object}
|
||||
urdf:
|
||||
type: object
|
||||
required: [authorized_fields]
|
||||
properties:
|
||||
authorized_fields:
|
||||
type: object
|
||||
additionalProperties:
|
||||
type: array
|
||||
uniqueItems: true
|
||||
items:
|
||||
enum: [origin.rpy, limit.lower, limit.upper, mimic.multiplier, mimic.offset]
|
||||
joint_coverage: {type: object}
|
||||
@@ -0,0 +1,14 @@
|
||||
$schema: https://json-schema.org/draft/2020-12/schema
|
||||
title: LinkerHand calibration product
|
||||
type: object
|
||||
required: [schema_version, profile_id, serial_number, cameras, artifacts]
|
||||
properties:
|
||||
schema_version: {type: integer, minimum: 2}
|
||||
profile_id: {type: string}
|
||||
profile_config: {type: string}
|
||||
serial_number: {type: string, pattern: "^[A-Za-z0-9_.-]+$"}
|
||||
namespace: {type: string}
|
||||
sdk: {type: object}
|
||||
cameras: {type: object, minProperties: 1}
|
||||
artifacts: {type: object}
|
||||
release: {type: object}
|
||||
@@ -0,0 +1,191 @@
|
||||
g20_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /g20/cb_left_hand_control_cmd
|
||||
state_topic: /g20/cb_left_hand_state
|
||||
info_topic: /g20/cb_left_hand_info
|
||||
setting_topic: /g20/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /g20_calibration/front/camera/camera_info
|
||||
front_detections_topic: /g20_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /g20_calibration/side/camera/camera_info
|
||||
side_detections_topic: /g20_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /g20_calibration/top/camera/camera_info
|
||||
top_detections_topic: /g20_calibration/top/apriltag/detections
|
||||
|
||||
# /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
normal_calibration_speed: 15
|
||||
index_roll_calibration_speed: 5
|
||||
index_flex_calibration_speed: 10
|
||||
# 19-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量，
|
||||
# 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束，
|
||||
# 始终保持速度5；任一方向采样余量不足也保持原速度。
|
||||
adaptive_formal_speed_enabled: true
|
||||
adaptive_formal_speed_max_scale: 1.5
|
||||
adaptive_formal_speed_minimum_bins: 64
|
||||
adaptive_formal_speed_maximum_bin_gap: 8
|
||||
speed_setting_settle_seconds: 0.25
|
||||
|
||||
# tag36h11尺寸是检测角点围成的黑色正方形边长，不包含外围白边。
|
||||
# 19张Tag的黑色码区外边长均为16 mm。自定义PnP必须与
|
||||
# apriltag_ros逐ID尺寸一致，禁止用纸张/白边尺寸代替码区尺寸。
|
||||
tag_size_m: 0.016
|
||||
# ROS 2无法从YAML空数组推断整数/浮点数组类型。这四个
|
||||
# 末端Tag仍显式写16 mm，防止节点启动时得到未初始化参数。
|
||||
tag_size_override_ids: [7, 14, 16, 18]
|
||||
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016]
|
||||
repetitions: 3
|
||||
# 19-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。
|
||||
g20_right_19_repetitions: 4
|
||||
preflight_frames: 60
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_detection_hz: 15.0
|
||||
minimum_feedback_hz: 25.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
pnp_maximum_translation_jump_m: 0.04
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
pnp_tracker_reset_seconds: 5.0
|
||||
# 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
|
||||
pnp_group_initialization_frames: 8
|
||||
# 侧面当前任务所需Tag在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
|
||||
pnp_group_normal_alignment_scale_deg: 5.0
|
||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||
# 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见；
|
||||
# 任一角点相对会话基准漂移超过2 px并连续5帧时，判定标定中基准被移动。
|
||||
fixed_base_maximum_corner_drift_px: 5.0
|
||||
fixed_base_movement_confirmation_frames: 10
|
||||
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic
|
||||
# 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、
|
||||
# 缩放或替代被动IP的自身Tag实测曲线。
|
||||
thumb_ip_pnp_coupling_multiplier: 1.03
|
||||
thumb_ip_pnp_coupling_scale_deg: 3.0
|
||||
thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
|
||||
top_pnp_invalid_reset_seconds: 1.0
|
||||
# 三维位姿必须与实测20通道状态严格按时间戳配对。
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
|
||||
axis_maximum_plane_rms_m: 0.003
|
||||
# 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
|
||||
# 轴线一致性仍沿用严格检查。
|
||||
passive_axis_maximum_plane_rms_m: 0.004
|
||||
axis_maximum_radial_rms_m: 0.003
|
||||
# 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
|
||||
axis_maximum_pose_line_rms_m: 0.001
|
||||
# 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
|
||||
# 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
|
||||
axis_maximum_rotation_circle_difference_deg: 1.0
|
||||
# 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
|
||||
# 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及最终轮留出。
|
||||
active_maximum_rotation_orthogonal_rms_deg: 2.5
|
||||
passive_maximum_rotation_orthogonal_rms_deg: 7.5
|
||||
zero_maximum_axis_cycle_difference_deg: 0.75
|
||||
# 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
|
||||
zero_maximum_axis_cone_mismatch_deg: 5.0
|
||||
zero_maximum_observability_condition_number: 10000000000.0
|
||||
zero_maximum_offset_deg: 20.0
|
||||
# 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何
|
||||
# 求解且不假定电气端点等于CAD上限；thumb_mcp及四指MCP pitch/PIP
|
||||
# 静态零位由实测全行程与CAD机械端点联合求解，不写死为0。
|
||||
zero_finger_maximum_offset_deg: 3.0
|
||||
# 只对实物已确认等同CAD端点的关节使用该限制；CMC电气端点不作此假设。
|
||||
mechanical_endpoint_maximum_offset_deg: 5.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈
|
||||
# 必须已经稳定；大残差仍由机械卡滞保护处理。
|
||||
steady_checkpoint_command_feedback_tolerance_u8: 8.0
|
||||
steady_checkpoint_maximum_feedback_range_u8: 2.0
|
||||
# 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
|
||||
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
|
||||
# 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
|
||||
right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
|
||||
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
|
||||
pinky_pip_zero_endpoint_tolerance_u8: 5.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
# roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。
|
||||
baseline_hold_seconds: 0.5
|
||||
minimum_baseline_hold_frames: 10
|
||||
# unified_engine_v2 不执行每任务全行程预检；保留参数仅兼容旧配置读取。
|
||||
task_precheck_hold_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
# 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
|
||||
# 这类硬故障不自动重试。
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
motor_stall_startup_grace_seconds: 1.0
|
||||
motor_stall_minimum_progress_u8: 1.0
|
||||
invalid_timeout_seconds: 3.0
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
# 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
|
||||
automatic_sweep_retry_limit: 1
|
||||
# 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
|
||||
automatic_fit_retry_limit: 0
|
||||
automatic_motion_retry_limit: 0
|
||||
# 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll，
|
||||
# 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。
|
||||
cross_view_roll_diagnostic_finger: ""
|
||||
# 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
|
||||
provisional_warning_ratio: 1.25
|
||||
retry_minimum_speed: 3
|
||||
retry_speed_scales: [1.0]
|
||||
retry_endpoint_hold_seconds: [0.5]
|
||||
|
||||
trajectory_maximum_plane_rms_m: 0.004
|
||||
trajectory_maximum_radial_rms_m: 0.004
|
||||
trajectory_minimum_radius_m: 0.003
|
||||
trajectory_minimum_arc_deg: 15.0
|
||||
# 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
|
||||
image_trajectory_maximum_radial_rms_px: 2.0
|
||||
image_trajectory_maximum_radial_p95_px: 3.5
|
||||
image_trajectory_minimum_radius_px: 20.0
|
||||
trajectory_maximum_cycle_travel_difference_deg: 3.0
|
||||
passive_maximum_cycle_travel_difference_deg: 10.0
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
# 旧布局仍用连续扫描正反程差门限；19-Tag产品的连续运动包含速度相关滞后，
|
||||
# 由方向曲线和最终留出验证建模，不再重复硬判。其绝对正反程门禁使用下面
|
||||
# 的九点稳态command_maximum_direction_gap_deg。
|
||||
maximum_hysteresis_deg: 2.0
|
||||
# 19-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
|
||||
# 不超过0.5°；四指roll例外：127以255→127为唯一物理零位，反向分支
|
||||
# 保留实测偏差，并改为检查分支间隙上限及跨轮稳定性。
|
||||
baseline_maximum_hysteresis_deg: 0.5
|
||||
directional_zero_maximum_branch_gap_deg: 2.0
|
||||
directional_zero_maximum_branch_gap_range_deg: 0.3
|
||||
cross_view_roll_maximum_branch_gap_difference_deg: 0.3
|
||||
# 正面roll是Tag中心的二维投影角，侧面roll是三维姿态角。允许一个有界的
|
||||
# 固定比例吸收Tag安装倾角/偏置带来的投影缩放，再严格比较两条曲线形状；
|
||||
# 比例过大、方向相反、形状RMS及两视角各自的四轮重复性仍会失败。
|
||||
cross_view_roll_maximum_shape_rms_deg: 1.25
|
||||
cross_view_roll_maximum_projection_scale_ratio: 1.5
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
passive_maximum_hysteresis_deg: 2.0
|
||||
# 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域
|
||||
# 的两条运行曲线仍原样保留固件方向死区，不能把command/feedback差算成回差。
|
||||
command_maximum_direction_gap_deg: 2.0
|
||||
|
||||
# 默认无额外随机动作；19-Tag产品最终一轮始终作为不可关闭的留出验证。
|
||||
validation_enabled: false
|
||||
# 第四轮留出求解后必须再走8个固定安全组合姿态；三机位规定Tag全部可见
|
||||
# 且实测20通道到位才允许发布。只保存Tag位姿，不保存原始图像。
|
||||
# Developer diagnostic only. The formal fourth sweep cycle already gives
|
||||
# every isolated PIP/DIP pair an independent holdout.
|
||||
combination_validation_enabled: false
|
||||
combination_validation_frames: 10
|
||||
combination_maximum_position_p95_m: 0.003
|
||||
combination_maximum_orientation_p95_deg: 2.0
|
||||
validation_command_count: 3
|
||||
validation_frames: 10
|
||||
validation_seed: 20260804
|
||||
validation_timeout_seconds: 20.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
# 19-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
|
||||
maximum_validation_error_deg: 3.0
|
||||
zero_maximum_confidence_half_width_deg: 1.5
|
||||
@@ -0,0 +1,62 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2, 3, 10, 11, 12, 13]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 15, 17]
|
||||
frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [8, 9]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.016, 0.016]
|
||||
@@ -0,0 +1,63 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2, 3, 10, 11, 12, 13]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
# Distal Tags use the same measured 16 mm black-code edge as all others.
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 7, 14, 15, 16, 17, 18]
|
||||
frames: [side_base, ring_pip, pinky_pip, pinky_dip, ring_dip, middle_pip, middle_dip, index_pip, index_dip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [8, 9]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.016, 0.016]
|
||||
@@ -0,0 +1,21 @@
|
||||
"""One-release compatibility surface for the former Python package name.
|
||||
|
||||
New code must import :mod:`linkerhand_calibration`. Only the documented
|
||||
configuration loader is re-exported here; calibration algorithms continue to
|
||||
have a single implementation in the renamed package.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import warnings
|
||||
|
||||
warnings.warn(
|
||||
"g20_thumb_apriltag_calibration is deprecated; "
|
||||
"import linkerhand_calibration instead",
|
||||
DeprecationWarning,
|
||||
stacklevel=2,
|
||||
)
|
||||
|
||||
from linkerhand_calibration.product import ProductConfig, load_product_config
|
||||
|
||||
__all__ = ["ProductConfig", "load_product_config"]
|
||||
+9
@@ -0,0 +1,9 @@
|
||||
"""Deprecated forwarding entry point for the runtime joint-state bridge."""
|
||||
|
||||
from linkerhand_calibration.calibrated_joint_state_bridge import main
|
||||
|
||||
__all__ = ["main"]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,9 @@
|
||||
"""Deprecated forwarding entry point for offline replay."""
|
||||
|
||||
from linkerhand_calibration.offline_replay import main
|
||||
|
||||
__all__ = ["main"]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,9 @@
|
||||
"""Deprecated forwarding entry point for the former Python package."""
|
||||
|
||||
from linkerhand_calibration.one_command import main
|
||||
|
||||
__all__ = ["main"]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+3
-3
@@ -1,4 +1,4 @@
|
||||
"""Publish calibrated G20 URDF joint angles from raw u8 commands."""
|
||||
"""Publish profile-calibrated URDF angles from raw command/feedback u8 values."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
@@ -14,10 +14,10 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument("input_topic", default_value=""),
|
||||
DeclareLaunchArgument("output_topic", default_value=""),
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="calibrated_joint_state_bridge",
|
||||
name=[
|
||||
"g20_calibrated_joint_state_bridge_",
|
||||
"calibrated_joint_state_bridge_",
|
||||
LaunchConfiguration("hand_type"),
|
||||
],
|
||||
output="screen",
|
||||
+182
-146
@@ -1,8 +1,9 @@
|
||||
"""Launch three Hikrobot views and one complete-G20 calibration owner."""
|
||||
"""Launch Profile-declared Hikrobot views and one calibration owner."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
import hashlib
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
@@ -22,36 +23,70 @@ from launch_ros.descriptions import ComposableNode
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
def _default_source_urdf(hand_type: str) -> Path:
|
||||
relative = Path(
|
||||
"assets/robots/hands/linker_hand"
|
||||
) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
|
||||
workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative
|
||||
try:
|
||||
installed = Path(get_package_share_directory("linkerhand_retarget")) / relative
|
||||
except Exception:
|
||||
installed = workspace
|
||||
return workspace if workspace.is_file() else installed
|
||||
|
||||
|
||||
def _launch_stack(context):
|
||||
from linkerhand_calibration.product import (
|
||||
ProductCalibrationContract,
|
||||
)
|
||||
from linkerhand_calibration.profiles import load_hand_profile
|
||||
from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics
|
||||
from linkerhand_calibration.runtime.diagnostic_capture import resolve_diagnostic_capture
|
||||
from linkerhand_calibration.runtime.observation_scope import required_observation_views
|
||||
|
||||
model = LaunchConfiguration("model").perform(context).strip().upper()
|
||||
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise RuntimeError("hand_type must be left or right")
|
||||
command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
|
||||
state_topic = f"/g20/cb_{hand_type}_hand_state"
|
||||
info_topic = f"/g20/cb_{hand_type}_hand_info"
|
||||
requested_source = LaunchConfiguration("source_urdf_path").perform(context)
|
||||
source_urdf = (
|
||||
Path(requested_source).expanduser().resolve()
|
||||
if requested_source
|
||||
else _default_source_urdf(hand_type).resolve()
|
||||
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
|
||||
try:
|
||||
profile_path = LaunchConfiguration("profile_config").perform(context).strip()
|
||||
if not profile_path:
|
||||
raise ValueError("online calibration requires a protected YAML Profile; use calibrate_hand --config")
|
||||
expected = LaunchConfiguration("profile_config_expected_sha256").perform(context).strip()
|
||||
if not expected or hashlib.sha256(Path(profile_path).read_bytes()).hexdigest() != expected:
|
||||
raise ValueError("Profile changed between product validation and launch")
|
||||
contract = ProductCalibrationContract(declarative=load_hand_profile(profile_path))
|
||||
key = contract.typed_profile.key
|
||||
if (key.model, key.side, key.layout) != (model, hand_type, tag_layout):
|
||||
raise ValueError("launch identity differs from the protected Profile")
|
||||
except ValueError as error:
|
||||
raise RuntimeError(str(error)) from error
|
||||
diagnostic = resolve_diagnostic_capture(
|
||||
contract.typed_profile,
|
||||
LaunchConfiguration("diagnostic_capture").perform(context).strip(),
|
||||
)
|
||||
views = required_observation_views(contract.typed_profile, diagnostic)
|
||||
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
if not requested_tag_config:
|
||||
raise RuntimeError("tag_config is required; use calibrate_hand --config")
|
||||
tag_config = Path(requested_tag_config).expanduser().resolve()
|
||||
if not tag_config.is_file():
|
||||
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
||||
topic_prefix = f"/{model.lower()}"
|
||||
uses_hcan = contract.typed_profile.sdk_adapter == "o12_hcan_sdk"
|
||||
if contract.typed_profile.sdk_adapter not in {"legacy_byte_sdk", "o12_hcan_sdk"}:
|
||||
raise RuntimeError("SDK adapter has no ROS launch binding")
|
||||
topics = sdk_topics(contract.typed_profile)
|
||||
command_topic, state_topic = topics.command, topics.feedback
|
||||
requested_source = LaunchConfiguration("source_urdf_path").perform(context)
|
||||
if not requested_source:
|
||||
raise RuntimeError("source_urdf_path is required; use calibrate_hand --config")
|
||||
source_urdf = Path(requested_source).expanduser().resolve()
|
||||
if not source_urdf.is_file():
|
||||
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
|
||||
expected_source_hash = LaunchConfiguration(
|
||||
"source_urdf_expected_sha256"
|
||||
).perform(context).strip().lower()
|
||||
if contract.typed_profile.artifacts.publish_corrected_urdf:
|
||||
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
||||
raise RuntimeError(
|
||||
"this profile requires source_urdf_expected_sha256 confirmed "
|
||||
"by the CAD/hardware owner"
|
||||
)
|
||||
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
|
||||
if actual_source_hash != expected_source_hash:
|
||||
raise RuntimeError(
|
||||
"source_urdf_expected_sha256 does not match source_urdf_path"
|
||||
)
|
||||
|
||||
hand_serial = LaunchConfiguration("serial_number").perform(context)
|
||||
if (
|
||||
@@ -78,31 +113,32 @@ def _launch_stack(context):
|
||||
|
||||
camera_serials = {
|
||||
view: LaunchConfiguration(f"{view}_camera_serial").perform(context)
|
||||
for view in VIEWS
|
||||
for view in views
|
||||
}
|
||||
if any(not serial for serial in camera_serials.values()):
|
||||
raise RuntimeError("all three camera serial numbers are required")
|
||||
if len(set(camera_serials.values())) != 3:
|
||||
raise RuntimeError("front/side/top camera serial numbers must be unique")
|
||||
raise RuntimeError("all required observation camera serial numbers are required")
|
||||
if len(set(camera_serials.values())) != len(views):
|
||||
raise RuntimeError("required observation camera serial numbers must be unique")
|
||||
|
||||
cameras = []
|
||||
components = []
|
||||
raw_topics = []
|
||||
info_topics = []
|
||||
detection_topics = []
|
||||
for view in VIEWS:
|
||||
namespace = f"/g20_calibration/{view}/camera"
|
||||
calibration_namespace = contract.typed_profile.namespace
|
||||
for view in views:
|
||||
namespace = f"{calibration_namespace}/{view}/camera"
|
||||
raw_topic = f"{namespace}/image_raw"
|
||||
info_topic = f"{namespace}/camera_info"
|
||||
rect_topic = f"{namespace}/image_rect"
|
||||
detector_namespace = f"/g20_calibration/{view}/apriltag"
|
||||
detector_namespace = f"{calibration_namespace}/{view}/apriltag"
|
||||
detection_topic = f"{detector_namespace}/detections"
|
||||
raw_topics.append(raw_topic)
|
||||
info_topics.append(info_topic)
|
||||
detection_topics.append(detection_topic)
|
||||
cameras.append(
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=namespace,
|
||||
@@ -118,9 +154,13 @@ def _launch_stack(context):
|
||||
"camera_name": LaunchConfiguration(
|
||||
f"{view}_camera_name"
|
||||
),
|
||||
"frame_id": f"g20_calibration_{view}_optical_frame",
|
||||
"frame_id": (
|
||||
f"{model.lower()}_calibration_{view}_optical_frame"
|
||||
),
|
||||
"image_width": 1624,
|
||||
"image_height": 1240,
|
||||
"timestamp_journal_path": str(Path(LaunchConfiguration("session_dir").perform(context))
|
||||
/ f"camera_timing_{view}.jsonl"),
|
||||
"frame_rate": ParameterValue(
|
||||
LaunchConfiguration("camera_frame_rate"),
|
||||
value_type=float,
|
||||
@@ -162,15 +202,10 @@ def _launch_stack(context):
|
||||
plugin="AprilTagNode",
|
||||
name="apriltag",
|
||||
namespace=detector_namespace,
|
||||
parameters=[
|
||||
LaunchConfiguration("tag_config"),
|
||||
{
|
||||
"detector.decimate": ParameterValue(
|
||||
LaunchConfiguration("apriltag_decimate"),
|
||||
value_type=float,
|
||||
)
|
||||
},
|
||||
],
|
||||
# Detector settings belong to the protected model YAML.
|
||||
# A generic launch default must not silently replace its
|
||||
# full-resolution setting for the small calibration Tags.
|
||||
parameters=[str(tag_config)],
|
||||
remappings=[
|
||||
("image_rect", rect_topic),
|
||||
("camera_info", info_topic),
|
||||
@@ -181,7 +216,7 @@ def _launch_stack(context):
|
||||
)
|
||||
|
||||
vision = ComposableNodeContainer(
|
||||
name="g20_three_camera_vision",
|
||||
name=f"{model.lower()}_calibration_vision",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
@@ -189,19 +224,35 @@ def _launch_stack(context):
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
)
|
||||
sdk = Node(
|
||||
package="linker_hand_ros2_sdk",
|
||||
executable="linker_hand_sdk",
|
||||
name="linker_hand_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[
|
||||
{
|
||||
sdk = (
|
||||
Node(
|
||||
package="linkerhand_calibration",
|
||||
executable="o12_sdk_bridge",
|
||||
name="o12_sdk_bridge",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[{
|
||||
"vendor_config": LaunchConfiguration("vendor_sdk_config"),
|
||||
"vendor_config_sha256": LaunchConfiguration("sdk_config_expected_sha256"),
|
||||
"vendor_python_package": LaunchConfiguration("vendor_sdk_python_package"),
|
||||
"vendor_package_sha256": LaunchConfiguration("sdk_package_expected_sha256"),
|
||||
"hand_type": hand_type,
|
||||
"hand_joint": "G20",
|
||||
"topic_prefix": f"/{model.lower()}/{hand_type}",
|
||||
}],
|
||||
)
|
||||
if uses_hcan
|
||||
else Node(
|
||||
package="linker_hand_ros2_sdk",
|
||||
executable="linker_hand_sdk",
|
||||
name="linker_hand_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[{
|
||||
"hand_type": hand_type,
|
||||
"hand_joint": model,
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": "/g20",
|
||||
"topic_prefix": topic_prefix,
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
@@ -213,62 +264,65 @@ def _launch_stack(context):
|
||||
# G20 velocity read sends another five synchronous CAN
|
||||
# queries, so keep it off the trajectory-critical path.
|
||||
"velocity_poll_rate": 1.0,
|
||||
# Calibration sends one endpoint command per sweep. Keep
|
||||
# polling the real motor state during the following motion;
|
||||
# otherwise the SDK republishes a stale state for 0.2 s and
|
||||
# creates 17-27 command-unit holes in the trajectory bins.
|
||||
# G20 sends an endpoint and L6 streams a bounded trajectory.
|
||||
# Keep polling the real motor state during either command path;
|
||||
# otherwise the SDK republishes stale state and creates large
|
||||
# command-unit holes in the trajectory bins.
|
||||
"defer_state_reads_while_commanding": False,
|
||||
"repeat_position_commands": False,
|
||||
"is_touch": False,
|
||||
}
|
||||
],
|
||||
}],
|
||||
)
|
||||
)
|
||||
calibration = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="three_camera_calibration_node",
|
||||
name="g20_calibration",
|
||||
name=f"{model.lower()}_calibration",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
arguments=[
|
||||
"--profile-id",
|
||||
contract.typed_profile.key.profile_id,
|
||||
"--profile-config", profile_path, "--profile-sha256", expected,
|
||||
],
|
||||
parameters=[
|
||||
LaunchConfiguration("calibration_config"),
|
||||
{
|
||||
"serial_number": hand_serial,
|
||||
"hand_type": hand_type,
|
||||
"session_dir": str(session_dir),
|
||||
# The SDK performs roughly 25 synchronous CAN queries whenever
|
||||
# cb_<side>_hand_info has a subscriber. Calibration only used
|
||||
# that topic to display a speed diagnostic, while those reads
|
||||
# created 17-33 command-unit holes in position trajectories.
|
||||
"info_topic": "/g20_calibration/disabled_hand_info",
|
||||
"resume_raw_samples_path": LaunchConfiguration(
|
||||
"resume_raw_samples_path"
|
||||
),
|
||||
"command_topic": command_topic,
|
||||
"state_topic": state_topic,
|
||||
"camera_extrinsics_file": LaunchConfiguration(
|
||||
"camera_extrinsics_file"
|
||||
),
|
||||
"source_urdf_path": str(source_urdf),
|
||||
"corrected_urdf_output_dir": LaunchConfiguration(
|
||||
"corrected_urdf_output_dir"
|
||||
"source_urdf_expected_sha256": LaunchConfiguration(
|
||||
"source_urdf_expected_sha256"
|
||||
),
|
||||
"camera_extrinsics_expected_sha256": LaunchConfiguration(
|
||||
"camera_extrinsics_expected_sha256"
|
||||
),
|
||||
"calibration_config_expected_sha256": LaunchConfiguration(
|
||||
"calibration_config_expected_sha256"
|
||||
),
|
||||
"tag_config_expected_sha256": LaunchConfiguration(
|
||||
"tag_config_expected_sha256"
|
||||
),
|
||||
"sdk_config_expected_sha256": LaunchConfiguration(
|
||||
"sdk_config_expected_sha256"
|
||||
),
|
||||
"sdk_package_expected_sha256": LaunchConfiguration("sdk_package_expected_sha256"),
|
||||
"profile_config_expected_sha256": LaunchConfiguration(
|
||||
"profile_config_expected_sha256"
|
||||
),
|
||||
**{
|
||||
f"{view}_camera_serial": camera_serials[view]
|
||||
for view in VIEWS
|
||||
},
|
||||
"commands_enabled": ParameterValue(
|
||||
LaunchConfiguration("commands_enabled"), value_type=bool
|
||||
),
|
||||
"normal_calibration_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
),
|
||||
"index_roll_calibration_speed": ParameterValue(
|
||||
LaunchConfiguration("index_roll_calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"index_flex_calibration_speed": ParameterValue(
|
||||
LaunchConfiguration("index_flex_calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"validation_enabled": ParameterValue(
|
||||
LaunchConfiguration("validation_enabled"), value_type=bool
|
||||
"diagnostic_capture": ParameterValue(
|
||||
LaunchConfiguration("diagnostic_capture"), value_type=str
|
||||
),
|
||||
},
|
||||
],
|
||||
@@ -292,27 +346,34 @@ def _launch_stack(context):
|
||||
*detection_topics,
|
||||
command_topic,
|
||||
state_topic,
|
||||
info_topic,
|
||||
"/g20_calibration/status",
|
||||
*(
|
||||
[
|
||||
f"/{model.lower()}/{hand_type}/calibration_health",
|
||||
]
|
||||
if uses_hcan else []
|
||||
),
|
||||
f"{calibration_namespace}/status",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
return [
|
||||
LogInfo(
|
||||
msg=(
|
||||
f"G20 {hand_type} three-camera session: {session_dir}; "
|
||||
f"{model} {hand_type} {tag_layout} calibration session: {session_dir}; "
|
||||
f"source_urdf={source_urdf}"
|
||||
)
|
||||
),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"Camera mapping: front="
|
||||
f"{camera_serials['front']} side={camera_serials['side']} "
|
||||
f"top={camera_serials['top']}"
|
||||
"Camera mapping: " + " ".join(f"{view}={serial}" for view, serial in camera_serials.items())
|
||||
)
|
||||
),
|
||||
*cameras,
|
||||
vision,
|
||||
Node(package="linkerhand_calibration", executable="tag_border_filter_node",
|
||||
name="tag_border_filter", output="screen",
|
||||
arguments=["--profile-config", profile_path, "--profile-sha256", expected,
|
||||
"--views", *views]),
|
||||
sdk,
|
||||
calibration,
|
||||
bag,
|
||||
@@ -321,9 +382,8 @@ def _launch_stack(context):
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
)
|
||||
info_root = Path.home() / ".ros" / "camera_info"
|
||||
return LaunchDescription(
|
||||
[
|
||||
# Camera processes publish ~2 MB frames across DDS. Force the
|
||||
@@ -341,66 +401,45 @@ def generate_launch_description() -> LaunchDescription:
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
DeclareLaunchArgument("hand_type", default_value="left"),
|
||||
DeclareLaunchArgument("model", default_value=""),
|
||||
DeclareLaunchArgument("hand_type", default_value=""),
|
||||
DeclareLaunchArgument("tag_layout", default_value=""),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument(
|
||||
"front_camera_serial", default_value="DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"side_camera_serial", default_value="DB2163749"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"top_camera_serial", default_value="DB2163739"
|
||||
),
|
||||
DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
|
||||
DeclareLaunchArgument(
|
||||
"front_camera_name", default_value="hikrobot_front_DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"side_camera_name", default_value="hikrobot_side_DB2163749"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"top_camera_name", default_value="hikrobot_top_DB2163739"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"front_camera_info_url",
|
||||
default_value=str(info_root / "hikrobot_DB2163742.yaml"),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"side_camera_info_url",
|
||||
default_value=str(info_root / "hikrobot_DB2163749.yaml"),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"top_camera_info_url",
|
||||
default_value=str(info_root / "hikrobot_DB2163739.yaml"),
|
||||
),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
|
||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||
DeclareLaunchArgument(
|
||||
"index_roll_calibration_speed", default_value="5"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"index_flex_calibration_speed", default_value="10"
|
||||
),
|
||||
DeclareLaunchArgument("validation_enabled", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_extrinsics_file",
|
||||
default_value=str(
|
||||
Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("camera_extrinsics_file", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"source_urdf_path", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"corrected_urdf_output_dir", default_value=""
|
||||
"source_urdf_expected_sha256", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_extrinsics_expected_sha256", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config_expected_sha256", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config_expected_sha256", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument("vendor_sdk_config", default_value=""),
|
||||
DeclareLaunchArgument("vendor_sdk_python_package", default_value=""),
|
||||
DeclareLaunchArgument("sdk_package_expected_sha256", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"sdk_config_expected_sha256", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"profile_config_expected_sha256", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument("profile_config", default_value=""),
|
||||
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
||||
DeclareLaunchArgument("diagnostic_capture", default_value=""),
|
||||
DeclareLaunchArgument("start_cameras", default_value="true"),
|
||||
DeclareLaunchArgument("start_sdk", default_value="true"),
|
||||
DeclareLaunchArgument("record_bag", default_value="false"),
|
||||
@@ -409,17 +448,14 @@ def generate_launch_description() -> LaunchDescription:
|
||||
default_value=str(Path.cwd() / "calibration_output"),
|
||||
),
|
||||
DeclareLaunchArgument("session_dir", default_value=""),
|
||||
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "three_camera_calibration.yaml"
|
||||
),
|
||||
default_value="",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "three_camera_tags.yaml"
|
||||
),
|
||||
default_value="",
|
||||
),
|
||||
OpaqueFunction(function=_launch_stack),
|
||||
]
|
||||
+13
-3
@@ -26,7 +26,7 @@ def _launch(context):
|
||||
namespace = f"/g20_extrinsics/{view}/camera"
|
||||
cameras.append(
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=namespace,
|
||||
@@ -85,7 +85,7 @@ def _launch(context):
|
||||
output="screen",
|
||||
)
|
||||
solver = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="three_camera_extrinsics_node",
|
||||
name="g20_camera_extrinsics",
|
||||
output="screen",
|
||||
@@ -112,6 +112,12 @@ def _launch(context):
|
||||
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"
|
||||
@@ -138,7 +144,7 @@ def _launch(context):
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
)
|
||||
camera_info = Path.home() / ".ros" / "camera_info"
|
||||
return LaunchDescription(
|
||||
@@ -182,6 +188,10 @@ def generate_launch_description() -> LaunchDescription:
|
||||
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",
|
||||
@@ -0,0 +1,18 @@
|
||||
"""Stable launch name for the profile-driven calibration stack."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
implementation = Path(__file__).with_name("three_camera_calibration.launch.py")
|
||||
spec = importlib.util.spec_from_file_location(
|
||||
"linkerhand_unified_calibration_launch", implementation
|
||||
)
|
||||
if spec is None or spec.loader is None:
|
||||
raise RuntimeError("unified calibration launch implementation is missing")
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(module)
|
||||
return module.generate_launch_description()
|
||||
@@ -0,0 +1,5 @@
|
||||
"""Profile-driven LinkerHand calibration and validated URDF correction."""
|
||||
|
||||
from .core import CalibrationProfile, ProfileKey
|
||||
|
||||
__all__ = ["CalibrationProfile", "ProfileKey"]
|
||||
+38
-4
@@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .core import PAIR_NAMES, robust_rotation_summary
|
||||
from .core import robust_rotation_summary
|
||||
from .pnp import SquareTagPose
|
||||
|
||||
|
||||
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
|
||||
"t3_t4": ("t3", "t4"),
|
||||
"t4_t5": ("t4", "t5"),
|
||||
}
|
||||
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -108,6 +109,7 @@ class Observation:
|
||||
class StateSample:
|
||||
stamp_ns: int
|
||||
position_u8: tuple[float, ...]
|
||||
channel_stamps_ns: tuple[int, ...] = ()
|
||||
|
||||
|
||||
def interpolate_state_u8(
|
||||
@@ -116,7 +118,7 @@ def interpolate_state_u8(
|
||||
*,
|
||||
maximum_skew_ns: int,
|
||||
) -> tuple[tuple[float, ...], int] | None:
|
||||
"""Interpolate the 20-D hand state at an image timestamp.
|
||||
"""Interpolate a profile-sized hand state at an image timestamp.
|
||||
|
||||
The SDK publishes state independently from the camera. Continuous
|
||||
calibration must therefore use the image timestamp instead of whichever
|
||||
@@ -126,6 +128,29 @@ def interpolate_state_u8(
|
||||
raise ValueError("maximum_skew_ns must be non-negative")
|
||||
if not samples:
|
||||
return None
|
||||
if any(sample.channel_stamps_ns for sample in samples):
|
||||
# A multi-frame SDK does not observe all motors at the same instant.
|
||||
# De-duplicate each channel by its CAN receipt, then interpolate it
|
||||
# independently. Repeated publication never creates extra support.
|
||||
samples = tuple(sample for sample in samples if sample.stamp_ns >= stamp_ns-maximum_skew_ns
|
||||
and sample.channel_stamps_ns and min(sample.channel_stamps_ns) <= stamp_ns+maximum_skew_ns)
|
||||
if not samples:
|
||||
return None
|
||||
count = len(samples[0].position_u8)
|
||||
channels = [dict() for _ in range(count)]
|
||||
for sample in samples:
|
||||
if len(sample.position_u8) != count or len(sample.channel_stamps_ns) != count:
|
||||
return None
|
||||
for index, (stamp, value) in enumerate(zip(sample.channel_stamps_ns, sample.position_u8)):
|
||||
if stamp in channels[index] and channels[index][stamp] != value:
|
||||
return None
|
||||
channels[index][stamp] = value
|
||||
matches = [interpolate_state_u8(tuple(StateSample(stamp, (value,))
|
||||
for stamp, value in sorted(channel.items())), stamp_ns, maximum_skew_ns=maximum_skew_ns)
|
||||
for channel in channels]
|
||||
if any(match is None for match in matches):
|
||||
return None
|
||||
return tuple(match[0][0] for match in matches), max(match[1] for match in matches)
|
||||
stamps = [int(sample.stamp_ns) for sample in samples]
|
||||
index = bisect_left(stamps, int(stamp_ns))
|
||||
|
||||
@@ -150,7 +175,11 @@ def interpolate_state_u8(
|
||||
fraction = before_gap / denominator
|
||||
before_values = np.asarray(before.position_u8, dtype=float)
|
||||
after_values = np.asarray(after.position_u8, dtype=float)
|
||||
if before_values.shape != (20,) or after_values.shape != (20,):
|
||||
if (
|
||||
before_values.ndim != 1
|
||||
or before_values.size == 0
|
||||
or after_values.shape != before_values.shape
|
||||
):
|
||||
return None
|
||||
interpolated = before_values + fraction * (after_values - before_values)
|
||||
return (
|
||||
@@ -162,11 +191,16 @@ def interpolate_state_u8(
|
||||
if nearest is None:
|
||||
return None
|
||||
gap = abs(int(stamp_ns) - int(nearest.stamp_ns))
|
||||
if gap > maximum_skew_ns or len(nearest.position_u8) != 20:
|
||||
if gap > maximum_skew_ns or not nearest.position_u8:
|
||||
return None
|
||||
return (tuple(float(value) for value in nearest.position_u8), gap)
|
||||
|
||||
|
||||
# Physical-angle profiles use the same timestamp interpolation. Keep the old
|
||||
# public name for compatibility and offer a unit-neutral spelling to new code.
|
||||
interpolate_state = interpolate_state_u8
|
||||
|
||||
|
||||
class ContinuousSweepCollector:
|
||||
"""Collect timestamp-synchronised observations during one end-to-end move."""
|
||||
|
||||
@@ -0,0 +1,377 @@
|
||||
"""Map explicit SDK commands or feedback to corrected URDF joint coordinates.
|
||||
|
||||
Unified artifacts load their certified manifest and standard URDF. The old
|
||||
readers below are used only when a historical payload is explicitly supplied.
|
||||
|
||||
The static encoder-zero corrections in ``zero_angles`` are already baked into
|
||||
the corrected URDF joint origins. This bridge therefore publishes only the
|
||||
dynamic ``angle_rad`` values and never adds the static offsets a second time.
|
||||
|
||||
Schema-v5 trajectories are fitted against timestamp-synchronised hardware
|
||||
feedback, not controller set-points. They must therefore be queried with the
|
||||
SDK ``hand_state`` topic. The retained schema-v4 path is command-indexed for
|
||||
backwards compatibility only.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
G20_COMMAND_NAMES: tuple[str, ...] = (
|
||||
"thumb_cmc_pitch",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"reserved_11",
|
||||
"reserved_12",
|
||||
"reserved_13",
|
||||
"reserved_14",
|
||||
"thumb_mcp",
|
||||
"index_pip",
|
||||
"middle_pip",
|
||||
"ring_pip",
|
||||
"pinky_pip",
|
||||
)
|
||||
|
||||
# Match the stable ordering used by the existing MuJoCo bridge. JointState
|
||||
# consumers must use names, but retaining the ordering also keeps logs and
|
||||
# direct comparisons deterministic.
|
||||
G20_URDF_JOINT_NAMES: tuple[str, ...] = (
|
||||
"index_dip",
|
||||
"index_mcp_pitch",
|
||||
"index_mcp_roll",
|
||||
"index_pip",
|
||||
"middle_dip",
|
||||
"middle_mcp_pitch",
|
||||
"middle_mcp_roll",
|
||||
"middle_pip",
|
||||
"pinky_dip",
|
||||
"pinky_mcp_pitch",
|
||||
"pinky_mcp_roll",
|
||||
"pinky_pip",
|
||||
"ring_dip",
|
||||
"ring_mcp_pitch",
|
||||
"ring_mcp_roll",
|
||||
"ring_pip",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_ip",
|
||||
"thumb_mcp",
|
||||
)
|
||||
|
||||
|
||||
class CalibratedCommandMapper:
|
||||
"""Validated, profile-specific lookup from SDK u8 values to URDF radians."""
|
||||
|
||||
def __init__(
|
||||
self, payload: Mapping[str, Any], *, expected_side: str | None = None
|
||||
) -> None:
|
||||
from .full_hand import get_hand_calibration_profile, infer_compact_payload_layout, validate_compact_payload
|
||||
from .compat.legacy_diagnostic_tools.models import get_default_registry, validate_schema_v6_runtime_payload
|
||||
from .compat.legacy_diagnostic_tools.models.o12.artifacts import validate_o12_runtime_payload
|
||||
from .core import ProfileKey
|
||||
schema_version = int(payload["schema_version"])
|
||||
if schema_version == 7:
|
||||
validate_o12_runtime_payload(payload)
|
||||
elif schema_version == 6:
|
||||
validate_schema_v6_runtime_payload(payload)
|
||||
else:
|
||||
validate_compact_payload(payload)
|
||||
side = str(payload["side"]).lower()
|
||||
if expected_side is not None and side != str(expected_side).lower():
|
||||
raise ValueError(
|
||||
f"calibration side {side!r} does not match requested side "
|
||||
f"{str(expected_side).lower()!r}"
|
||||
)
|
||||
quality = payload["quality"]
|
||||
if quality.get("passed") is not True:
|
||||
raise ValueError("calibration quality.passed must be true")
|
||||
layout_id = (
|
||||
str(payload["layout_id"])
|
||||
if schema_version in {6, 7}
|
||||
else infer_compact_payload_layout(payload)
|
||||
)
|
||||
self.side = side
|
||||
self.layout_id = layout_id
|
||||
self.model = str(payload["model"]).upper()
|
||||
self.profile_id = str(
|
||||
payload.get("profile_id", f"G20/{side}/{layout_id}/v1")
|
||||
)
|
||||
self.serial_number = str(payload["serial_number"])
|
||||
self.input_domain = str(
|
||||
payload.get(
|
||||
"curve_input_domain",
|
||||
"command_u8" if schema_version == 4 else "",
|
||||
)
|
||||
)
|
||||
if self.input_domain not in {"command_u8", "feedback_u8", "feedback_rad"}:
|
||||
raise ValueError("calibration curve_input_domain is invalid")
|
||||
if schema_version in {6, 7}:
|
||||
self.command_names = tuple(str(value) for value in payload["command_names"])
|
||||
self.urdf_joint_names = tuple(str(name) for name in payload["joints"])
|
||||
self._motor_by_joint = {
|
||||
name: int(payload["joints"][name]["motor_index"])
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
registered = get_default_registry().get(
|
||||
ProfileKey.parse(self.profile_id)
|
||||
)
|
||||
self.feedback_name_aliases = dict(
|
||||
registered.profile.command.feedback_name_aliases
|
||||
)
|
||||
self.feedback_by_index = bool(
|
||||
registered.profile.command.feedback_by_index
|
||||
)
|
||||
else:
|
||||
profile = get_hand_calibration_profile(side, layout_id)
|
||||
self.command_names = G20_COMMAND_NAMES
|
||||
self.urdf_joint_names = G20_URDF_JOINT_NAMES
|
||||
self._motor_by_joint = {
|
||||
name: int(profile.joint_specs[name].motor_index)
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
self.feedback_name_aliases = {}
|
||||
self.feedback_by_index = False
|
||||
self._curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
self._decreasing_curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name].get(
|
||||
"decreasing_rad", payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
)
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
self._increasing_curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name].get(
|
||||
"increasing_rad", payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
)
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
self._previous_by_motor: dict[int, float] = {}
|
||||
self._direction_by_motor: dict[int, str] = {}
|
||||
self.direction_deadband_u8 = 0.002 if schema_version == 7 else 0.5
|
||||
self._knots = {
|
||||
name: tuple(float(value) for value in payload["joints"][name].get(
|
||||
"curve_input_knots_rad", ()
|
||||
))
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
self._raw_increasing_branch = {
|
||||
name: str(payload["joints"][name].get(
|
||||
"raw_increasing_curve_branch", "increasing"
|
||||
))
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def _command_index(value: float) -> int:
|
||||
command = float(value)
|
||||
if not math.isfinite(command):
|
||||
raise ValueError("calibrated command positions must be finite")
|
||||
return max(0, min(255, int(math.floor(command + 0.5))))
|
||||
|
||||
def map_positions(
|
||||
self, positions: Sequence[float], names: Sequence[str] = ()
|
||||
) -> tuple[float, ...]:
|
||||
values = tuple(float(value) for value in positions)
|
||||
if names and not self.feedback_by_index:
|
||||
if len(names) != len(values):
|
||||
raise ValueError(
|
||||
"JointState names and positions must have equal length"
|
||||
)
|
||||
if len(set(names)) != len(names):
|
||||
raise ValueError("JointState names must be unique")
|
||||
by_name = dict(zip((str(name) for name in names), values))
|
||||
for alias, canonical in self.feedback_name_aliases.items():
|
||||
if alias in by_name and canonical not in by_name:
|
||||
by_name[canonical] = by_name[alias]
|
||||
missing = [name for name in self.command_names if name not in by_name]
|
||||
if missing:
|
||||
raise ValueError(
|
||||
f"{self.model} feedback is missing named channels: "
|
||||
+ ",".join(missing)
|
||||
)
|
||||
command = tuple(by_name[name] for name in self.command_names)
|
||||
else:
|
||||
if len(values) != len(self.command_names):
|
||||
raise ValueError(
|
||||
f"unnamed {self.model} feedback must contain exactly "
|
||||
f"{len(self.command_names)} positions"
|
||||
)
|
||||
command = values
|
||||
indices = (
|
||||
() if self.input_domain == "feedback_rad"
|
||||
else tuple(self._command_index(value) for value in command)
|
||||
)
|
||||
direction_by_motor: dict[int, str | None] = {}
|
||||
for motor, value in enumerate(command):
|
||||
previous = self._previous_by_motor.get(motor)
|
||||
direction = self._direction_by_motor.get(motor)
|
||||
if previous is not None:
|
||||
if value > previous + self.direction_deadband_u8:
|
||||
direction = "increasing"
|
||||
elif value < previous - self.direction_deadband_u8:
|
||||
direction = "decreasing"
|
||||
direction_by_motor[motor] = direction
|
||||
result: list[float] = []
|
||||
for name in self.urdf_joint_names:
|
||||
motor = self._motor_by_joint[name]
|
||||
direction = direction_by_motor[motor]
|
||||
if self.input_domain == "feedback_rad" and direction is not None:
|
||||
raw_increasing = self._raw_increasing_branch[name]
|
||||
direction = (
|
||||
raw_increasing
|
||||
if direction == "increasing"
|
||||
else "increasing" if raw_increasing == "decreasing" else "decreasing"
|
||||
)
|
||||
curves = (
|
||||
self._increasing_curves
|
||||
if direction == "increasing"
|
||||
else self._decreasing_curves
|
||||
if direction == "decreasing"
|
||||
else self._curves
|
||||
)
|
||||
if self.input_domain == "feedback_rad":
|
||||
result.append(float(np.interp(command[motor], self._knots[name], curves[name])))
|
||||
else:
|
||||
result.append(curves[name][indices[motor]])
|
||||
for motor, value in enumerate(command):
|
||||
self._previous_by_motor[motor] = value
|
||||
direction = direction_by_motor[motor]
|
||||
if direction is not None:
|
||||
self._direction_by_motor[motor] = direction
|
||||
return tuple(result)
|
||||
|
||||
|
||||
def load_calibrated_command_mapper(
|
||||
calibration_file: str | Path, *, expected_side: str | None = None, input_kind="command"
|
||||
) -> CalibratedCommandMapper:
|
||||
path = Path(calibration_file).expanduser().resolve()
|
||||
if not path.is_file():
|
||||
raise ValueError(f"calibration JSON does not exist: {path}")
|
||||
payload = json.loads(path.read_text(encoding="utf-8"))
|
||||
if path.name == "release_manifest.json" or payload.get("format") in {"unified_calibration_v1", "unified_calibration_v2", "unified_calibration_v3"}:
|
||||
from .runtime.artifacts.reader import load_unified_mapper
|
||||
return load_unified_mapper(path, expected_side=expected_side, input_kind=input_kind)
|
||||
return CalibratedCommandMapper(payload, expected_side=expected_side)
|
||||
|
||||
|
||||
def default_input_topic(
|
||||
hand_type: str, input_domain: str, model: str = "G20"
|
||||
) -> str:
|
||||
side = str(hand_type).lower()
|
||||
if side not in {"left", "right"}:
|
||||
raise ValueError("hand_type must be left or right")
|
||||
if input_domain == "feedback_u8":
|
||||
return f"/{str(model).lower()}/cb_{side}_hand_state"
|
||||
if input_domain == "command_u8":
|
||||
return f"/{str(model).lower()}/cb_{side}_hand_control_cmd"
|
||||
if input_domain == "feedback_rad":
|
||||
return f"/{str(model).lower()}/{side}/joint_states"
|
||||
if input_domain == "command_rad":
|
||||
return f"/{str(model).lower()}/{side}/joint_cmd"
|
||||
raise ValueError("calibration curve_input_domain is invalid")
|
||||
|
||||
|
||||
class CalibratedJointStateBridge(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("calibrated_joint_state_bridge")
|
||||
self.declare_parameter("hand_type", "right")
|
||||
self.declare_parameter("calibration_file", "")
|
||||
self.declare_parameter("input_topic", "")
|
||||
self.declare_parameter("output_topic", "")
|
||||
self.declare_parameter("input_kind", "command")
|
||||
|
||||
hand_type = str(self.get_parameter("hand_type").value).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise ValueError("hand_type must be left or right")
|
||||
calibration_file = str(self.get_parameter("calibration_file").value)
|
||||
if not calibration_file:
|
||||
raise ValueError("calibration_file is required")
|
||||
self.mapper = load_calibrated_command_mapper(
|
||||
calibration_file, expected_side=hand_type, input_kind=str(self.get_parameter("input_kind").value)
|
||||
)
|
||||
input_topic = str(self.get_parameter("input_topic").value).strip()
|
||||
output_topic = str(self.get_parameter("output_topic").value).strip()
|
||||
self.input_topic = input_topic or default_input_topic(
|
||||
hand_type, self.mapper.input_domain, self.mapper.model
|
||||
)
|
||||
self.output_topic = (
|
||||
output_topic
|
||||
or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state"
|
||||
)
|
||||
if self.output_topic in {self.input_topic, default_input_topic(hand_type,
|
||||
"feedback_rad" if self.mapper.input_domain.endswith("rad") else "feedback_u8", self.mapper.model)}:
|
||||
raise ValueError("mapped output must not overwrite the real SDK command/feedback topic")
|
||||
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
|
||||
self.subscription = self.create_subscription(
|
||||
JointState, self.input_topic, self._command_callback, 10
|
||||
)
|
||||
self._last_error = ""
|
||||
self.get_logger().info(
|
||||
f"loaded {self.mapper.profile_id} calibration for "
|
||||
f"{self.mapper.serial_number}: "
|
||||
f"{self.input_topic} ({self.mapper.input_domain}) -> "
|
||||
f"{self.output_topic}"
|
||||
)
|
||||
|
||||
def _command_callback(self, command: JointState) -> None:
|
||||
try:
|
||||
positions = self.mapper.map_positions(command.position, command.name)
|
||||
except ValueError as error:
|
||||
message = str(error)
|
||||
if message != self._last_error:
|
||||
self.get_logger().error(message)
|
||||
self._last_error = message
|
||||
return
|
||||
self._last_error = ""
|
||||
result = JointState()
|
||||
result.header = command.header
|
||||
result.name = list(self.mapper.urdf_joint_names)
|
||||
result.position = list(positions)
|
||||
self.publisher.publish(result)
|
||||
|
||||
|
||||
def main(args: Sequence[str] | None = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node: CalibratedJointStateBridge | None = None
|
||||
try:
|
||||
node = CalibratedJointStateBridge()
|
||||
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,182 @@
|
||||
"""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"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ClockLatch:
|
||||
device_tick: int
|
||||
before_ns: int
|
||||
after_ns: int
|
||||
|
||||
@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
|
||||
|
||||
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+1_000_000
|
||||
if elapsed < 0 or abs(elapsed-host_elapsed) > tolerance:
|
||||
raise ValueError("camera/host clock discontinuity or counter unit mismatch")
|
||||
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=""):
|
||||
self.camera, self.mvs, self.clock_ns = camera, mvs, clock_ns
|
||||
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):
|
||||
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")
|
||||
return ClockLatch(tick, before, self.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
|
||||
self.frame_clock.update(valid)
|
||||
self._write(dict(kind="camera_clock_sync", policy=CAMERA_TIMING_POLICY,
|
||||
ticks_per_second=self.frame_clock.ticks_per_second,
|
||||
latches=[asdict(x) for x in latches], anchor=asdict(self.frame_clock.anchor)))
|
||||
if self.journal is not None:
|
||||
self.journal.flush()
|
||||
self.next_refresh = time.monotonic()+.5
|
||||
|
||||
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,21 @@
|
||||
"""Compatibility adapters for one-release calibration migrations."""
|
||||
|
||||
from .config_v1 import (
|
||||
legacy_default_profile_key,
|
||||
product_profile_key,
|
||||
resolve_legacy_profile_alias,
|
||||
)
|
||||
from .defaults import (
|
||||
default_product_config_path,
|
||||
default_three_camera_config_path,
|
||||
)
|
||||
from .paths import resolve_renamed_package_path
|
||||
|
||||
__all__ = [
|
||||
"default_product_config_path",
|
||||
"default_three_camera_config_path",
|
||||
"legacy_default_profile_key",
|
||||
"product_profile_key",
|
||||
"resolve_legacy_profile_alias",
|
||||
"resolve_renamed_package_path",
|
||||
]
|
||||
@@ -0,0 +1,47 @@
|
||||
"""Identity migration for deployed product configuration schemas."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Mapping
|
||||
|
||||
from ..core import ProfileKey
|
||||
|
||||
|
||||
def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
|
||||
version = int(raw.get("schema_version", -1))
|
||||
if version in {2, 3}:
|
||||
key = ProfileKey.parse(str(raw.get("profile_id", "")))
|
||||
for field, actual in (
|
||||
("model", key.model),
|
||||
("side", key.side),
|
||||
("tag_layout", key.layout),
|
||||
):
|
||||
configured = str(raw.get(field, "")).strip()
|
||||
if configured and configured.lower() != actual.lower():
|
||||
raise ValueError(f"{field} differs from profile_id")
|
||||
return key
|
||||
if version != 1:
|
||||
raise ValueError("product config schema_version must be 1, 2 or 3")
|
||||
model = str(raw.get("model", "")).strip().upper()
|
||||
side = str(raw.get("side", "")).strip().lower()
|
||||
layout = str(raw.get("tag_layout", "")).strip().lower()
|
||||
if not layout and (model, side) == ("G20", "right"):
|
||||
layout = "g20_right_19"
|
||||
return ProfileKey(model, side, layout, 1)
|
||||
|
||||
|
||||
def legacy_default_profile_key() -> ProfileKey:
|
||||
"""Preserve the former no-argument executable for one release."""
|
||||
return ProfileKey("G20", "right", "g20_right_19", 1)
|
||||
|
||||
|
||||
def resolve_legacy_profile_alias(key: ProfileKey) -> ProfileKey:
|
||||
"""Map retired layout identifiers to their reviewed physical profile."""
|
||||
if (
|
||||
key.model == "G20"
|
||||
and key.side == "right"
|
||||
and key.layout == "g20_right_15"
|
||||
and key.revision == 1
|
||||
):
|
||||
return ProfileKey("G20", "right", "g20_right_19", 1)
|
||||
return key
|
||||
@@ -0,0 +1,16 @@
|
||||
"""One-release default selection for invocations without ``--config``."""
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def default_product_config_path() -> Path:
|
||||
share = Path(get_package_share_directory("linkerhand_calibration"))
|
||||
return share / "config/g20_right_product.yaml"
|
||||
|
||||
|
||||
def default_three_camera_config_path() -> Path:
|
||||
"""Resolve the installed calibration defaults through the ROS index."""
|
||||
share = Path(get_package_share_directory("linkerhand_calibration"))
|
||||
return share / "config/three_camera_calibration.yaml"
|
||||
@@ -0,0 +1,4 @@
|
||||
"""Legacy single-camera algorithms retained for one compatibility release."""
|
||||
from .session_v1 import uses_coupled_full_hand_zero_solver
|
||||
|
||||
__all__ = ["uses_coupled_full_hand_zero_solver"]
|
||||
@@ -0,0 +1,24 @@
|
||||
"""Version selection for replaying durable pre-v3 hardware sessions."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
def uses_coupled_full_hand_zero_solver(
|
||||
session_start: Mapping[str, Any],
|
||||
) -> bool:
|
||||
"""Return the solver contract recorded by the legacy session header.
|
||||
|
||||
Capabilities are not consulted by the live runtime. This adapter reads
|
||||
the durable v1 header only so offline replay can reproduce an artifact
|
||||
created before the independent thumb solver was introduced.
|
||||
"""
|
||||
capabilities = {
|
||||
str(value) for value in session_start.get("capabilities", ())
|
||||
}
|
||||
return (
|
||||
int(session_start.get("sample_schema_version", 1)) == 1
|
||||
and "palm_axis_side_channel_v2" in capabilities
|
||||
and "palm_axis_relative_motion_v3" not in capabilities
|
||||
)
|
||||
+14
@@ -0,0 +1,14 @@
|
||||
# 历史兼容区
|
||||
|
||||
此目录保留旧格式读写、旧布局、历史数据诊断及必要的离线工具，不参与正式在线调度。
|
||||
四型号的 runner/node/pipeline 已由 `runtime/runner.py`、`runtime/session.py` 和
|
||||
`runtime/artifacts/finalization.py` 替代。只保留仍被历史工具调用的入口；
|
||||
L6/O6 无调用的 pipeline 包装已删除。
|
||||
|
||||
旧独立断点实现、低速预检和在线节点已移除。旧发布函数已拒绝更新正式发布指针。
|
||||
这里生成的离线诊断文件不能作为标准 URDF 已验收的证据；正式回放使用
|
||||
`calibrate_hand --config <product.yaml> --offline-raw <raw_samples.jsonl>`。
|
||||
|
||||
不要在此目录增加新型号。新型号提供 Profile、产品 YAML、原始 CAD/mesh；仅新 SDK 协议增加 Adapter。
|
||||
需要恢复已删除的历史实现时，使用工作区
|
||||
`calibration_output/refactor_backup.4WRWNn/` 中的归档，不要重新接入生产入口。
|
||||
+1
@@ -0,0 +1 @@
|
||||
"""Read-only compatibility for archived sessions and diagnostic tools."""
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
"""Model- and side-specific calibration policies."""
|
||||
|
||||
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import (
|
||||
EngineBindings,
|
||||
ProfileRegistry,
|
||||
RegisteredProfile,
|
||||
get_default_registry,
|
||||
)
|
||||
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.runtime_schema import validate_schema_v6_runtime_payload
|
||||
|
||||
__all__ = [
|
||||
"EngineBindings",
|
||||
"ProfileRegistry",
|
||||
"RegisteredProfile",
|
||||
"get_default_registry",
|
||||
"validate_schema_v6_runtime_payload",
|
||||
]
|
||||
+15
@@ -0,0 +1,15 @@
|
||||
"""Registered profiles for this hand family."""
|
||||
|
||||
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import ProfileRegistry
|
||||
|
||||
|
||||
def register_profiles(registry: ProfileRegistry) -> None:
|
||||
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.legacy_11 import build_left_profile, build_right_profile
|
||||
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.right_19 import build_profile
|
||||
|
||||
registry.register(build_profile())
|
||||
registry.register(build_left_profile())
|
||||
registry.register(build_right_profile())
|
||||
|
||||
|
||||
__all__ = ["register_profiles"]
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user