diff --git a/.gitignore b/.gitignore index 11b5c74..3d58267 100644 --- a/.gitignore +++ b/.gitignore @@ -62,7 +62,7 @@ Thumbs.db # src/linkerhand_retarget/resource/linkerforce_v2/profiles/. /profiles/ /calibration_output/ -/config/g20_three_camera_extrinsics.yaml +/config/*_three_camera_extrinsics.yaml *.wear_check.json *.checkpoint.json *.verification.json diff --git a/docs/G20三相机零位标定与URDF修正操作说明.md b/docs/G20三相机零位标定与URDF修正操作说明.md new file mode 100644 index 0000000..4296e07 --- /dev/null +++ b/docs/G20三相机零位标定与URDF修正操作说明.md @@ -0,0 +1,669 @@ +# G20 三相机零位标定与 URDF 修正操作说明 + +> 适用工程：`linkerhand_retarget_ros2` +> 适用标定包：`g20_thumb_apriltag_calibration` +> 文档基线：2026-08-11 当前 V3 轴坐标系逻辑 +> 适用对象：G20 左手和右手 + +## 1. 文档目的 + +本文说明当前代码中 G20 三相机、11 个 AprilTag 的完整标定逻辑及现场操作流程，包含： + +- 20 通道 u8 命令到 21 个 URDF 关节动态角度曲线的标定； +- 拇指 CMC roll/yaw/pitch 的静态 URDF 零位求解； +- 四指动态曲线继承和静态 CAD 零位保护； +- 第三轮留出验证、自动重采、暂停和恢复策略； +- 从原始 CAD URDF 生成修正 URDF； +- 使用已有 `raw_samples.jsonl` 进行无运动离线重放； +- 修正 URDF 和配套 JSON 的仿真使用方法。 + +本文中的右手 `G20_RIGHT_001/20260811_120146` 数值仅用于说明当前算法的实际结果，**不是代码中写死的标定角度，也不是其他机械手的目标值**。 + +## 2. 标定的最终产物 + +一次完整标定同时生成两类互相配套的产物： + +1. 精简标定 JSON：保存每个关节按命令 `0～255` 索引的 256 点 `angle_rad` 动态曲线，以及 URDF 静态零偏和质量指标。 +2. 修正 URDF：只把通过验证的静态零偏写入原始 CAD URDF 的主动关节 `origin.rpy`。 + +运行时必须遵守以下规则： + +- 修正 URDF 已经包含静态 `urdf_zero_offset_rad`，运行时不能再加一次； +- 动态关节位置必须使用同一机械手、同一侧、同一次标定 JSON 中的 `angle_rad`； +- 左手曲线不能用于右手，右手曲线不能用于左手； +- 不能用已经修正过的 URDF 作为下一次标定的源文件。 + +## 3. 硬件和坐标配置 + +### 3.1 三台相机 + +默认机位和序列号如下： + +| 机位 | 默认序列号 | 默认作用 | +|---|---|---| +| 正面 `front` | `DB2163742` | 拇指 CMC pitch/roll、MCP/IP、参考指 MCP roll | +| 侧面 `side` | `DB2163749` | 参考指 MCP pitch、PIP/DIP | +| 上面 `top` | `DB2163739` | 拇指 CMC yaw | + +默认采集参数： + +- 分辨率和像素格式由海康节点配置； +- 帧率：`30 Hz`； +- 曝光：`5000 us`； +- 增益：`0 dB`； +- 自动曝光：关闭； +- AprilTag 检测降采样：`decimate=1.5`； +- 启动文件固定使用 `rmw_fastrtps_cpp` 和 64 MB Fast DDS 共享内存配置。 + +每台相机必须有对应当前镜头、焦距、分辨率的独立内参文件： + +```text +~/.ros/camera_info/hikrobot_DB2163742.yaml +~/.ros/camera_info/hikrobot_DB2163749.yaml +~/.ros/camera_info/hikrobot_DB2163739.yaml +``` + +三相机外参默认文件： + +```text +/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml +``` + +移动相机、改变镜头焦距/对焦、改变分辨率或重新标定内参后，必须重新标定外参。 + +### 3.2 11 个 AprilTag + +使用 `tag36h11`，有效黑框边长配置为 `16 mm`，不包含外围白边。 + +| Tag ID | 机位 | 固定位置或运动件 | +|---:|---|---| +| 0 | 正面 | 掌壳固定基准 | +| 1 | 正面 | 拇指 CMC 后连杆 | +| 2 | 正面 | 拇指 MCP 后连杆 | +| 3 | 正面 | 拇指 IP 后末节 | +| 4 | 侧面 | 掌壳侧面固定基准 | +| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 | +| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 | +| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 | +| 8 | 上面 | 掌壳或底座固定基准 | +| 9 | 上面 | 拇指 CMC yaw 运动件 | +| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 | + +Tag 必须固定在刚性件上，不能跨关节、贴在软胶上、在运动中翘起或移动。Tag 的平面内旋转不要求贴正，但整个标定会话中安装姿态必须保持不变。 + +## 4. 左右手配置差异 + +左右手使用同一套拟合、留出验证和 URDF 写入算法，但使用独立的参考指、电机和镜像避挡姿态。 + +| 项目 | 左手 | 右手 | +|---|---|---| +| 四指参考源 | 食指 `index` | 小指 `pinky` | +| 扫描电机顺序 | `0/5/15/6/1/16/10` | `0/5/15/9/4/19/10` | +| 参考指 MCP roll | 电机 6 | 电机 9 | +| 参考指 MCP pitch | 电机 1 | 电机 4 | +| 参考指 PIP | 电机 16 | 电机 19 | +| 参考指 roll 避挡 | 其他侧摆电机置 0 | 其他侧摆电机置 255 | +| 电机 0 辅助姿态 | 使用基准姿态 | 电机 5、10 固定 255 | + +两侧的非零拇指静态零偏都必须由各自当前会话的数据计算，不共享任何标定角度。 + +## 5. 扫描任务和运动策略 + +### 5.1 固定基准命令 + +调用 `/g20_calibration/start` 后，程序先下发并确认以下 20 通道姿态： + +```text +[255, 255, 255, 255, 255, 255, 127, 127, 127, 127, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] +``` + +普通关节以命令 `255` 为动态曲线零点；四指 MCP 侧摆以命令 `127` 为动态曲线零点。 + +### 5.2 七个扫描任务 + +每个任务执行 3 轮 `255→0→255`，即每个任务 6 个方向。总计： + +```text +7 个任务 × 3 轮 × 2 个方向 = 42 个扫描方向 +``` + +| 顺序 | 任务 | 电机 | 机位 | 同时拟合 | +|---:|---|---:|---|---| +| 1 | 拇指 CMC pitch | 0 | 正面 | `thumb_cmc_pitch` | +| 2 | 拇指 CMC roll | 5 | 正面 | `thumb_cmc_roll` | +| 3 | 拇指 MCP | 15 | 正面 | `thumb_mcp`、被动 `thumb_ip` | +| 4 | 参考指 MCP roll | 左6/右9 | 正面 | 参考指 `mcp_roll` | +| 5 | 参考指 MCP pitch | 左1/右4 | 侧面 | 参考指 `mcp_pitch` | +| 6 | 参考指 PIP | 左16/右19 | 侧面 | 参考指 `pip`、被动 `dip` | +| 7 | 拇指 CMC yaw | 10 | 上面 | `thumb_cmc_yaw` | + +特殊辅助姿态： + +- 扫描拇指 yaw 时，电机 5 固定为 `145`，避免 Tag 9 姿态过斜； +- 右手扫描电机 0 时，电机 5 和 10 固定为 `255`； +- 扫描参考指 MCP roll 时，其余三指侧摆移到对应左右手避挡端； +- 当前方向重试和人工恢复时，辅助姿态保持一致。 + +### 5.3 标定速度 + +G20 SDK 使用五指速度数组： + +- 常规速度：`15`； +- 参考指 MCP roll：参考指速度 `5`； +- 参考指 MCP pitch/PIP：参考指速度 `10`； +- 自动重试速度比例：`80% / 60% / 50%`，最低速度不低于 `3`。 + +## 6. 轨迹和关节轴拟合 + +### 6.1 时间戳配对 + +每帧 Tag 图像与 20 通道机械手状态按时间戳配对，默认最大允许偏差为 `50 ms`。标定曲线按实际命令分箱，但已确认的固件端点饱和反馈可以归入对应的命令端点分箱。 + +每个扫描方向至少需要： + +- 40 帧同步有效数据； +- 覆盖至少 240 个 u8； +- 至少 32 个有效整数分箱； +- 相邻有效分箱最大间隔不超过 16； +- 同时包含命令 0 和 255 端点。 + +### 6.2 动态角度曲线 + +程序使用父/子 Tag 的完整相对旋转轨迹拟合关节转角，分别拟合下降和上升方向，检查单调修正、正反程回差和三轮行程一致性，再生成按命令索引的 256 点 `angle_rad`。 + +四指策略： + +- 左手实测食指动态曲线，继承给中指、无名指、小指； +- 右手实测小指动态曲线，继承给食指、中指、无名指； +- 继承仅用于动态命令—角度关系； +- 不把参考指的静态装配偏差复制给其他独立电机。 + +### 6.3 三维轴方向和轴线位置 + +当前算法不直接用单帧平面 Tag PnP 姿态作为关节角： + +- 轴方向主要来自整段相对旋转的螺旋轴； +- 斜视且三维运动平面可观的关节，保留旋转轴和中心圆轴的交叉检查； +- 接近端视的关节使用相对姿态轴约束圆轨迹方向； +- 轴线上一点由整段相对 SE(3) 的 `(I-R)p=t` 方程拟合； +- 接近沿轴观察时，丢弃单目无法稳定确定的光轴深度，只使用图像平面内可观分量。 + +### 6.4 IPPE 双分支处理 + +每轮运动前在静止端点联合 8 帧选择整组最稳定的平面 Tag PnP 分支。侧面 Tag 4/5/6/7 还检查贴面法向一致性，避免选择低重投影误差但几何镜像的分支。 + +## 7. 当前 V3 静态零位求解逻辑 + +### 7.1 为什么不能用两条根轴线间距确定掌部旋转 + +拇指 CMC roll 根轴和四指 MCP roll 根轴在 CAD 中近似平行。旧逻辑使用两条三维轴线的空间间距确定掌坐标系绕根轴的旋转，但单目 PnP 的固定深度偏差会改变这条间距方向，并被误算成稳定的拇指 roll 静态零偏。 + +这种误差可以三轮高度重复，因此“重复性好”并不能证明绝对零位正确。 + +### 7.2 V3 掌坐标系锚定 + +当前 V3 使用： + +1. 行程更充分的根轴实测方向； +2. 保持原始 CAD 直立的参考指 MCP pitch 实测轴方向； +3. 两条根轴线位置只用于平移，不参与绕根轴旋转。 + +左手使用食指 MCP pitch，右手使用小指 MCP pitch。该逻辑全部由当前会话轨迹计算，不包含按左右手或序列号写死的拇指角度。 + +### 7.3 拇指零位依赖链 + +拇指静态零位按可观测链逐级求解： + +- `thumb_cmc_yaw` 实测轴方向观测 `thumb_cmc_roll` 零位； +- `thumb_cmc_pitch` 实测轴方向观测 `thumb_cmc_yaw` 零位； +- `thumb_mcp` 实测轴线相位观测 `thumb_cmc_pitch` 零位； +- 被动 `thumb_ip` 的浅圆弧只作为诊断，不能覆盖 `thumb_mcp` 的原始 CAD 静态零位。 + +逐关节一维鲁棒求解可防止远端异常把已经确定的上游零位一起拖到边界。 + +### 7.4 四指静态零位保护 + +当前 11-Tag 布局只能直接观测一根参考指，不能证明四根独立电机具有相同绝对装配相位。因此： + +- 四指 MCP roll 静态修正固定为原始 CAD 0； +- 四指 MCP pitch 静态修正固定为原始 CAD 0； +- 四指 PIP 静态修正固定为原始 CAD 0； +- `thumb_mcp` 静态修正固定为原始 CAD 0； +- 这些关节的动态 256 点曲线仍然实测或继承。 + +这里的“0”表示不修改原始 CAD `origin.rpy`，不是额外写入某台机械手的人工标定角度。 + +## 8. 质量门限和留出验证 + +### 8.1 预检门限 + +| 指标 | 默认要求 | +|---|---:| +| 预检窗口 | 60 帧 | +| 所需 Tag 同时有效率 | ≥95% | +| 检测频率 | ≥15 Hz，正常应接近30 Hz | +| Hamming | 0 | +| Decision margin | ≥30 | +| Tag 最小边长 | ≥30 px | +| PnP 重投影 RMS | ≤1.5 px | +| 图像—状态时间差 | ≤50 ms | + +### 8.2 轨迹和轴门限 + +| 指标 | 默认要求 | +|---|---:| +| 主动关节旋转轴外 RMS | ≤2.5° | +| 被动关节旋转轴外 RMS | ≤7.5° | +| 三轮轴方向极差 | ≤0.75° | +| 轴线径向 RMS | ≤3 mm | +| SE(3) 轴线拟合 RMS | ≤1 mm | +| 可观三维圆的姿态轴/圆轴夹角 | ≤1° | +| 父子轴锥角几何不一致 | ≤5° | +| 主动曲线三轮行程差 | ≤3° | +| 被动曲线三轮行程差 | ≤10° | +| 主动最大单调修正 | ≤2° | +| 主动最大回差 | ≤5° | +| 被动最大单调修正 | ≤3° | +| 被动最大回差 | ≤7.5° | + +拇指可求解静态偏移默认安全范围为 `±20°`。四指静态偏移不由相机相位覆盖，保持原始 CAD。 + +### 8.3 第三轮强制留出 + +前两轮用于训练，第三轮必须作为独立留出验证： + +- 轨迹角度 MAE ≤1°； +- 轨迹角度 P95 ≤2°； +- 非零静态修正必须在第三轮优于原始 URDF； +- 改善必须通过按三轮分组的 95% bootstrap 置信检查； +- 最终再使用三轮全部数据重拟合正式结果。 + +`validation_enabled:=false` 只关闭额外随机机械动作，不能关闭第三轮留出验证。 + +## 9. 运动安全和端点处理 + +默认端点容差为 `±2 u8`，只有经过实机确认的固件饱和端点使用专用容差： + +| 端点 | 专用容差 | +|---|---:| +| 拇指 yaw 电机10，命令0 | ±4 u8 | +| 右手拇指 yaw 电机10，命令255 | ±5 u8，实机可能反馈250 | +| 右手小指 PIP 电机19，命令0 | ±5 u8，实机可能反馈5 | + +运动保护： + +- 单方向扫描超时：90 秒； +- 连续 8 秒没有至少 1 u8 的目标方向进展：立即暂停并保持当前位置； +- 机械停滞不消耗遮挡/采样自动重试预算； +- 发现摩擦、碰撞或仍在变化的反馈时，不要反复调用 `resume` 强推； +- 只有确认是稳定固件端点时，才允许为该电机、该端点配置专用容差。 + +## 10. 标定前准备 + +### 10.1 构建和加载环境 + +```bash +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 +``` + +每次修改代码并重新构建后，必须关闭旧标定进程，在新终端重新 `source install/setup.bash` 后启动。 + +### 10.2 现场检查 + +开始前确认： + +- `can0` 已启动； +- MVS 客户端没有占用三台相机； +- 三个内参文件和外参文件对应当前相机安装； +- 11 个 Tag 固定、平整、全行程可见； +- 机械手全行程没有碰撞； +- 没有其他节点向同一只手发布位置命令； +- 准备好随时断开电机电源； +- 源 URDF 是原始 CAD 文件。 + +## 11. 禁止运动预检 + +### 11.1 右手预检 + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.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 \ + source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf \ + can_interface:=can0 \ + commands_enabled:=false +``` + +### 11.2 左手预检 + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.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 \ + commands_enabled:=false +``` + +`commands_enabled:=false` 时不允许标定位置运动。完成检查后应关闭预检进程，再启动正式流程，避免相机和 SDK 被两个进程同时占用。 + +## 12. 正式标定操作 + +### 12.1 右手正式启动 + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.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 \ + source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf \ + can_interface:=can0 +``` + +### 12.2 左手正式启动 + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.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 +``` + +### 12.3 查看状态 + +另开一个终端： + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.bash + +ros2 topic echo --once \ + /g20_calibration/status_text \ + --field data +``` + +需要连续监控时去掉 `--once`。 + +查看三个校正画面： + +```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 +``` + +### 12.4 开始运动 + +只有状态进入“等待开始”，三个机位均显示“就绪”、外参匹配、所需 Tag 无缺失、SDK 正常后，才调用一次： + +```bash +ros2 service call \ + /g20_calibration/start \ + std_srvs/srv/Trigger {} +``` + +不要重复调用 `start`。程序会自动完成基准姿态、42 个扫描方向、拟合、第三轮留出验证、正式 JSON 和 URDF 写入。 + +## 13. 暂停、恢复和终止 + +### 13.1 手工暂停 + +```bash +ros2 service call \ + /g20_calibration/pause \ + std_srvs/srv/Trigger {} +``` + +暂停会保持当前实际位置，不会自动返回基准。 + +### 13.2 恢复可恢复故障 + +```bash +ros2 service call \ + /g20_calibration/resume \ + std_srvs/srv/Trigger {} +``` + +恢复规则： + +- 只要求当前活动机位恢复就绪； +- 当前失败方向会从起点完整重扫； +- 关节拟合失败会清除当前失败关节数据并重扫该关节的 6 个方向； +- 已经通过的其他关节数据保留； +- 不要用 `start` 代替 `resume`。 + +### 13.3 终止 + +```bash +ros2 service call \ + /g20_calibration/abort \ + std_srvs/srv/Trigger {} +``` + +终止会停止任务并保持当前位置，不主动移动机械手。 + +## 14. 自动重试和失败分类 + +| 失败类型 | 程序行为 | 操作建议 | +|---|---|---| +| 短时 Tag 丢失、同步中断、端点/分箱不足 | 自动重扫当前方向，最多3次 | 修正遮挡后必要时 `resume` | +| 单轮/单关节轨迹拟合失败 | 优先重扫失败轮次；最多自动重采2轮 | 修正可见性或机械行程后 `resume` | +| 电机8秒无进展 | 立即保持并暂停，不消耗采样重试 | 先排查机械问题或确认固件端点 | +| 三轮轴方向或零位离散过大 | 当前关节结束后暂停 | 修正问题后 `resume` 重扫该关节 | +| 零位触边、父子轴几何不一致、留出无改善 | 稳定模型失败，不自动重扫 | `resume` 被拒绝；修正根因后启动新会话 | +| 操作员暂停 | 保持当前位置 | 确认安全后 `resume` | + +自动重试只改变采集速度和保持时间，不放宽最终质量门限。 + +## 15. 常见状态问题 + +### 15.1 预检只有约 3 Hz + +优先检查： + +- 是否通过正式 launch 启动，从而加载 Fast DDS 大图共享内存配置； +- 是否还有旧相机或 MVS 客户端占用设备； +- `camera_info` 和 `image_raw/image_rect` 是否都接近 30 Hz； +- 是否重新 `source install/setup.bash`； +- 是否存在多个图像查看或录制进程造成额外负载。 + +不要通过降低 Tag 有效率门限绕过帧率问题。 + +### 15.2 显示“缺失Tag=无”，但同时有效率不足 + +“当前缺失”只表示最新帧；同时有效率是预检滑动窗口内所有必需 Tag 同帧有效的比例。等待窗口更新，或排查间歇性遮挡、反光、角点质量和 PnP 分支失败。 + +### 15.3 电机目标255、反馈稳定250 + +当前代码只对右手电机10的255端配置 `±5 u8`。其他电机不能因为一次卡滞而放宽。先确认反馈确实稳定在固件端点，且不存在摩擦或碰撞。 + +### 15.4 被动 DIP 轴外残差过大 + +被动关节允许更宽的单轴残差，但仍必须满足跨轮轴方向和第三轮留出。若 Tag、外参均正常，需检查耦合机构是否存在非理想运动、松动或采样过程中 PnP 分支变化；不要直接放宽最终门限。 + +### 15.5 零位/URDF模型验证失败 + +该失败表示当前稳定观测无法由“原始 CAD + 纯关节零位旋转”解释。重复运动通常不会修复，程序会拒绝 `resume`。应检查原始 URDF、Tag所在刚性件、相机外参身份和标定模型后启动新会话。 + +## 16. 输出目录和文件 + +默认会话目录： + +```text +calibration_output/<序列号>// +``` + +主要文件： + +```text +raw_samples.jsonl +g20__<序列号>_calibration.json +``` + +修正 URDF 默认写入原始 URDF 所在目录： + +```text +linkerhand_g20__zero_calibrated_<序列号>_<时间戳>.urdf +``` + +写入约束： + +- 每次都从原始 CAD URDF 生成； +- 采用 `T_original × Rot(axis, offset)`； +- 只修改通过验收的主动关节 `origin.rpy`； +- 不修改 `origin.xyz`、`axis.xyz`、mesh、mimic、连杆长度或机械限位； +- 不覆盖原始 URDF； +- 失败时不生成正式 JSON 和正式修正 URDF。 + +程序会拒绝名称包含 `zero_calibrated` 或已有校准时间戳特征的源 URDF，防止重复修正。 + +## 17. 使用已有采样离线重放 + +当完整 `raw_samples.jsonl` 已存在时，可以使用当前代码重新拟合和生成新产物，不连接相机、不发送机械手命令。 + +先只读验证： + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.bash + +python3 -m g20_thumb_apriltag_calibration.offline_replay \ + calibration_output/G20_RIGHT_001/20260811_120146 \ + --output-tag AXIS_FRAME_V3 +``` + +确认报告 `passed: true` 后写入新产物： + +```bash +python3 -m g20_thumb_apriltag_calibration.offline_replay \ + calibration_output/G20_RIGHT_001/20260811_120146 \ + --output-tag AXIS_FRAME_V3 \ + --write +``` + +`--output-tag` 只允许安全文件名字符。离线重放拒绝覆盖已有 JSON、URDF 和报告。 + +离线流程会额外验证： + +- 原始 URDF 哈希在处理前后不变； +- 写出的 URDF 等价于求解的运动学修正； +- URDF 未修改关节平移和机械限位； +- 将修正 URDF 作为候选模型重新求解后，残余零偏不超过允许值； +- 正式文件哈希与临时候选文件一致。 + +## 18. 仿真运行 + +加载修正 URDF 后，使用同次标定 JSON 将 20 通道 u8 命令映射为 21 个 URDF 关节角： + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.bash + +ros2 launch g20_thumb_apriltag_calibration \ + calibrated_joint_state_bridge.launch.py \ + hand_type:=right \ + calibration_file:=/home/lxp/projects/linkerhand_retarget_ros2/calibration_output/G20_RIGHT_001/<时间戳>/g20_right_G20_RIGHT_001_calibration.json +``` + +默认： + +- 订阅 `/cb_right_hand_control_cmd`； +- 发布 `/sim/mujoco/g20/right/joint_state`； +- 发布值只包含动态 `angle_rad`； +- 不会再次叠加 URDF 静态零偏。 + +启动前必须停止其他向同一仿真关节话题发布的桥接节点，避免多个发布者同时驱动模型。 + +## 19. 当前右手 V3 样例结果 + +会话： + +```text +G20_RIGHT_001/20260811_120146 +``` + +使用当前 V3 掌坐标系锚定逻辑离线重放后： + +| 关节 | 静态 URDF 修正 | +|---|---:| +| `thumb_cmc_roll` | `+3.833362°` | +| `thumb_cmc_yaw` | `+1.310573°` | +| `thumb_cmc_pitch` | `+1.896384°` | +| `thumb_mcp` | `0°`，保留CAD | +| 四指 MCP roll/pitch、PIP | `0°`，保留CAD | + +该次 roll 三轮估计为： + +```text +3.825454° / 3.866482° / 3.907776° +``` + +这些值说明当前会话的重复性，不是新标定的固定目标。重新标定应使用新采样独立计算；如果结果明显偏离且无法通过跨轮/留出门限，程序应拒绝生成正式产物。 + +对应 V3 文件： + +```text +src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/ + linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_20260811_120146_AXIS_FRAME_V3.urdf + +calibration_output/G20_RIGHT_001/20260811_120146/ + g20_right_G20_RIGHT_001_calibration_AXIS_FRAME_V3.json + g20_right_G20_RIGHT_001_offline_validation_AXIS_FRAME_V3.json +``` + +## 20. 正式验收检查表 + +正式使用新产物前逐项确认： + +- [ ] 标定使用原始 CAD URDF，而不是旧修正 URDF； +- [ ] 三个相机序列号、内参身份和外参身份匹配； +- [ ] 三个机位接近 30 Hz； +- [ ] 所需 Tag 同时有效率达到 95%； +- [ ] 42 个扫描方向全部完成； +- [ ] 三轮轴方向极差、轨迹残差和回差通过； +- [ ] 第三轮轨迹 MAE/P95 通过； +- [ ] 非零零位在第三轮显著改善原始 URDF； +- [ ] 状态为 `COMPLETE`； +- [ ] 正式 JSON 和修正 URDF 均已生成； +- [ ] 仿真加载的是新 URDF 和同次 JSON； +- [ ] 仿真关节话题只有一个发布者； +- [ ] 使用典型张开、握拳和拇指—食指捏合姿态与实机复核。 + diff --git a/docs/O30右手三相机标定与URDF修正操作说明.md b/docs/O30右手三相机标定与URDF修正操作说明.md new file mode 100644 index 0000000..2fcdfc5 --- /dev/null +++ b/docs/O30右手三相机标定与URDF修正操作说明.md @@ -0,0 +1,157 @@ +# O30 右手三相机标定与 URDF 修正操作说明 + +> 支持范围：O30 右手、20 个有效电机、20 个主动 URDF 关节。 + +## 1. 固定输入 + +O30 SDK 工程： + +```text +/home/lxp/projects/linkerhand-o30-ros2 +``` + +原始 CAD URDF： + +```text +/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf +``` + +标定基准命令固定为： + +```text +[0, 0, 255, 205, 165, 20, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +``` + +20 个通道均按完整 `255→0→255` 行程扫描。Tag 0～10 的机位、ID 和 +父子连杆角色与 G20 右手标定保持一致。 + +## 2. SDK 与 URDF 映射 + +| SDK 下标 | SDK 名称 | URDF 关节 | +|---:|---|---| +| 0 | `thumb_roll` | `thumb_cmc_roll` | +| 1 | `thumb_yaw` | `thumb_cmc_yaw` | +| 2～5 | `index/middle/ring/little_yaw` | 四指 `mcp_roll` | +| 6 | `thumb_root1` | `thumb_mcp` | +| 7～10 | 四指 `root1` | 四指 `mcp_pitch` | +| 11～14 | 四指 `root2` | 四指 `pip` | +| 15 | `thumb_tip` | `thumb_ip` | +| 16～19 | 四指 `tip` | 四指 `dip` | + +O30 没有 `thumb_cmc_pitch`，也没有 G20 的被动 IP/DIP；O30 JSON 中的 +20 个 URDF 关节全部标记为主动关节。 + +## 3. 扫描任务 + +右手以小指为四指参考源，共执行 8 个任务、每项 3 轮： + +| 顺序 | 关节 | 电机 | 机位 | +|---:|---|---:|---| +| 1 | `thumb_cmc_roll` | 0 | front | +| 2 | `thumb_mcp` | 6 | front | +| 3 | `thumb_ip` | 15 | front | +| 4 | `pinky_mcp_roll` | 5 | front | +| 5 | `pinky_mcp_pitch` | 10 | side | +| 6 | `pinky_pip` | 14 | side | +| 7 | `pinky_dip` | 19 | side | +| 8 | `thumb_cmc_yaw` | 1 | top | + +总计 `8 × 3 × 2 = 48` 个唯一扫描方向。O30 每一轮固定按 +`0→255`、`255→0` 完成一次往返，因此单关节三轮均为 `0→255→0`，并在 +每轮第一个方向前重新初始化活动机位的PnP跟踪。状态中的计划进度只统计48个 +唯一方向，另行显示包含自动重扫在内的实际执行方向次数。扫描小指 `mcp_roll` 时，电机 +2/3/4 固定为 255，使未贴 Tag 的三指避开正面机位。 + +O30 命令增大时 URDF 动态角度增大；这与 G20 的命令—角度方向相反，标定 +JSON 会保存单调非递减曲线，并在各电机自己的基准命令处严格归零。 + +O30 的固件反馈端点允许使用独立于 G20 的到位死区：一般通道为 `±4 u8`； +拇指 CMC 侧摆电机 0 在低速命令 `255` 端为 `±9 u8`； +食指 MCP 侧摆电机 2 在命令 `255` 端为 `±8 u8`； +O30 固件的速度 `0` 仍然很快，且实机位置-时间模式无法可靠连续往返，因此标定 +强制使用普通位置模式和最低内部速度 `o30_internal_speed_u8:=0`。标定节点只发送 +最终目标，O30 SDK 在独立的约 125 Hz 控制线程中生成单字节位置斜坡，避免相机/PnP +计算延迟导致 3～5 u8 的补偿跳步。默认 `o30_command_full_range_seconds:=6.0`， +完整走过 `0～255` 约需 6 秒，两个方向采用相同限速。暂停、终止或异常时立即把 +斜坡目标替换为当前反馈并恢复直控模式。该策略不改变安全扫描范围，也不改变四指 +MCP 侧摆静态零位固定为 CAD 0 的规则。拇指 CMC 侧摆使用 `1 u8` 细步，其余 +通道累计 `3 u8` 后再向固件更新目标；后者为每个目标留出保持时间，避免电机6等 +关节因持续刷新单格目标而一直不启动，总体斜率和正反向用时保持不变。 +拇指 MCP（电机 6）在命令 0 端实测稳定反馈为 7，因此该端使用 `±8 u8`。 +这些门限只判断固件是否已经稳定到位，不改变实际下发的 `0～255` 扫描范围。 + +## 4. 静态 URDF 零位策略 + +- 四指 `mcp_roll`：测量小指动态曲线，但四个关节的静态 URDF 零偏全部固定为 0。 +- 小指 `mcp_pitch`：可由下游 PIP 轴观测并通过留出验证后修正。 +- 小指 `pip`：可由下游 DIP 轴线相位观测并通过留出验证后修正。 +- 小指 `dip`、拇指 `ip`：没有下游观察关节，静态零偏固定为 0。 +- 食指、中指、无名指：动态曲线从小指曲线按各自基准命令重新归零；静态偏差不继承。 +- 拇指 `cmc_roll/cmc_yaw/mcp`：按相邻下游轴逐级求解并执行第三轮留出验证。 + +修正 URDF 只改通过验证的关节 `origin.rpy`，不改 `origin.xyz`、`axis`、 +mesh、连杆长度和 CAD 限位。生成后的 URDF 不能作为下一次标定的源文件。 + +## 5. 启动 + +先保证 O30 SDK 已编译，并依次加载 ROS、O30 SDK 和本工程： + +```bash +source /opt/ros/jazzy/setup.bash +source /home/lxp/projects/linkerhand-o30-ros2/install/setup.bash +source /home/lxp/projects/linkerhand_retarget_ros2/install/setup.bash +``` + +预检（不运动）： + +```bash +ros2 launch g20_thumb_apriltag_calibration \ + three_camera_calibration.launch.py \ + hand_model:=O30 hand_type:=right \ + serial_number:=O30_RIGHT_001 \ + source_urdf_path:=/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf \ + camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/o30_three_camera_extrinsics.yaml \ + start_sdk:=false commands_enabled:=false +``` + +正式启动，蓝色/黑色厂商 CANFD 盒： + +```bash +ros2 launch g20_thumb_apriltag_calibration \ + three_camera_calibration.launch.py \ + hand_model:=O30 hand_type:=right \ + serial_number:=O30_RIGHT_001 \ + source_urdf_path:=/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf \ + camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/o30_three_camera_extrinsics.yaml \ + o30_comm_type:=libcanbus canfd_device:=0 o30_auto_setup:=false \ + o30_internal_speed_u8:=0 o30_command_full_range_seconds:=6.0 +``` + +三个机位预检通过后查看状态并启动： + +```bash +ros2 topic echo /o30_calibration/status_text +ros2 service call /o30_calibration/start std_srvs/srv/Trigger {} +``` + +暂停、继续和终止服务分别为： + +```text +/o30_calibration/pause +/o30_calibration/resume +/o30_calibration/abort +``` + +## 6. 产物 + +会话目录中生成： + +```text +raw_samples.jsonl +o30_right__calibration.json +``` + +修正 URDF 默认生成在原始 URDF 同目录，文件名包含 +`zero_calibrated__`。JSON 的动态角度必须和这次生成的修正 +URDF 配套使用；运行时不能再次叠加 `urdf_zero_offset_rad`。 diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 324e28c..e3ef84e 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -1,4 +1,8 @@ -# G20 左右手 AprilTag 标定 +# G20 左右手与 O30 右手 AprilTag 标定 + +O30 右手使用相同的三相机/11-Tag 几何采集框架，但采用独立的 20 电机 +profile、8 项扫描任务和主动关节零位策略。完整映射、固定基准命令和启动方法见 +[O30 右手操作说明](../../docs/O30右手三相机标定与URDF修正操作说明.md)。 ## 三机位三维关节轴零位标定（schema v4） diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml index dea3733..b428973 100644 --- a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml @@ -1,4 +1,4 @@ -g20_calibration: +/**: ros__parameters: command_topic: /g20/cb_left_hand_control_cmd state_topic: /g20/cb_left_hand_state @@ -16,6 +16,10 @@ g20_calibration: normal_calibration_speed: 15 index_roll_calibration_speed: 5 index_flex_calibration_speed: 10 + # O30固件速度0仍很快，标定时使用最低内部速度，并由SDK位置斜坡控制平均速度。 + o30_internal_speed_u8: 0 + # O30 SDK位置斜坡完整走过0～255所需时间；仅O30使用，G20不受影响。 + o30_command_full_range_seconds: 6.0 speed_setting_settle_seconds: 0.25 # tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。 @@ -65,6 +69,14 @@ g20_calibration: zero_finger_maximum_offset_deg: 3.0 endpoint_tolerance_u8: 2.0 + # O30实机在多数目标处会稳定相差最多4；仅O30使用，不改变G20门限。 + o30_endpoint_tolerance_u8: 4.0 + # O30拇指CMC侧摆（电机0）低速命令255端实测稳定反馈为247；该端使用±9。 + o30_thumb_cmc_roll_255_endpoint_tolerance_u8: 9.0 + # O30食指MCP侧摆（电机2）命令255端实测稳定反馈为248；该端使用±8。 + o30_index_mcp_roll_255_endpoint_tolerance_u8: 8.0 + # O30拇指MCP（电机6）命令0端实测稳定反馈为7；该端使用±8。 + o30_thumb_mcp_zero_endpoint_tolerance_u8: 8.0 # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。 thumb_yaw_zero_endpoint_tolerance_u8: 4.0 # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。 diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_tags_o30.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_tags_o30.yaml new file mode 100644 index 0000000..2e9b6ad --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_tags_o30.yaml @@ -0,0 +1,62 @@ +/o30_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] + frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll] + sizes: [0.016, 0.016, 0.016, 0.016, 0.016] + +/o30_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, 7] + frames: [side_base, pinky_mcp, pinky_pip, pinky_dip] + sizes: [0.016, 0.016, 0.016, 0.016] + +/o30_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] diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py index f227b07..1a315bd 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py @@ -1,4 +1,4 @@ -"""Map G20 u8 commands to URDF joint angles using one calibration JSON. +"""Map supported-hand u8 commands to URDF angles using 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 @@ -69,9 +69,64 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = ( "thumb_mcp", ) +O30_COMMAND_NAMES: tuple[str, ...] = ( + "thumb_roll", + "thumb_yaw", + "index_yaw", + "middle_yaw", + "ring_yaw", + "little_yaw", + "thumb_root1", + "index_root1", + "middle_root1", + "ring_root1", + "little_root1", + "index_root2", + "middle_root2", + "ring_root2", + "little_root2", + "thumb_tip", + "index_tip", + "middle_tip", + "ring_tip", + "little_tip", +) + +O30_URDF_JOINT_NAMES: tuple[str, ...] = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_mcp", + "thumb_ip", + "index_mcp_roll", + "index_mcp_pitch", + "index_pip", + "index_dip", + "middle_mcp_roll", + "middle_mcp_pitch", + "middle_pip", + "middle_dip", + "ring_mcp_roll", + "ring_mcp_pitch", + "ring_pip", + "ring_dip", + "pinky_mcp_roll", + "pinky_mcp_pitch", + "pinky_pip", + "pinky_dip", +) + +COMMAND_NAMES_BY_MODEL = { + "G20": G20_COMMAND_NAMES, + "O30": O30_COMMAND_NAMES, +} +URDF_JOINT_NAMES_BY_MODEL = { + "G20": G20_URDF_JOINT_NAMES, + "O30": O30_URDF_JOINT_NAMES, +} + class CalibratedCommandMapper: - """Validated, side-specific lookup from a G20 command to URDF radians.""" + """Validated, model/side-specific lookup from commands to URDF radians.""" def __init__( self, payload: Mapping[str, Any], *, expected_side: str | None = None @@ -86,26 +141,31 @@ class CalibratedCommandMapper: quality = payload["quality"] if quality.get("passed") is not True: raise ValueError("calibration quality.passed must be true") - profile = get_hand_calibration_profile(side) + model = str(payload["model"]).upper() + profile = get_hand_calibration_profile(side, model) + urdf_joint_names = URDF_JOINT_NAMES_BY_MODEL[model] + self.model = model self.side = side self.serial_number = str(payload["serial_number"]) + self.command_names = COMMAND_NAMES_BY_MODEL[model] + self.urdf_joint_names = urdf_joint_names self._motor_by_joint = { name: int(profile.joint_specs[name].motor_index) - for name in G20_URDF_JOINT_NAMES + for name in urdf_joint_names } self._curves = { name: tuple( float(value) for value in payload["joints"][name]["angle_rad"] ) - for name in G20_URDF_JOINT_NAMES + for name in 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") + raise ValueError("command positions must be finite") return max(0, min(255, int(math.floor(command + 0.5)))) def map_positions( @@ -120,22 +180,24 @@ class CalibratedCommandMapper: 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] + missing = [name for name in self.command_names if name not in by_name] if missing: raise ValueError( - "G20 command is missing named channels: " + ",".join(missing) + f"{self.model} command is missing named channels: " + + ",".join(missing) ) - command = tuple(by_name[name] for name in G20_COMMAND_NAMES) + command = tuple(by_name[name] for name in self.command_names) else: - if len(values) != len(G20_COMMAND_NAMES): + if len(values) != len(self.command_names): raise ValueError( - "unnamed G20 command must contain exactly 20 positions" + f"unnamed {self.model} command must contain exactly " + f"{len(self.command_names)} 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 + for name in self.urdf_joint_names ) @@ -152,25 +214,32 @@ def load_calibrated_command_mapper( class CalibratedJointStateBridge(Node): def __init__(self) -> None: super().__init__("g20_calibrated_joint_state_bridge") + self.declare_parameter("hand_model", "G20") self.declare_parameter("hand_type", "right") self.declare_parameter("calibration_file", "") self.declare_parameter("input_topic", "") self.declare_parameter("output_topic", "") + hand_model = str(self.get_parameter("hand_model").value).upper() 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") + get_hand_calibration_profile(hand_type, hand_model) 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 ) + if self.mapper.model != hand_model: + raise ValueError( + f"calibration model {self.mapper.model!r} does not match " + f"requested model {hand_model!r}" + ) 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" + output_topic + or f"/sim/mujoco/{hand_model.lower()}/{hand_type}/joint_state" ) self.publisher = self.create_publisher(JointState, self.output_topic, 10) self.subscription = self.create_subscription( @@ -178,7 +247,8 @@ class CalibratedJointStateBridge(Node): ) self._last_error = "" self.get_logger().info( - f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: " + f"loaded {hand_type} {hand_model} calibration for " + f"{self.mapper.serial_number}: " f"{self.input_topic} -> {self.output_topic}" ) @@ -194,7 +264,7 @@ class CalibratedJointStateBridge(Node): self._last_error = "" result = JointState() result.header = command.header - result.name = list(G20_URDF_JOINT_NAMES) + result.name = list(self.mapper.urdf_joint_names) result.position = list(positions) self.publisher.publish(result) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py index 63c9e70..286b9a3 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py @@ -1,4 +1,4 @@ -"""Pure three-camera G20 calibration model and compact runtime schema. +"""Pure three-camera hand calibration models 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: @@ -36,6 +36,59 @@ THREE_CAMERA_BASELINE_COMMAND: tuple[int, ...] = ( 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, ) +O30_RIGHT_BASELINE_COMMAND: tuple[int, ...] = ( + 0, 0, 255, 205, 165, 20, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, +) + +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", +) + +# Names published by linker_hand_o30_ros2_sdk. The SDK ignores command +# names, but using its names here lets state messages be safely reordered. +O30_COMMAND_NAMES: tuple[str, ...] = ( + "thumb_roll", + "thumb_yaw", + "index_yaw", + "middle_yaw", + "ring_yaw", + "little_yaw", + "thumb_root1", + "index_root1", + "middle_root1", + "ring_root1", + "little_root1", + "index_root2", + "middle_root2", + "ring_root2", + "little_root2", + "thumb_tip", + "index_tip", + "middle_tip", + "ring_tip", + "little_tip", +) + @dataclass(frozen=True) class JointSpec: @@ -47,6 +100,7 @@ class JointSpec: child_role: str | None source_joint: str | None = None zero_kind: str | None = None + command_increasing: bool = False @property def measured(self) -> bool: @@ -74,7 +128,10 @@ class JointCurveFit: @dataclass(frozen=True) class HandCalibrationProfile: + model: str side: str + command_names: tuple[str, ...] + baseline_command: tuple[int, ...] reference_finger: str view_tags: Mapping[str, Mapping[str, int]] preflight_view_roles: Mapping[str, tuple[str, ...]] @@ -129,7 +186,7 @@ _MOTOR_BY_JOINT: dict[str, int] = { } -def _build_hand_profile(side: str) -> HandCalibrationProfile: +def _build_g20_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") @@ -283,7 +340,10 @@ def _build_hand_profile(side: str) -> HandCalibrationProfile: # pose and hide Tag 1 from the front camera. thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {} return HandCalibrationProfile( + model="G20", side=hand_side, + command_names=G20_COMMAND_NAMES, + baseline_command=THREE_CAMERA_BASELINE_COMMAND, reference_finger=reference, view_tags=view_tags, preflight_view_roles=preflight_view_roles, @@ -295,12 +355,132 @@ def _build_hand_profile(side: str) -> HandCalibrationProfile: ) -LEFT_HAND_PROFILE = _build_hand_profile("left") -RIGHT_HAND_PROFILE = _build_hand_profile("right") +def _build_o30_right_profile() -> HandCalibrationProfile: + reference = "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}, + } + joint_specs: dict[str, JointSpec] = { + "thumb_cmc_roll": JointSpec( + "thumb_cmc_roll", 0, True, "front", "front_base", "thumb_cmc", + zero_kind="urdf_axis_chain", + command_increasing=True, + ), + "thumb_cmc_yaw": JointSpec( + "thumb_cmc_yaw", 1, True, "top", "top_base", "thumb_yaw", + zero_kind="urdf_axis_chain", + command_increasing=True, + ), + "thumb_mcp": JointSpec( + "thumb_mcp", 6, True, "front", "thumb_cmc", "thumb_mcp", + zero_kind="urdf_axis_chain", + command_increasing=True, + ), + "thumb_ip": JointSpec( + "thumb_ip", 15, True, "front", "thumb_mcp", "thumb_ip", + zero_kind="urdf_axis_chain", + command_increasing=True, + ), + } + motor_groups = { + "mcp_roll": (2, 3, 4, 5), + "mcp_pitch": (7, 8, 9, 10), + "pip": (11, 12, 13, 14), + "dip": (16, 17, 18, 19), + } + role_pairs = { + "mcp_roll": ("front", "front_base", roll_role), + "mcp_pitch": ("side", "side_base", mcp_role), + "pip": ("side", mcp_role, pip_role), + "dip": ("side", pip_role, dip_role), + } + for suffix, motors in motor_groups.items(): + for finger, motor in zip(_FINGERS, motors): + name = f"{finger}_{suffix}" + source = None if finger == reference else f"{reference}_{suffix}" + view, parent_role, child_role = role_pairs[suffix] + joint_specs[name] = JointSpec( + name, + motor, + True, + view if source is None else None, + parent_role if source is None else None, + child_role if source is None else None, + source_joint=source, + zero_kind="urdf_axis_chain" if source is None else "inherited", + command_increasing=True, + ) + sweep_specs = ( + SweepSpec("front", 0, ("thumb_cmc_roll",)), + SweepSpec("front", 6, ("thumb_mcp",)), + SweepSpec("front", 15, ("thumb_ip",)), + SweepSpec("front", 5, ("pinky_mcp_roll",)), + SweepSpec("side", 10, ("pinky_mcp_pitch",)), + SweepSpec("side", 14, ("pinky_pip",)), + SweepSpec("side", 19, ("pinky_dip",)), + SweepSpec("top", 1, ("thumb_cmc_yaw",)), + ) + image_joints = frozenset( + { + "thumb_cmc_roll", + "thumb_mcp", + "thumb_ip", + "pinky_mcp_roll", + "pinky_mcp_pitch", + "pinky_pip", + } + ) + return HandCalibrationProfile( + model="O30", + side="right", + command_names=O30_COMMAND_NAMES, + baseline_command=O30_RIGHT_BASELINE_COMMAND, + reference_finger=reference, + view_tags=view_tags, + preflight_view_roles={view: tuple(tags) for view, tags in view_tags.items()}, + joint_specs=joint_specs, + sweep_specs=sweep_specs, + image_trajectory_joints=image_joints, + # The right-hand camera clearance convention matches the G20 right + # hand: move the three untagged splay motors to the far endpoint. + roll_clearance_commands={2: 255, 3: 255, 4: 255}, + thumb_pitch_clearance_commands={}, + ) -def get_hand_calibration_profile(side: str) -> HandCalibrationProfile: +LEFT_HAND_PROFILE = _build_g20_hand_profile("left") +RIGHT_HAND_PROFILE = _build_g20_hand_profile("right") +O30_RIGHT_HAND_PROFILE = _build_o30_right_profile() + + +def get_hand_calibration_profile( + side: str, model: str = "G20" +) -> HandCalibrationProfile: + hand_model = str(model).upper() value = str(side).lower() + if hand_model == "O30": + if value != "right": + raise ValueError("O30 calibration currently supports only the right hand") + return O30_RIGHT_HAND_PROFILE + if hand_model != "G20": + raise ValueError("hand model must be G20 or O30") if value == "left": return LEFT_HAND_PROFILE if value == "right": @@ -353,7 +533,7 @@ def calibration_auxiliary_commands( 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: + if profile.model == "G20" and spec.motor_index == 10: return dict(THUMB_YAW_CLEARANCE_COMMANDS) return {} @@ -361,18 +541,19 @@ def calibration_auxiliary_commands( def build_full_hand_command( motor_index: int, command_u8: int, - baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + baseline: Sequence[int] | None = None, profile: HandCalibrationProfile = LEFT_HAND_PROFILE, ) -> list[int]: - if len(baseline) != 20: + selected_baseline = profile.baseline_command if baseline is None else baseline + if len(selected_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") + raise ValueError("motor_index is not a controlled 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] + result = [int(value) for value in selected_baseline] if any(not 0 <= value <= 255 for value in result): raise ValueError("baseline values must be in [0, 255]") result[motor] = command @@ -382,7 +563,7 @@ def build_full_hand_command( def build_calibration_motion_command( spec: SweepSpec, command_u8: int, - baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + baseline: Sequence[int] | None = None, profile: HandCalibrationProfile = LEFT_HAND_PROFILE, ) -> list[int]: """Build a sweep command, including any required clearance pose.""" @@ -407,7 +588,7 @@ def build_calibration_speed_profile( index_flex_speed: int, profile: HandCalibrationProfile = LEFT_HAND_PROFILE, ) -> list[int]: - """Return G20's five per-finger speeds for one calibration task.""" + """Return the five-value speed command accepted by the selected SDK.""" normal = int(normal_speed) index_roll = int(index_roll_speed) index_flex = int(index_flex_speed) @@ -416,6 +597,22 @@ def build_calibration_speed_profile( ): raise ValueError("calibration speeds must be in [0, 255]") speeds = [normal] * 5 + if profile.model == "O30": + # The current O30 ROS SDK accepts the common five-value command and + # broadcasts its first entry to all 20 motors. Only one motor moves + # during calibration, so broadcast the active task's intended speed. + task_speed = ( + index_roll + if spec.motor_index == profile.reference_roll_motor + else index_flex + if spec.motor_index in { + profile.reference_pitch_motor, + profile.reference_pip_motor, + profile.joint_specs[f"{profile.reference_finger}_dip"].motor_index, + } + else normal + ) + return [task_speed] * 5 if spec.motor_index == profile.reference_roll_motor: speeds[profile.reference_speed_slot] = index_roll elif spec.motor_index in { @@ -733,6 +930,35 @@ def fit_projected_zero( return round(float(circular_median_rad(angles)), 8) +def remap_inherited_curve( + values: Sequence[float], + *, + source_zero_command_u8: int, + target_zero_command_u8: int, +) -> list[float]: + """Re-centre a shared curve at the target motor's neutral command. + + O30 finger-yaw motors have different straight-hand commands. A literal + copy of the reference pinky's curve would therefore make the index, + middle, and ring fingers non-zero at their own baseline. Preserve the + measured command scale and subtract its value at the target neutral; this + changes only the dynamic angle datum and does not invent a static URDF + correction for an unobserved motor. + """ + curve = np.asarray(values, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError("inherited curve must contain 256 finite values") + source_zero = int(source_zero_command_u8) + target_zero = int(target_zero_command_u8) + if not 0 <= source_zero <= 255 or not 0 <= target_zero <= 255: + raise ValueError("curve zero commands must be in [0, 255]") + if abs(float(curve[source_zero])) > 1.0e-6: + raise ValueError("source curve must be zero at source_zero_command_u8") + result = curve.copy() + result -= float(result[target_zero]) + return [round(float(value), 8) for value in result] + + def build_compact_payload( *, serial_number: str, @@ -740,10 +966,12 @@ def build_compact_payload( urdf_zero_offsets_rad: Mapping[str, float], validation_errors_rad: Sequence[float], passed: bool, - baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + baseline: Sequence[int] | None = None, side: str = "left", + model: str = "G20", ) -> dict[str, Any]: - profile = get_hand_calibration_profile(side) + profile = get_hand_calibration_profile(side, model) + selected_baseline = profile.baseline_command if baseline is None else baseline if set(measured_fits) != set(profile.measured_joints): raise ValueError("measured_fits must contain all directly measured joints") expected_active = { @@ -751,19 +979,35 @@ def build_compact_payload( } if set(urdf_zero_offsets_rad) != expected_active: raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set") - if len(baseline) != 20: + if len(selected_baseline) != 20: raise ValueError("baseline must contain exactly 20 commands") + measured_curves = { + name: [round(float(value), 8) for value in fit.angle_rad] + for name, fit in measured_fits.items() + } 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] + source_spec = profile.joint_specs[source_name] + source_zero = int(selected_baseline[source_spec.motor_index]) + target_zero = int(selected_baseline[spec.motor_index]) + angle_rad = ( + measured_curves[source_name] + if spec.source_joint is None + else remap_inherited_curve( + measured_curves[source_name], + source_zero_command_u8=source_zero, + target_zero_command_u8=target_zero, + ) + ) joint: dict[str, Any] = { "motor_index": int(spec.motor_index), - "angle_rad": [round(float(value), 8) for value in fit.angle_rad], + "angle_rad": angle_rad, } if spec.active: - joint["zero_command_u8"] = int(baseline[spec.motor_index]) + joint["zero_command_u8"] = target_zero joint["zero_angles"] = { "urdf_zero_offset_rad": round( float(urdf_zero_offsets_rad[name]), 8 @@ -780,12 +1024,12 @@ def build_compact_payload( p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan") payload = { "schema_version": 4, - "model": "G20", + "model": profile.model, "side": profile.side, "serial_number": str(serial_number), "angle_unit": "rad", "command_range": [0, 255], - "baseline_command_u8": [int(value) for value in baseline], + "baseline_command_u8": [int(value) for value in selected_baseline], "joints": joints, "quality": { "passed": bool(passed), @@ -810,9 +1054,9 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: 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"])) + model = str(payload["model"]).upper() + side = str(payload["side"]).lower() + profile = get_hand_calibration_profile(side, model) 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"] @@ -820,7 +1064,10 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: 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") + raise ValueError( + f"payload must contain exactly {len(profile.joint_specs)} " + f"{profile.model} joints" + ) for name, spec in profile.joint_specs.items(): joint = joints[name] @@ -837,8 +1084,14 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: 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") + differences = np.diff(curve) + if ( + np.any(differences < -1.0e-7) + if spec.command_increasing + else np.any(differences > 1.0e-7) + ): + direction = "non-decreasing" if spec.command_increasing else "non-increasing" + raise ValueError(f"{name}.angle_rad must be {direction}") if spec.active: zero = joint["zero_command_u8"] if not isinstance(zero, int) or not 0 <= zero <= 255: @@ -863,10 +1116,17 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> 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") + expected_curve = ( + remap_inherited_curve( + source["angle_rad"], + source_zero_command_u8=int(source["zero_command_u8"]), + target_zero_command_u8=int(joint["zero_command_u8"]), + ) + if spec.active + else source["angle_rad"] + ) + if joint["angle_rad"] != expected_curve: + raise ValueError(f"{name} must remap its source curve exactly") # 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. diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py index e879a27..1ea9e49 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py @@ -65,7 +65,14 @@ def _output_suffix(output_tag: str | None) -> str: 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"]) + if not isinstance(payload, Mapping): + raise ValueError("calibration config must contain a mapping") + node = payload.get("/**", payload.get("g20_calibration")) + if not isinstance(node, Mapping) or not isinstance( + node.get("ros__parameters"), Mapping + ): + raise ValueError("calibration config is missing ros__parameters") + return dict(node["ros__parameters"]) def _latest_attempt_records( @@ -129,6 +136,7 @@ def _fit_curve( return fit_rotation_joint_curve( records, zero_command_u8=int(baseline[motor]), + command_increasing=profile.joint_specs[name].command_increasing, ) @@ -140,16 +148,10 @@ def _fit_axes( extrinsics_file: Path, repetitions: int, ) -> list[JointAxisMeasurement]: - zero_profile = get_zero_calibration_profile(profile.side) + zero_profile = get_zero_calibration_profile(profile.side, profile.model) 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", - } + upstream_by_joint = zero_profile.parallel_axis_parent_joint def fit_one(name: str, cycle: int) -> JointAxisMeasurement: key = (name, cycle) @@ -169,6 +171,7 @@ def _fit_axes( axis_common_constraint=constraint, constrained_circle_joints=zero_profile.constrained_circle_joints, view_normal_common_xyz=view_normal, + command_increasing=spec.command_increasing, ) condition = build_calibration_motion_command( spec, @@ -224,7 +227,7 @@ def _quality_failures( ) -> list[str]: failures: list[str] = [] repetitions = int(parameters["repetitions"]) - zero_profile = get_zero_calibration_profile(profile.side) + zero_profile = get_zero_calibration_profile(profile.side, profile.model) axis_by_key = {(item.joint, item.cycle): item for item in axes} expected_directions = {"decreasing", "increasing"} for name in profile.measured_joints: @@ -247,8 +250,10 @@ def _quality_failures( ] attempts = attempts_by_direction.get((cycle, direction), set()) if len(attempts) != 1: + attempt_list = sorted(attempts) failures.append( - f"{name} cycle {cycle + 1} {direction}: ambiguous attempts {sorted(attempts)}" + f"{name} cycle {cycle + 1} {direction}: " + f"ambiguous attempts {attempt_list}" ) continue commands = sorted({int(record["command_u8"]) for record in selected}) @@ -510,9 +515,10 @@ def replay_session( ) parameters = _load_parameters(config) start, records_by_joint, raw_path = _load_raw_session(session) + hand_model = str(start.get("hand_model", "G20")).upper() side = str(start["hand_type"]).lower() - profile = get_hand_calibration_profile(side) - zero_profile = get_zero_calibration_profile(side) + profile = get_hand_calibration_profile(side, hand_model) + zero_profile = get_zero_calibration_profile(side, hand_model) baseline = tuple(int(value) for value in start["baseline_command_u8"]) if len(baseline) != 20: raise ValueError("session baseline must contain exactly 20 commands") @@ -627,6 +633,7 @@ def replay_session( "maximum_validation_mae_rad": maximum_validation_mae, "maximum_validation_p95_rad": maximum_validation_p95, "hand_type": side, + "hand_model": hand_model, } holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments) if not holdout_zero.passed: @@ -683,12 +690,14 @@ def replay_session( passed=True, baseline=baseline, side=side, + model=hand_model, ) validate_compact_payload(payload) stamp = session.name final_json = session / ( - f"g20_{side}_{hand_serial}_calibration{output_suffix}.json" + f"{hand_model.lower()}_{side}_{hand_serial}_calibration" + f"{output_suffix}.json" ) expected_urdf_name = ( f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}" @@ -696,7 +705,8 @@ def replay_session( ) final_urdf = source_urdf.parent / expected_urdf_name report_path = session / ( - f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json" + f"{hand_model.lower()}_{side}_{hand_serial}_offline_validation" + f"{output_suffix}.json" ) if write_outputs: existing = [path for path in (final_json, final_urdf, report_path) if path.exists()] @@ -751,6 +761,7 @@ def replay_session( report: dict[str, Any] = { "passed": True, "session_dir": str(session), + "model": hand_model, "side": side, "serial_number": hand_serial, "output_tag": output_tag, @@ -820,7 +831,10 @@ def replay_session( def main() -> None: parser = argparse.ArgumentParser( - description="Replay and independently validate a complete G20 calibration session." + description=( + "Replay and independently validate a complete supported-hand " + "calibration session." + ) ) parser.add_argument("session_dir") parser.add_argument("--serial-number", default=None) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py index c648bf1..f52298d 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py @@ -99,10 +99,17 @@ def _task_text(active: Mapping[str, Any]) -> str: target = active.get("target_u8") cycle = active.get("cycle", "?") repetitions = active.get("repetitions", "?") + direction_index = active.get("direction_index") + direction_text = ( + "" if direction_index is None else f"第{direction_index}/2程，" + ) task = ( f"{view}机位，{joint_text}，电机{active.get('motor_index')}，" - f"第{cycle}/{repetitions}轮，{start}→{target}" + f"第{cycle}/{repetitions}轮，{direction_text}{start}→{target}" ) + sequence = active.get("cycle_sequence_u8", []) + if len(sequence) == 3: + task += "（本轮" + "→".join(str(value) for value in sequence) + "）" fit_attempt = int(active.get("fit_attempt", 1)) if fit_attempt > 1: task += ( @@ -383,9 +390,14 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str: reason_zh, action_zh = three_camera_reason_zh( state, str(payload.get("reason", "")), active ) + service_prefix = str(payload.get("service_prefix", "/g20_calibration")) + if service_prefix != "/g20_calibration": + reason_zh = reason_zh.replace("/g20_calibration", service_prefix) + action_zh = action_zh.replace("/g20_calibration", service_prefix) progress = float(payload.get("progress", 0.0)) completed = payload.get("completed_sweeps", 0) total = payload.get("total_sweeps", 0) + executed = int(payload.get("executed_sweep_directions", completed)) scan_progress = float( payload.get( "scan_progress", @@ -400,6 +412,17 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str: f"扫描进度{scan_progress:.1%}）", f"当前任务：{_task_text(active)}", ] + if executed > int(completed): + lines.append( + f"实际采集：已启动{executed}个方向（含自动重扫）；" + f"计划进度只统计{total}个唯一方向，重扫不会重复增加计划进度" + ) + attempt = int(active.get("attempt", active.get("fit_attempt", 1))) + if attempt > 1: + lines.append( + f"整关节重采：当前为第{attempt}/" + f"{active.get('fit_attempt_limit', '?')}次采集结果" + ) if state == "RETURN_BASELINE": baseline_command = payload.get("baseline_command_u8", []) return_command = payload.get("return_command_u8", baseline_command) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py index ec0ea97..6cd5288 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py @@ -1,4 +1,4 @@ -"""One-command three-camera calibration for either complete G20 hand.""" +"""One-command three-camera calibration for supported Linker Hand models.""" from __future__ import annotations @@ -31,7 +31,6 @@ from .acquisition import ( update_pnp_reset_watchdog, ) from .core import ( - COMMAND_NAMES, DIRECTION_DECREASING, DIRECTION_INCREASING, robust_rotation_summary, @@ -68,6 +67,7 @@ from .urdf_zero import ( JointAxisMeasurement, ZeroCalibrationProfile, ZeroSolveResult, + UrdfKinematicModel, fit_joint_axis_measurement, fit_rotation_joint_curve, measure_rotation_joint_observation, @@ -114,6 +114,22 @@ def _overall_progress( return min(0.90, 0.90 * scan) +def _sweep_direction_order( + profile: HandCalibrationProfile, +) -> tuple[str, str]: + """Return one complete cycle in the model's baseline-friendly order.""" + if profile.model == "O30": + return DIRECTION_INCREASING, DIRECTION_DECREASING + return DIRECTION_DECREASING, DIRECTION_INCREASING + + +def _cycle_sequence_u8(profile: HandCalibrationProfile) -> tuple[int, int, int]: + """Return the three endpoints traversed by one out-and-back cycle.""" + if _sweep_direction_order(profile)[0] == DIRECTION_INCREASING: + return 0, 255, 0 + return 255, 0, 255 + + def _stamp_ns(stamp: Any) -> int: return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) @@ -272,7 +288,8 @@ class G20ThreeCameraCalibrationNode(Node): self.session_dir.mkdir(parents=True, exist_ok=True) self.raw_path = self.session_dir / "raw_samples.jsonl" self.final_path = self.session_dir / ( - f"g20_{self.hand_type}_{_safe_name(self.serial_number)}_calibration.json" + f"{self.hand_model.lower()}_{self.hand_type}_" + f"{_safe_name(self.serial_number)}_calibration.json" ) self.views = { @@ -284,7 +301,11 @@ class G20ThreeCameraCalibrationNode(Node): self.state_history: deque[StateSample] = deque(maxlen=1200) self.latest_hand_info: dict[str, Any] = {} self.commanded_speed_profile: tuple[int, ...] = () + self.o30_position_mode_commanded = False + self.o30_position_ramp_commanded = False self.speed_commanded_at = 0.0 + self.pending_command_u8: tuple[int, ...] | None = None + self.pending_command_not_before = 0.0 self.state = STATE_PREFLIGHT self.reason = "waiting_for_three_cameras_tags_and_sdk" @@ -296,6 +317,7 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_items: list[SweepItem] = [] self.sweep_index = 0 + self.executed_sweep_directions = 0 self.active_sweep: SweepItem | None = None self.sweep_frames: list[FrameObservation] = [] # Keep synchronised endpoint observations acquired while the motor is @@ -384,6 +406,7 @@ class G20ThreeCameraCalibrationNode(Node): ) def _declare_parameters(self) -> None: + self.declare_parameter("hand_model", "G20") self.declare_parameter("hand_type", "left") self.declare_parameter("serial_number", "UNSET") self.declare_parameter("session_dir", "calibration_output/session") @@ -414,6 +437,8 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("normal_calibration_speed", 15) self.declare_parameter("index_roll_calibration_speed", 5) self.declare_parameter("index_flex_calibration_speed", 10) + self.declare_parameter("o30_internal_speed_u8", 0) + self.declare_parameter("o30_command_full_range_seconds", 6.0) self.declare_parameter("speed_setting_settle_seconds", 0.25) self.declare_parameter("repetitions", 3) self.declare_parameter("preflight_frames", 60) @@ -449,6 +474,16 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("zero_maximum_offset_deg", 20.0) self.declare_parameter("zero_finger_maximum_offset_deg", 3.0) self.declare_parameter("endpoint_tolerance_u8", 2.0) + self.declare_parameter("o30_endpoint_tolerance_u8", 4.0) + self.declare_parameter( + "o30_thumb_cmc_roll_255_endpoint_tolerance_u8", 9.0 + ) + self.declare_parameter( + "o30_index_mcp_roll_255_endpoint_tolerance_u8", 8.0 + ) + self.declare_parameter( + "o30_thumb_mcp_zero_endpoint_tolerance_u8", 8.0 + ) self.declare_parameter("thumb_yaw_zero_endpoint_tolerance_u8", 4.0) self.declare_parameter( "right_thumb_yaw_255_endpoint_tolerance_u8", 5.0 @@ -503,12 +538,13 @@ class G20ThreeCameraCalibrationNode(Node): def value(name: str) -> Any: return self.get_parameter(name).value + self.hand_model = str(value("hand_model")).upper() self.hand_type = str(value("hand_type")).lower() self.profile: HandCalibrationProfile = get_hand_calibration_profile( - self.hand_type + self.hand_type, self.hand_model ) self.zero_profile: ZeroCalibrationProfile = ( - get_zero_calibration_profile(self.hand_type) + get_zero_calibration_profile(self.hand_type, self.hand_model) ) self.serial_number = str(value("serial_number")) if self.serial_number == "UNSET": @@ -542,7 +578,14 @@ class G20ThreeCameraCalibrationNode(Node): self.command_topic = str(value("command_topic")) self.state_topic = str(value("state_topic")) self.info_topic = str(value("info_topic")) - if self.hand_type == "right": + if self.hand_model == "O30": + if self.command_topic == "/g20/cb_left_hand_control_cmd": + self.command_topic = f"/cb_{self.hand_type}_hand_control_cmd" + if self.state_topic == "/g20/cb_left_hand_state": + self.state_topic = f"/cb_{self.hand_type}_hand_state" + if self.info_topic == "/g20/cb_left_hand_info": + self.info_topic = f"/cb_{self.hand_type}_hand_info" + elif self.hand_type == "right": if self.command_topic == "/g20/cb_left_hand_control_cmd": self.command_topic = "/g20/cb_right_hand_control_cmd" if self.state_topic == "/g20/cb_left_hand_state": @@ -550,6 +593,11 @@ class G20ThreeCameraCalibrationNode(Node): if self.info_topic == "/g20/cb_left_hand_info": self.info_topic = "/g20/cb_right_hand_info" self.setting_topic = str(value("setting_topic")) + if ( + self.hand_model == "O30" + and self.setting_topic == "/g20/cb_hand_setting_cmd" + ): + self.setting_topic = "/cb_hand_setting_cmd" self.camera_info_topics = { view: str(value(f"{view}_camera_info_topic")) for view in ("front", "side", "top") @@ -570,6 +618,10 @@ class G20ThreeCameraCalibrationNode(Node): self.index_flex_calibration_speed = int( value("index_flex_calibration_speed") ) + self.o30_internal_speed_u8 = int(value("o30_internal_speed_u8")) + self.o30_command_full_range_seconds = float( + value("o30_command_full_range_seconds") + ) self.speed_setting_settle_seconds = float( value("speed_setting_settle_seconds") ) @@ -644,6 +696,18 @@ class G20ThreeCameraCalibrationNode(Node): ) self.zero_joint_maximum_offsets_rad: dict[str, float] = {} self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) + self.o30_endpoint_tolerance_u8 = float( + value("o30_endpoint_tolerance_u8") + ) + self.o30_thumb_cmc_roll_255_endpoint_tolerance_u8 = float( + value("o30_thumb_cmc_roll_255_endpoint_tolerance_u8") + ) + self.o30_index_mcp_roll_255_endpoint_tolerance_u8 = float( + value("o30_index_mcp_roll_255_endpoint_tolerance_u8") + ) + self.o30_thumb_mcp_zero_endpoint_tolerance_u8 = float( + value("o30_thumb_mcp_zero_endpoint_tolerance_u8") + ) self.thumb_yaw_zero_endpoint_tolerance_u8 = float( value("thumb_yaw_zero_endpoint_tolerance_u8") ) @@ -743,9 +807,10 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError("baseline_command_u8 must contain exactly 20 values") if any(value < 0 or value > 255 for value in self.baseline_command): raise ValueError("baseline_command_u8 values must be in [0, 255]") - if self.baseline_command != THREE_CAMERA_BASELINE_COMMAND: + if self.baseline_command != self.profile.baseline_command: raise ValueError( - "schema v4 requires the fixed G20 three-camera baseline" + f"schema v4 requires the fixed {self.hand_model} " + "three-camera baseline" ) if any(not serial for serial in self.camera_serials.values()): raise ValueError("front/side/top camera serial parameters are required") @@ -755,7 +820,10 @@ class G20ThreeCameraCalibrationNode(Node): ) source_name = self.source_urdf_path.name.lower() opposite = "right" if self.hand_type == "left" else "left" - if f"g20_{opposite}" in source_name: + if ( + f"{self.hand_model.lower()}_{opposite}" in source_name + or f"{self.hand_model.lower()}i_{opposite}" in source_name + ): raise ValueError( "source_urdf_path hand side does not match hand_type" ) @@ -769,16 +837,68 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError( "source_urdf_path must be the original CAD URDF" ) + urdf_model = UrdfKinematicModel(self.source_urdf_path) + missing_urdf_joints = sorted( + set(self.profile.joint_specs) - set(urdf_model.joints) + ) + if missing_urdf_joints: + raise ValueError( + f"source_urdf_path is not a {self.hand_model} " + f"{self.hand_type} URDF; missing joints: " + + ",".join(missing_urdf_joints) + ) if not 0 <= self.normal_calibration_speed <= 255: raise ValueError("normal_calibration_speed must be in [0, 255]") if not 0 <= self.index_roll_calibration_speed <= 255: raise ValueError("index_roll_calibration_speed must be in [0, 255]") if not 0 <= self.index_flex_calibration_speed <= 255: raise ValueError("index_flex_calibration_speed must be in [0, 255]") + if not 0 <= self.o30_internal_speed_u8 <= 255: + raise ValueError("o30_internal_speed_u8 must be in [0, 255]") + if not 1.0 <= self.o30_command_full_range_seconds <= 30.0: + raise ValueError( + "o30_command_full_range_seconds must be in [1, 30]" + ) if self.speed_setting_settle_seconds < 0.0: raise ValueError("speed_setting_settle_seconds must be non-negative") if not 0.0 <= self.endpoint_tolerance_u8 <= 10.0: raise ValueError("endpoint_tolerance_u8 must be in [0, 10]") + if not ( + self.endpoint_tolerance_u8 + <= self.o30_endpoint_tolerance_u8 + <= 10.0 + ): + raise ValueError( + "o30_endpoint_tolerance_u8 must be between the G20 " + "endpoint tolerance and 10" + ) + if not ( + self.o30_endpoint_tolerance_u8 + <= self.o30_thumb_cmc_roll_255_endpoint_tolerance_u8 + <= 10.0 + ): + raise ValueError( + "o30_thumb_cmc_roll_255_endpoint_tolerance_u8 must be " + "between the O30 endpoint tolerance and 10" + ) + if not ( + self.o30_endpoint_tolerance_u8 + <= self.o30_index_mcp_roll_255_endpoint_tolerance_u8 + <= 10.0 + ): + raise ValueError( + "o30_index_mcp_roll_255_endpoint_tolerance_u8 must be " + "between the O30 endpoint tolerance and 10" + ) + if not ( + self.o30_endpoint_tolerance_u8 + <= self.o30_thumb_mcp_zero_endpoint_tolerance_u8 + <= 10.0 + ): + raise ValueError( + "o30_thumb_mcp_zero_endpoint_tolerance_u8 must be between " + "the O30 endpoint tolerance and 10" + ) if not ( self.endpoint_tolerance_u8 <= self.thumb_yaw_zero_endpoint_tolerance_u8 @@ -1049,9 +1169,10 @@ class G20ThreeCameraCalibrationNode(Node): def _state_callback(self, message: JointState) -> None: if len(message.position) != 20: return - if len(message.name) == 20 and set(message.name) == set(COMMAND_NAMES): + command_names = self.profile.command_names + if len(message.name) == 20 and set(message.name) == set(command_names): lookup = dict(zip(message.name, message.position)) - state = tuple(float(lookup[name]) for name in COMMAND_NAMES) + state = tuple(float(lookup[name]) for name in command_names) else: state = tuple(float(value) for value in message.position) stamp = _stamp_ns(message.header.stamp) @@ -1388,9 +1509,8 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_sweep_items = [ SweepItem(spec, cycle, direction) for cycle in sorted(cycles) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, + for direction in _sweep_direction_order( + getattr(self, "profile", LEFT_HAND_PROFILE) ) ] # A planar Tag PnP tracker can occasionally remain on a continuous but @@ -1456,9 +1576,10 @@ class G20ThreeCameraCalibrationNode(Node): SweepItem(spec, cycle, direction) for spec in self.profile.sweep_specs for cycle in range(self.repetitions) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) + for direction in _sweep_direction_order(self.profile) ] self.sweep_index = 0 + self.executed_sweep_directions = 0 self.retry_sweep_spec = None self.retry_resume_index = None self.retry_sweep_items.clear() @@ -1485,6 +1606,7 @@ class G20ThreeCameraCalibrationNode(Node): self.raw_path, { "kind": "session_start", + "hand_model": self.hand_model, "hand_type": self.hand_type, "reference_finger": self.profile.reference_finger, "view_tags": { @@ -1574,50 +1696,156 @@ class G20ThreeCameraCalibrationNode(Node): self, request: Trigger.Request, response: Trigger.Response ) -> Trigger.Response: del request - self._publish_speed_profile(self._normal_speed_profile()) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if profile.model != "O30": + self._publish_speed_profile(self._normal_speed_profile()) hold_current = getattr(self, "_publish_hold_current", None) if hold_current is not None: hold_current() + self._restore_o30_position_mode() self.state = STATE_ABORTED self.reason = "operator_abort" response.success = True response.message = "calibration aborted and current pose held" return response - def _publish_command(self, values: list[int]) -> None: + def _publish_command_now(self, values: tuple[int, ...]) -> None: if not self.commands_enabled: return message = JointState() message.header.stamp = self.get_clock().now().to_msg() - message.name = list(COMMAND_NAMES) + message.name = list(self.profile.command_names) message.position = [float(value) for value in values] self.command_publisher.publish(message) + def _publish_command( + self, + values: list[int], + *, + wait_for_settings: bool = True, + ) -> None: + if not self.commands_enabled: + return + command = tuple(int(value) for value in values) + now = time.monotonic() + not_before = float(getattr(self, "speed_commanded_at", 0.0)) + float( + getattr(self, "speed_setting_settle_seconds", 0.0) + ) + if wait_for_settings and now < not_before: + self.pending_command_u8 = command + self.pending_command_not_before = not_before + return + self.pending_command_u8 = None + G20ThreeCameraCalibrationNode._publish_command_now(self, command) + + def _flush_pending_command(self, now: float) -> None: + command = getattr(self, "pending_command_u8", None) + if command is None: + return + not_before = max( + float(getattr(self, "pending_command_not_before", 0.0)), + float(getattr(self, "speed_commanded_at", 0.0)) + + float(getattr(self, "speed_setting_settle_seconds", 0.0)), + ) + if now < not_before: + return + self.pending_command_u8 = None + G20ThreeCameraCalibrationNode._publish_command_now(self, command) + def _publish_speed_profile(self, speeds: list[int]) -> None: if not self.commands_enabled: return profile = tuple(int(value) for value in speeds) - if profile == self.commanded_speed_profile: - return + changed = G20ThreeCameraCalibrationNode._ensure_o30_position_mode(self) + if G20ThreeCameraCalibrationNode._ensure_o30_position_ramp(self): + changed = True + if profile != self.commanded_speed_profile: + message = String() + message.data = json.dumps( + { + "setting_cmd": "set_speed", + "params": { + "hand_type": self.hand_type, + "speed": list(profile), + }, + } + ) + self.setting_publisher.publish(message) + self.commanded_speed_profile = profile + changed = True + if changed: + self.speed_commanded_at = time.monotonic() + + def _ensure_o30_position_mode(self) -> bool: + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if not self.commands_enabled or profile.model != "O30": + return False + if getattr(self, "o30_position_mode_commanded", False): + return False message = String() message.data = json.dumps( { - "setting_cmd": "set_speed", + "setting_cmd": "clear_move_time", + "params": {"hand_type": self.hand_type}, + } + ) + self.setting_publisher.publish(message) + self.o30_position_mode_commanded = True + return True + + def _ensure_o30_position_ramp(self) -> bool: + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if not self.commands_enabled or profile.model != "O30": + return False + if getattr(self, "o30_position_ramp_commanded", False): + return False + message = String() + message.data = json.dumps( + { + "setting_cmd": "set_position_ramp", "params": { "hand_type": self.hand_type, - "speed": list(profile), + "full_range_seconds": self.o30_command_full_range_seconds, }, } ) self.setting_publisher.publish(message) - self.commanded_speed_profile = profile - self.speed_commanded_at = time.monotonic() + self.o30_position_ramp_commanded = True + return True + + def _restore_o30_position_mode(self) -> None: + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if not self.commands_enabled or profile.model != "O30": + return + self.pending_command_u8 = None + clear_ramp = String() + clear_ramp.data = json.dumps( + { + "setting_cmd": "clear_position_ramp", + "params": {"hand_type": self.hand_type}, + } + ) + self.setting_publisher.publish(clear_ramp) + clear_mode = String() + clear_mode.data = json.dumps( + { + "setting_cmd": "clear_move_time", + "params": {"hand_type": self.hand_type}, + } + ) + self.setting_publisher.publish(clear_mode) + self.o30_position_mode_commanded = True + self.o30_position_ramp_commanded = False def _normal_speed_profile(self) -> list[int]: + if getattr(self, "profile", LEFT_HAND_PROFILE).model == "O30": + return [self.o30_internal_speed_u8] * 5 return [self.normal_calibration_speed] * 5 def _speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if profile.model == "O30": + return [self.o30_internal_speed_u8] * 5 speeds = build_calibration_speed_profile( spec, normal_speed=self.normal_calibration_speed, @@ -1661,11 +1889,12 @@ class G20ThreeCameraCalibrationNode(Node): def _publish_hold_current(self) -> None: if len(self.latest_state_u8) != 20: return + self.pending_command_u8 = None values = [ int(np.clip(np.rint(value), 0, 255)) for value in self.latest_state_u8 ] - self._publish_command(values) + self._publish_command(values, wait_for_settings=False) def _begin_return_baseline(self, after: str) -> None: self.baseline_after = str(after) @@ -1986,7 +2215,12 @@ class G20ThreeCameraCalibrationNode(Node): self, spec: SweepSpec, endpoint_u8: int ) -> float: """Return the measured feedback deadband for one sweep endpoint.""" - if spec.motor_index == 10 and int(endpoint_u8) == 0: + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if ( + profile.model == "G20" + and spec.motor_index == 10 + and int(endpoint_u8) == 0 + ): return float( getattr( self, @@ -1994,7 +2228,11 @@ class G20ThreeCameraCalibrationNode(Node): self.endpoint_tolerance_u8, ) ) - if spec.motor_index == 19 and int(endpoint_u8) == 0: + if ( + profile.model == "G20" + and spec.motor_index == 19 + and int(endpoint_u8) == 0 + ): return float( getattr( self, @@ -2011,7 +2249,56 @@ class G20ThreeCameraCalibrationNode(Node): ) -> float: """Return a side/motor-specific feedback deadband when established.""" profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if int(motor_index) == 10 and int(endpoint_u8) == 0: + if profile.model == "O30": + default = float( + getattr( + self, + "o30_endpoint_tolerance_u8", + self.endpoint_tolerance_u8, + ) + ) + thumb_roll_motor = profile.joint_specs["thumb_cmc_roll"].motor_index + if ( + int(motor_index) == int(thumb_roll_motor) + and int(endpoint_u8) == 255 + ): + return float( + getattr( + self, + "o30_thumb_cmc_roll_255_endpoint_tolerance_u8", + 9.0, + ) + ) + index_roll_motor = profile.joint_specs["index_mcp_roll"].motor_index + if ( + int(motor_index) == int(index_roll_motor) + and int(endpoint_u8) == 255 + ): + return float( + getattr( + self, + "o30_index_mcp_roll_255_endpoint_tolerance_u8", + 8.0, + ) + ) + thumb_mcp_motor = profile.joint_specs["thumb_mcp"].motor_index + if ( + int(motor_index) == int(thumb_mcp_motor) + and int(endpoint_u8) == 0 + ): + return float( + getattr( + self, + "o30_thumb_mcp_zero_endpoint_tolerance_u8", + 8.0, + ) + ) + return default + if ( + profile.model == "G20" + and int(motor_index) == 10 + and int(endpoint_u8) == 0 + ): return float( getattr( self, @@ -2021,6 +2308,7 @@ class G20ThreeCameraCalibrationNode(Node): ) if ( profile.side == "right" + and profile.model == "G20" and int(motor_index) == 10 and int(endpoint_u8) == 255 ): @@ -2046,7 +2334,7 @@ class G20ThreeCameraCalibrationNode(Node): self.active_sweep = self.sweep_items[self.sweep_index] self.active_sweep_is_fit_retry = False item = self.active_sweep - if item.direction == DIRECTION_DECREASING: + if item.direction == _sweep_direction_order(self.profile)[0]: runtime = getattr(self, "views", {}).get(item.spec.view) reset_trackers = getattr(self, "_reset_view_trackers", None) if runtime is not None and reset_trackers is not None: @@ -2093,6 +2381,9 @@ class G20ThreeCameraCalibrationNode(Node): def _begin_active_sweep(self, now: float) -> None: assert self.active_sweep is not None + self.executed_sweep_directions = int( + getattr(self, "executed_sweep_directions", 0) + ) + 1 self.sweep_frames.clear() self.sweep_frames.extend(self.sweep_start_frames) self.sweep_start_frames.clear() @@ -2139,7 +2430,9 @@ class G20ThreeCameraCalibrationNode(Node): ] ) return fit_rotation_joint_curve( - records, zero_command_u8=zero_command + records, + zero_command_u8=zero_command, + command_increasing=profile.joint_specs[joint_name].command_increasing, ) def _fit_axis_measurement( @@ -2148,19 +2441,10 @@ class G20ThreeCameraCalibrationNode(Node): constraint: Sequence[float] | None = None profile = getattr(self, "profile", LEFT_HAND_PROFILE) zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) - reference = profile.reference_finger - upstream_joint = { - # These neighbouring axes are parallel in the fixed source URDF. - # A small planar Tag's monocular PnP orientation can have a stable - # field-dependent bias, so use the well-observed upstream axis - # direction and let each Tag-centre trajectory independently fit - # its physical axis line. This changes no URDF geometry and is - # valid only because the source-URDF axes are deliberately locked. - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - f"{reference}_pip": f"{reference}_mcp_pitch", - f"{reference}_dip": f"{reference}_pip", - }.get(joint_name) + # These neighbouring axes are parallel in the source URDF. Use the + # well-observed upstream direction while independently fitting each + # physical axis line from its Tag-centre trajectory. + upstream_joint = zero_profile.parallel_axis_parent_joint.get(joint_name) if upstream_joint is not None: # Resolve recursively so thumb_ip receives the already constrained # thumb_mcp direction (and index_dip the constrained index_pip @@ -2186,6 +2470,7 @@ class G20ThreeCameraCalibrationNode(Node): axis_common_constraint=constraint, constrained_circle_joints=zero_profile.constrained_circle_joints, view_normal_common_xyz=view_normal, + command_increasing=spec.command_increasing, ) condition_command = build_calibration_motion_command( spec, @@ -2543,6 +2828,14 @@ class G20ThreeCameraCalibrationNode(Node): "motor_index": spec.motor_index, "joints": list(spec.joints), "attempt": attempt, + "fit_attempt_limit": ( + int(getattr(self, "automatic_fit_retry_limit", 0)) + 1 + ), + "cycle_sequence_u8": list( + _cycle_sequence_u8( + getattr(self, "profile", LEFT_HAND_PROFILE) + ) + ), "cycles_to_rescan": [ cycle + 1 for cycle in sorted(self.retry_cycles) ], @@ -2690,6 +2983,14 @@ class G20ThreeCameraCalibrationNode(Node): ), }, ) + if getattr(self, "active_sweep_is_fit_retry", False): + # During an entire-joint fit retry the current item has already + # been popped from retry_sweep_items. Put it back before the + # remaining direction; otherwise an acquisition retry silently + # skips the failed direction and the cycle later lacks endpoints. + retry_items = getattr(self, "retry_sweep_items", None) + if retry_items is not None: + retry_items.insert(0, item) self.reason = f"automatic_retry_{reason}" self._begin_return_baseline("retry_sweep") @@ -2973,6 +3274,7 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cycle_difference_rad ), hand_type=self.hand_type, + hand_model=self.hand_model, ) append_jsonl( self.raw_path, @@ -3091,6 +3393,7 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cycle_difference_rad ), hand_type=self.hand_type, + hand_model=self.hand_model, ) if not zero_result.passed: raise RuntimeError("final_all_cycle_zero_refit_failed") @@ -3241,9 +3544,11 @@ class G20ThreeCameraCalibrationNode(Node): passed=passed, baseline=self.baseline_command, side=self.hand_type, + model=self.hand_model, ) atomic_write_json(self.final_path, payload) self.completed_payload = payload + self._restore_o30_position_mode() self.state = STATE_COMPLETE self.reason = "calibration_passed" if passed else "quality_failed" self.get_logger().info( @@ -3252,6 +3557,7 @@ class G20ThreeCameraCalibrationNode(Node): def _pause(self, reason: str) -> None: self._publish_hold_current() + self._restore_o30_position_mode() self.paused_reason = str(reason) self.state = STATE_PAUSED self.reason = str(reason) @@ -3355,6 +3661,7 @@ class G20ThreeCameraCalibrationNode(Node): def _timer_callback(self) -> None: now = time.monotonic() try: + G20ThreeCameraCalibrationNode._flush_pending_command(self, now) self._advance(now) except Exception as error: self.get_logger().error(f"Calibration paused: {error}") @@ -3645,6 +3952,15 @@ class G20ThreeCameraCalibrationNode(Node): "cycle": self.active_sweep.cycle + 1, "repetitions": self.repetitions, "direction": self.active_sweep.direction, + "direction_index": ( + _sweep_direction_order(self.profile).index( + self.active_sweep.direction + ) + + 1 + ), + "cycle_sequence_u8": list( + _cycle_sequence_u8(self.profile) + ), "fit_attempt": self.sweep_attempts.get(motor, 1), "fit_attempt_limit": self.automatic_fit_retry_limit + 1, "automatic_retry_count": self.sweep_retry_counts.get( @@ -3729,6 +4045,12 @@ class G20ThreeCameraCalibrationNode(Node): "normal_speed": self.normal_calibration_speed, "index_roll_speed": self.index_roll_calibration_speed, "index_flex_speed": self.index_flex_calibration_speed, + "o30_internal_speed_u8": self.o30_internal_speed_u8, + "o30_command_full_range_seconds": ( + self.o30_command_full_range_seconds + if self.hand_model == "O30" + else None + ), }, } elif self.active_validation is not None: @@ -3766,6 +4088,12 @@ class G20ThreeCameraCalibrationNode(Node): "normal_speed": self.normal_calibration_speed, "index_roll_speed": self.index_roll_calibration_speed, "index_flex_speed": self.index_flex_calibration_speed, + "o30_internal_speed_u8": self.o30_internal_speed_u8, + "o30_command_full_range_seconds": ( + self.o30_command_full_range_seconds + if self.hand_model == "O30" + else None + ), }, } elif self.fit_failure: @@ -3778,7 +4106,9 @@ class G20ThreeCameraCalibrationNode(Node): ) ) payload = { + "hand_model": self.hand_model, "hand_type": self.hand_type, + "service_prefix": f"/{self.hand_model.lower()}_calibration", "reference_finger": self.profile.reference_finger, "state": self.state, "reason": self.reason, @@ -3794,6 +4124,7 @@ class G20ThreeCameraCalibrationNode(Node): "scan_progress": round(scan_progress, 4), "completed_sweeps": self.sweep_index, "total_sweeps": len(self.sweep_items), + "executed_sweep_directions": self.executed_sweep_directions, "baseline_command_u8": list(self.baseline_command), "return_command_u8": list( G20ThreeCameraCalibrationNode._current_return_command(self) @@ -3836,6 +4167,9 @@ def main(args: list[str] | None = None) -> None: pass finally: if node is not None: + if rclpy.ok(): + node._restore_o30_position_mode() + rclpy.spin_once(node, timeout_sec=0.1) node.destroy_node() if rclpy.ok(): rclpy.shutdown() diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py index 051a116..7f7be83 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py @@ -45,6 +45,7 @@ class ZeroCalibrationProfile: axis_parent_joint: Mapping[str, str] phase_parent_joint: Mapping[str, str] offset_observer_joint: Mapping[str, str] + parallel_axis_parent_joint: Mapping[str, str] fixed_direct_zero_offsets_rad: Mapping[str, float] static_output_zero_offsets_rad: Mapping[str, float] @@ -59,47 +60,104 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: reference_pitch = f"{reference}_mcp_pitch" reference_pip = f"{reference}_pip" reference_dip = f"{reference}_dip" - direct = ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - reference_roll, - reference_pitch, - reference_pip, + if hand.model == "O30": + direct = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_mcp", + "thumb_ip", + reference_roll, + reference_pitch, + reference_pip, + reference_dip, + ) + else: + direct = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + reference_roll, + reference_pitch, + reference_pip, + ) + inherited_suffixes = ( + ("mcp_roll", "mcp_pitch", "pip", "dip") + if hand.model == "O30" + else ("mcp_roll", "mcp_pitch", "pip") ) inherited = { f"{finger}_{suffix}": f"{reference}_{suffix}" for finger in ("index", "middle", "ring", "pinky") if finger != reference - for suffix in ("mcp_roll", "mcp_pitch", "pip") - } - axis_parent = { - "thumb_cmc_yaw": "thumb_cmc_roll", - "thumb_cmc_pitch": "thumb_cmc_yaw", - reference_pitch: reference_roll, - } - phase_parent = { - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - reference_pip: reference_pitch, - reference_dip: reference_pip, - } - observer = { - "thumb_cmc_roll": "thumb_cmc_yaw", - "thumb_cmc_yaw": "thumb_cmc_pitch", - "thumb_cmc_pitch": "thumb_mcp", - "thumb_mcp": "thumb_ip", - reference_roll: reference_pitch, - reference_pitch: reference_pip, - reference_pip: reference_dip, - } - fixed_direct_zero_offsets = { - "thumb_mcp": 0.0, - reference_roll: 0.0, - reference_pitch: 0.0, - reference_pip: 0.0, + for suffix in inherited_suffixes } + if hand.model == "O30": + axis_parent = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_mcp": "thumb_cmc_yaw", + reference_pitch: reference_roll, + } + phase_parent = { + "thumb_ip": "thumb_mcp", + reference_pip: reference_pitch, + reference_dip: reference_pip, + } + observer = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_mcp", + "thumb_mcp": "thumb_ip", + reference_roll: reference_pitch, + reference_pitch: reference_pip, + reference_pip: reference_dip, + } + # Finger yaw/mcp_roll is intentionally dynamic-only. The terminal + # thumb-IP and pinky-DIP axes have no downstream zero observer. Pinky + # MCP-pitch and PIP remain independently observable through the PIP + # and DIP axes; static offsets are never copied to untagged fingers. + fixed_direct_zero_offsets = { + "thumb_ip": 0.0, + reference_roll: 0.0, + reference_dip: 0.0, + } + parallel_axis_parent = { + "thumb_ip": "thumb_mcp", + reference_pip: reference_pitch, + reference_dip: reference_pip, + } + else: + axis_parent = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", + reference_pitch: reference_roll, + } + phase_parent = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + reference_pip: reference_pitch, + reference_dip: reference_pip, + } + observer = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_cmc_pitch", + "thumb_cmc_pitch": "thumb_mcp", + "thumb_mcp": "thumb_ip", + reference_roll: reference_pitch, + reference_pitch: reference_pip, + reference_pip: reference_dip, + } + fixed_direct_zero_offsets = { + "thumb_mcp": 0.0, + reference_roll: 0.0, + reference_pitch: 0.0, + reference_pip: 0.0, + } + parallel_axis_parent = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + reference_pip: reference_pitch, + reference_dip: reference_pip, + } # No per-device or per-side numeric calibration belongs in the profile. # Any non-zero thumb CMC origin must come from this session's measured # trajectories and pass the independent-cycle validation below. @@ -107,17 +165,7 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: return ZeroCalibrationProfile( hand=hand, direct_zero_joints=direct, - axis_joints=( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - "thumb_ip", - reference_roll, - reference_pitch, - reference_pip, - reference_dip, - ), + axis_joints=hand.measured_joints, inherited_zero_joints=inherited, inherited_static_zero_joints={}, constrained_circle_joints=( @@ -127,6 +175,7 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: axis_parent_joint=axis_parent, phase_parent_joint=phase_parent, offset_observer_joint=observer, + parallel_axis_parent_joint=parallel_axis_parent, # The active MCP trajectory is observable, but its absolute static # phase is inferred only through the passive thumb-IP axis-line centre. # In the fixed front monocular view that shallow arc has a 37-80 degree @@ -147,8 +196,10 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: ) -def get_zero_calibration_profile(side: str) -> ZeroCalibrationProfile: - return _build_zero_profile(get_hand_calibration_profile(side)) +def get_zero_calibration_profile( + side: str, model: str = "G20" +) -> ZeroCalibrationProfile: + return _build_zero_profile(get_hand_calibration_profile(side, model)) LEFT_ZERO_PROFILE = _build_zero_profile(LEFT_HAND_PROFILE) @@ -367,7 +418,10 @@ def _baseline_reference( def fit_rotation_joint_curve( - records: Sequence[Mapping[str, Any]], *, zero_command_u8: int + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + command_increasing: bool = False, ) -> JointCurveFit: """Fit a command curve from full parent-to-child tag orientations.""" samples = [dict(record) for record in records] @@ -392,8 +446,18 @@ def fit_rotation_joint_curve( vectors.append(vector) commands.append(int(record["command_u8"])) axis = fit_rotation_axis(vectors, commands) + if command_increasing: + axis = -axis values_by_record = [float(vector @ axis) for vector in vectors] - curves, correction, hysteresis = _fit_joint_curve(samples, values_by_record) + fit_values = ( + [-value for value in values_by_record] + if command_increasing + else values_by_record + ) + curves, correction, hysteresis = _fit_joint_curve(samples, fit_values) + if command_increasing: + for key in ("angle_rad", "decreasing_rad", "increasing_rad"): + curves[key] = [-float(value) for value in curves[key]] for key in ("angle_rad", "decreasing_rad", "increasing_rad"): values = np.asarray(curves[key], dtype=float) values -= float(values[int(zero_command_u8)]) @@ -739,6 +803,7 @@ def fit_joint_axis_measurement( axis_common_constraint: Sequence[float] | None = None, constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS, view_normal_common_xyz: Sequence[float] | None = None, + command_increasing: bool = False, ) -> JointAxisMeasurement: """Fit one physical screw axis from one complete scan cycle.""" samples = [ @@ -768,6 +833,8 @@ def fit_joint_axis_measurement( ] commands = [int(record["command_u8"]) for record in samples] rotation_axis_child = fit_rotation_axis(rotation_vectors, commands) + if command_increasing: + rotation_axis_child = -rotation_axis_child # reference maps the child Tag frame at baseline into the parent Tag # frame. The quaternion delta axis is expressed in that child frame, # while the fitted centre circle is expressed in the parent frame. This @@ -1053,6 +1120,7 @@ def solve_urdf_zero_offsets( maximum_axis_cone_mismatch_rad: float = math.radians(5.0), maximum_pose_axis_line_rms_m: float = 0.001, hand_type: str = "left", + hand_model: str = "G20", fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, static_output_zero_offsets_rad: Mapping[str, float] | None = None, ) -> ZeroSolveResult: @@ -1065,7 +1133,7 @@ def solve_urdf_zero_offsets( length, along-axis Tag placement, and monocular depth bias from being absorbed as an encoder-zero correction. """ - profile = get_zero_calibration_profile(hand_type) + profile = get_zero_calibration_profile(hand_type, hand_model) model = UrdfKinematicModel(source_urdf) training = [m for m in measurements if m.cycle in set(training_cycles)] validation = [m for m in measurements if m.cycle == int(validation_cycle)] @@ -1262,8 +1330,9 @@ def solve_urdf_zero_offsets( predicted_orientation_axis, ) candidates: list[tuple[float, Rotation, np.ndarray]] = [] - for primary_sign in (1.0, -1.0): - for orientation_sign in (1.0, -1.0): + signs = (1.0,) if profile.hand.model == "O30" else (1.0, -1.0) + for primary_sign in signs: + for orientation_sign in signs: observed_frame = frame_from_two_axes( primary_sign * observed_axes[primary], orientation_sign * observed_orientation_axis, diff --git a/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py b/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py index 25a4a46..5c1a311 100644 --- a/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py @@ -1,4 +1,4 @@ -"""Publish calibrated G20 URDF joint angles from raw u8 commands.""" +"""Publish calibrated URDF joint angles from raw u8 commands.""" from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -9,6 +9,7 @@ from launch_ros.actions import Node def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ + DeclareLaunchArgument("hand_model", default_value="G20"), DeclareLaunchArgument("hand_type", default_value="right"), DeclareLaunchArgument("calibration_file"), DeclareLaunchArgument("input_topic", default_value=""), @@ -16,14 +17,12 @@ def generate_launch_description() -> LaunchDescription: Node( package="g20_thumb_apriltag_calibration", executable="calibrated_joint_state_bridge", - name=[ - "g20_calibrated_joint_state_bridge_", - LaunchConfiguration("hand_type"), - ], + name="calibrated_joint_state_bridge", output="screen", emulate_tty=True, parameters=[ { + "hand_model": LaunchConfiguration("hand_model"), "hand_type": LaunchConfiguration("hand_type"), "calibration_file": LaunchConfiguration( "calibration_file" diff --git a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py index 515a176..ea26df0 100644 --- a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py @@ -1,4 +1,4 @@ -"""Launch three Hikrobot views and one complete-G20 calibration owner.""" +"""Launch three Hikrobot views and one supported-hand calibration owner.""" from __future__ import annotations @@ -21,11 +21,32 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode from launch_ros.parameter_descriptions import ParameterValue +from g20_thumb_apriltag_calibration.full_hand import ( + get_hand_calibration_profile, +) + VIEWS = ("front", "side", "top") -def _default_source_urdf(hand_type: str) -> Path: +def _default_source_urdf(hand_model: str, hand_type: str) -> Path: + if hand_model == "O30": + relative = ( + Path("linkerhand-urdf/O30/urdf_0803-right/src") + / "linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf" + ) + candidates = ( + Path.cwd().parent / relative, + Path.home() / "projects" / relative, + ) + return next( + (candidate for candidate in candidates if candidate.is_file()), + Path.home() + / "projects" + / "linkerhand-urdf/O30/urdf_0803-right/src" + / "linkerhand_O30i_right.urdf" + / "linkerhand_O30i_right-0803.urdf", + ) relative = Path( "assets/robots/hands/linker_hand" ) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf" @@ -38,17 +59,31 @@ def _default_source_urdf(hand_type: str) -> Path: def _launch_stack(context): + hand_model = LaunchConfiguration("hand_model").perform(context).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" + try: + profile = get_hand_calibration_profile(hand_type, hand_model) + except ValueError as error: + raise RuntimeError(str(error)) from error + model_key = hand_model.lower() + calibration_namespace = f"/{model_key}_calibration" + if hand_model == "O30": + command_topic = f"/cb_{hand_type}_hand_control_cmd" + state_topic = f"/cb_{hand_type}_hand_state" + hand_info_topic = f"/cb_{hand_type}_hand_info" + setting_topic = "/cb_hand_setting_cmd" + tag_config = LaunchConfiguration("o30_tag_config") + else: + command_topic = f"/g20/cb_{hand_type}_hand_control_cmd" + state_topic = f"/g20/cb_{hand_type}_hand_state" + hand_info_topic = f"/g20/cb_{hand_type}_hand_info" + setting_topic = "/g20/cb_hand_setting_cmd" + tag_config = LaunchConfiguration("tag_config") 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() + else _default_source_urdf(hand_model, hand_type).resolve() ) if not source_urdf.is_file(): raise RuntimeError(f"source URDF does not exist: {source_urdf}") @@ -91,14 +126,14 @@ def _launch_stack(context): info_topics = [] detection_topics = [] for view in VIEWS: - namespace = f"/g20_calibration/{view}/camera" + namespace = f"{calibration_namespace}/{view}/camera" raw_topic = f"{namespace}/image_raw" - info_topic = f"{namespace}/camera_info" + camera_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) + info_topics.append(camera_info_topic) detection_topics.append(detection_topic) cameras.append( Node( @@ -118,7 +153,9 @@ def _launch_stack(context): "camera_name": LaunchConfiguration( f"{view}_camera_name" ), - "frame_id": f"g20_calibration_{view}_optical_frame", + "frame_id": ( + f"{model_key}_calibration_{view}_optical_frame" + ), "image_width": 1624, "image_height": 1240, "frame_rate": ParameterValue( @@ -151,7 +188,7 @@ def _launch_stack(context): namespace=namespace, remappings=[ ("image", raw_topic), - ("camera_info", info_topic), + ("camera_info", camera_info_topic), ("image_rect", rect_topic), ], parameters=[{"queue_size": 1}], @@ -163,7 +200,7 @@ def _launch_stack(context): name="apriltag", namespace=detector_namespace, parameters=[ - LaunchConfiguration("tag_config"), + tag_config, { "detector.decimate": ParameterValue( LaunchConfiguration("apriltag_decimate"), @@ -173,7 +210,7 @@ def _launch_stack(context): ], remappings=[ ("image_rect", rect_topic), - ("camera_info", info_topic), + ("camera_info", camera_info_topic), ], extra_arguments=[{"use_intra_process_comms": True}], ), @@ -181,7 +218,7 @@ def _launch_stack(context): ) vision = ComposableNodeContainer( - name="g20_three_camera_vision", + name=f"{model_key}_three_camera_vision", namespace="/", package="rclcpp_components", executable="component_container_mt", @@ -189,59 +226,84 @@ 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=[ - { - "hand_type": hand_type, - "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, - # Match 30 Hz cameras so state/image p95 skew stays below 50 ms. - "state_poll_rate": 30.0, - # Calibration does not consume measured joint velocity. A - # 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. - "defer_state_reads_while_commanding": False, - "repeat_position_commands": False, - "is_touch": False, - } - ], - ) + if hand_model == "O30": + sdk = Node( + package="linker_hand_o30_ros2_sdk", + executable="linker_hand_o30_ros2_sdk", + name="linker_hand_o30_ros2_sdk", + output="screen", + condition=IfCondition(LaunchConfiguration("start_sdk")), + parameters=[ + { + "hand_type": hand_type, + "hand_joint": "O30", + "is_touch": False, + "canfd_device": ParameterValue( + LaunchConfiguration("canfd_device"), value_type=int + ), + "comm_type": LaunchConfiguration("o30_comm_type"), + "channel": LaunchConfiguration("can_interface"), + "bitrate": ParameterValue( + LaunchConfiguration("o30_bitrate"), value_type=int + ), + "dbitrate": ParameterValue( + LaunchConfiguration("o30_dbitrate"), value_type=int + ), + "auto_setup": ParameterValue( + LaunchConfiguration("o30_auto_setup"), value_type=bool + ), + } + ], + ) + else: + 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": hand_type, + "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": 30.0, + "velocity_poll_rate": 1.0, + "defer_state_reads_while_commanding": False, + "repeat_position_commands": False, + "is_touch": False, + } + ], + ) calibration = Node( package="g20_thumb_apriltag_calibration", executable="three_camera_calibration_node", - name="g20_calibration", + name=f"{model_key}_calibration", output="screen", emulate_tty=True, parameters=[ LaunchConfiguration("calibration_config"), { "serial_number": hand_serial, + "hand_model": hand_model, "hand_type": hand_type, "session_dir": str(session_dir), # The SDK performs roughly 25 synchronous CAN queries whenever # cb__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", + "info_topic": f"{calibration_namespace}/disabled_hand_info", "command_topic": command_topic, "state_topic": state_topic, + "setting_topic": setting_topic, + "baseline_command_u8": list(profile.baseline_command), "camera_extrinsics_file": LaunchConfiguration( "camera_extrinsics_file" ), @@ -253,6 +315,14 @@ def _launch_stack(context): f"{view}_camera_serial": camera_serials[view] for view in VIEWS }, + **{ + f"{view}_camera_info_topic": info_topics[index] + for index, view in enumerate(VIEWS) + }, + **{ + f"{view}_detections_topic": detection_topics[index] + for index, view in enumerate(VIEWS) + }, "commands_enabled": ParameterValue( LaunchConfiguration("commands_enabled"), value_type=bool ), @@ -267,6 +337,13 @@ def _launch_stack(context): LaunchConfiguration("index_flex_calibration_speed"), value_type=int, ), + "o30_internal_speed_u8": ParameterValue( + LaunchConfiguration("o30_internal_speed_u8"), value_type=int + ), + "o30_command_full_range_seconds": ParameterValue( + LaunchConfiguration("o30_command_full_range_seconds"), + value_type=float, + ), "validation_enabled": ParameterValue( LaunchConfiguration("validation_enabled"), value_type=bool ), @@ -292,15 +369,15 @@ def _launch_stack(context): *detection_topics, command_topic, state_topic, - info_topic, - "/g20_calibration/status", + hand_info_topic, + f"{calibration_namespace}/status", ], output="screen", ) return [ LogInfo( msg=( - f"G20 {hand_type} three-camera session: {session_dir}; " + f"{hand_model} {hand_type} three-camera session: {session_dir}; " f"source_urdf={source_urdf}" ) ), @@ -341,6 +418,7 @@ def generate_launch_description() -> LaunchDescription: name="FASTRTPS_DEFAULT_PROFILES_FILE", value=str(package_share / "config" / "fastdds_large_images.xml"), ), + DeclareLaunchArgument("hand_model", default_value="G20"), DeclareLaunchArgument("hand_type", default_value="left"), DeclareLaunchArgument("serial_number", default_value="UNSET"), DeclareLaunchArgument( @@ -380,6 +458,11 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("auto_exposure", default_value="false"), DeclareLaunchArgument("apriltag_decimate", default_value="1.5"), DeclareLaunchArgument("can_interface", default_value="can0"), + DeclareLaunchArgument("canfd_device", default_value="0"), + DeclareLaunchArgument("o30_comm_type", default_value="socketcan"), + DeclareLaunchArgument("o30_bitrate", default_value="1000000"), + DeclareLaunchArgument("o30_dbitrate", default_value="5000000"), + DeclareLaunchArgument("o30_auto_setup", default_value="true"), DeclareLaunchArgument("calibration_speed", default_value="15"), DeclareLaunchArgument( "index_roll_calibration_speed", default_value="5" @@ -387,6 +470,10 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "index_flex_calibration_speed", default_value="10" ), + DeclareLaunchArgument("o30_internal_speed_u8", default_value="0"), + DeclareLaunchArgument( + "o30_command_full_range_seconds", default_value="6.0" + ), DeclareLaunchArgument("validation_enabled", default_value="false"), DeclareLaunchArgument( "camera_extrinsics_file", @@ -421,6 +508,12 @@ def generate_launch_description() -> LaunchDescription: package_share / "config" / "three_camera_tags.yaml" ), ), + DeclareLaunchArgument( + "o30_tag_config", + default_value=str( + package_share / "config" / "three_camera_tags_o30.yaml" + ), + ), OpaqueFunction(function=_launch_stack), ] ) diff --git a/src/g20_thumb_apriltag_calibration/package.xml b/src/g20_thumb_apriltag_calibration/package.xml index ed71c5b..7995b06 100644 --- a/src/g20_thumb_apriltag_calibration/package.xml +++ b/src/g20_thumb_apriltag_calibration/package.xml @@ -3,7 +3,7 @@ g20_thumb_apriltag_calibration 0.1.0 - Three-view Hikrobot AprilTag calibration for the complete left G20 hand. + Three-view Hikrobot AprilTag calibration for G20 hands and the O30 right hand. lxp MIT @@ -15,6 +15,7 @@ launch launch_ros linker_hand_ros2_sdk + linker_hand_o30_ros2_sdk rclcpp_components rclpy rosbag2 diff --git a/src/g20_thumb_apriltag_calibration/setup.py b/src/g20_thumb_apriltag_calibration/setup.py index c14c7a9..8dfad1d 100644 --- a/src/g20_thumb_apriltag_calibration/setup.py +++ b/src/g20_thumb_apriltag_calibration/setup.py @@ -26,7 +26,7 @@ setup( zip_safe=True, maintainer="lxp", maintainer_email="support@linker-robotics.com", - description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand", + description="Three-view AprilTag calibration for G20 and O30 hands", license="MIT", entry_points={ "console_scripts": [ diff --git a/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py b/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py index 9b2ead7..cf8eb84 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py +++ b/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py @@ -5,9 +5,13 @@ import pytest from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import ( G20_COMMAND_NAMES, G20_URDF_JOINT_NAMES, + O30_COMMAND_NAMES, + O30_URDF_JOINT_NAMES, CalibratedCommandMapper, ) from g20_thumb_apriltag_calibration.full_hand import ( + JointCurveFit, + build_compact_payload, get_hand_calibration_profile, ) @@ -85,3 +89,43 @@ 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"]) + + +def test_o30_mapper_uses_sdk_names_and_twenty_active_urdf_joints() -> None: + profile = get_hand_calibration_profile("right", "O30") + + def fit_for(name: str) -> JointCurveFit: + zero = profile.baseline_command[profile.joint_specs[name].motor_index] + curve = tuple((command - zero) * 0.001 for command in range(256)) + return JointCurveFit( + angle_rad=curve, + decreasing_rad=curve, + increasing_rad=curve, + circle={}, + maximum_monotonic_correction_rad=0.0, + maximum_hysteresis_rad=0.0, + quality={}, + ) + + payload = build_compact_payload( + serial_number="O30_RIGHT_TEST", + measured_fits={name: fit_for(name) for name in profile.measured_joints}, + urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints}, + validation_errors_rad=[0.0], + passed=True, + side="right", + model="O30", + ) + mapper = CalibratedCommandMapper(payload, expected_side="right") + + assert mapper.model == "O30" + assert mapper.command_names == O30_COMMAND_NAMES + assert mapper.urdf_joint_names == O30_URDF_JOINT_NAMES + baseline = profile.baseline_command + assert mapper.map_positions(baseline, O30_COMMAND_NAMES) == pytest.approx( + [0.0] * 20 + ) + moved = list(baseline) + moved[19] = 100 + result = dict(zip(O30_URDF_JOINT_NAMES, mapper.map_positions(moved))) + assert result["pinky_dip"] == pytest.approx(0.1) diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index f28b4d9..a179b04 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/src/g20_thumb_apriltag_calibration/test/test_config.py @@ -56,35 +56,50 @@ def test_front_tag_parameters_match_namespaced_detector() -> None: def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None: - tags = yaml.safe_load( - (PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text() - ) expected = { "front": [0, 1, 2, 3, 10], "side": [4, 5, 6, 7], "top": [8, 9], } - all_ids = set() - for view, ids in expected.items(): - key = f"/g20_calibration/{view}/apriltag/apriltag" - parameters = tags[key]["ros__parameters"] - assert parameters["tag"]["ids"] == ids - assert parameters["size"] == 0.016 - assert parameters["tag"]["sizes"] == [0.016] * len(ids) - assert parameters["qos_profile"] == "sensor_data" - assert parameters["detector"]["decimate"] == 1.5 - all_ids.update(ids) - assert all_ids == set(range(11)) - assert tags[ - "/g20_calibration/side/apriltag/apriltag" - ]["ros__parameters"]["tag"]["frames"][0] == "side_base" + for model in ("g20", "o30"): + suffix = "" if model == "g20" else "_o30" + tags = yaml.safe_load( + ( + PACKAGE_ROOT + / "config" + / f"three_camera_tags{suffix}.yaml" + ).read_text() + ) + all_ids = set() + for view, ids in expected.items(): + key = f"/{model}_calibration/{view}/apriltag/apriltag" + parameters = tags[key]["ros__parameters"] + assert parameters["tag"]["ids"] == ids + assert parameters["size"] == 0.016 + assert parameters["tag"]["sizes"] == [0.016] * len(ids) + assert parameters["qos_profile"] == "sensor_data" + assert parameters["detector"]["decimate"] == 1.5 + all_ids.update(ids) + assert all_ids == set(range(11)) + assert tags[ + f"/{model}_calibration/side/apriltag/apriltag" + ]["ros__parameters"]["tag"]["frames"][0] == "side_base" + + +def test_three_camera_launch_selects_o30_tag_parameters() -> None: + launch_text = ( + PACKAGE_ROOT / "launch" / "three_camera_calibration.launch.py" + ).read_text() + + assert 'tag_config = LaunchConfiguration("o30_tag_config")' in launch_text + assert '"three_camera_tags_o30.yaml"' in launch_text def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text() ) - parameters = config["g20_calibration"]["ros__parameters"] + parameters = config["/**"]["ros__parameters"] assert parameters["tag_size_m"] == 0.016 @@ -114,6 +129,8 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["normal_calibration_speed"] == 15 assert parameters["index_roll_calibration_speed"] == 5 assert parameters["index_flex_calibration_speed"] == 10 + assert parameters["o30_internal_speed_u8"] == 0 + assert parameters["o30_command_full_range_seconds"] == 6.0 assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0 assert parameters["speed_setting_settle_seconds"] >= 0.2 assert parameters["top_pnp_invalid_reset_seconds"] == 1.0 @@ -126,6 +143,14 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["minimum_detection_hz"] == 15.0 assert parameters["minimum_state_span_u8"] >= 240.0 assert parameters["endpoint_tolerance_u8"] == 2.0 + assert parameters["o30_endpoint_tolerance_u8"] == 4.0 + assert ( + parameters["o30_thumb_cmc_roll_255_endpoint_tolerance_u8"] == 9.0 + ) + assert ( + parameters["o30_index_mcp_roll_255_endpoint_tolerance_u8"] == 8.0 + ) + assert parameters["o30_thumb_mcp_zero_endpoint_tolerance_u8"] == 8.0 assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0 assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0 assert parameters["minimum_sweep_bins"] >= 32 diff --git a/src/g20_thumb_apriltag_calibration/test/test_o30_support.py b/src/g20_thumb_apriltag_calibration/test/test_o30_support.py new file mode 100644 index 0000000..26e4fa7 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_o30_support.py @@ -0,0 +1,263 @@ +import math +from pathlib import Path + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from g20_thumb_apriltag_calibration.full_hand import ( + O30_COMMAND_NAMES, + O30_RIGHT_BASELINE_COMMAND, + O30_RIGHT_HAND_PROFILE, + JointCurveFit, + build_calibration_motion_command, + build_calibration_speed_profile, + build_compact_payload, + get_hand_calibration_profile, + validate_compact_payload, +) +from g20_thumb_apriltag_calibration.urdf_zero import ( + JointAxisMeasurement, + UrdfKinematicModel, + _angles_from_state, + get_zero_calibration_profile, + solve_urdf_zero_offsets, + write_zero_corrected_urdf, +) + + +O30_SOURCE_URDF = Path( + "/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/" + "linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf" +) + + +def _curve(zero_command: int, travel_rad: float = 0.8) -> JointCurveFit: + values = np.asarray( + [travel_rad * (command - zero_command) / 255.0 for command in range(256)] + ) + 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 test_o30_right_profile_matches_sdk_and_requested_baseline() -> None: + profile = get_hand_calibration_profile("right", "O30") + + assert profile is O30_RIGHT_HAND_PROFILE + assert profile.command_names == O30_COMMAND_NAMES + assert profile.baseline_command == ( + 0, 0, 255, 205, 165, 20, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + ) + assert O30_RIGHT_BASELINE_COMMAND == profile.baseline_command + assert len(profile.active_joints) == 20 + assert profile.passive_joints == () + assert set(profile.measured_joints) == { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_mcp", + "thumb_ip", + "pinky_mcp_roll", + "pinky_mcp_pitch", + "pinky_pip", + "pinky_dip", + } + assert [spec.motor_index for spec in profile.sweep_specs] == [ + 0, 6, 15, 5, 10, 14, 19, 1, + ] + assert profile.joint_specs["index_mcp_roll"].motor_index == 2 + assert profile.joint_specs["pinky_mcp_roll"].motor_index == 5 + assert profile.joint_specs["thumb_mcp"].motor_index == 6 + assert profile.joint_specs["pinky_mcp_pitch"].motor_index == 10 + assert profile.joint_specs["pinky_pip"].motor_index == 14 + assert profile.joint_specs["thumb_ip"].motor_index == 15 + assert profile.joint_specs["pinky_dip"].motor_index == 19 + + with pytest.raises(ValueError, match="only the right hand"): + get_hand_calibration_profile("left", "O30") + + +def test_o30_pinky_roll_clearance_and_sdk_speed_broadcast() -> None: + profile = O30_RIGHT_HAND_PROFILE + roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 5) + command = build_calibration_motion_command(roll, 100, profile=profile) + + assert command[0:6] == [0, 0, 255, 255, 255, 100] + assert command[6:] == [0] * 14 + assert build_calibration_speed_profile( + roll, + normal_speed=15, + index_roll_speed=5, + index_flex_speed=10, + profile=profile, + ) == [5] * 5 + + dip = next(spec for spec in profile.sweep_specs if spec.motor_index == 19) + assert build_calibration_speed_profile( + dip, + normal_speed=15, + index_roll_speed=5, + index_flex_speed=10, + profile=profile, + ) == [10] * 5 + + +def test_o30_static_zero_policy_and_compact_payload() -> None: + profile = O30_RIGHT_HAND_PROFILE + zero = get_zero_calibration_profile("right", "O30") + + assert set(zero.fixed_direct_zero_offsets_rad) == { + "thumb_ip", + "pinky_mcp_roll", + "pinky_dip", + } + assert "pinky_mcp_pitch" not in zero.fixed_direct_zero_offsets_rad + assert "pinky_pip" not in zero.fixed_direct_zero_offsets_rad + assert zero.inherited_static_zero_joints == {} + assert zero.inherited_zero_joints["index_dip"] == "pinky_dip" + + measured = { + name: _curve( + profile.baseline_command[profile.joint_specs[name].motor_index] + ) + for name in profile.measured_joints + } + offsets = {name: 0.0 for name in profile.active_joints} + offsets["pinky_mcp_pitch"] = 0.01 + offsets["pinky_pip"] = -0.02 + payload = build_compact_payload( + serial_number="O30_RIGHT_TEST", + measured_fits=measured, + urdf_zero_offsets_rad=offsets, + validation_errors_rad=[0.001], + passed=True, + side="right", + model="O30", + ) + + validate_compact_payload(payload) + assert payload["model"] == "O30" + assert len(payload["joints"]) == 20 + for name in ( + "index_mcp_roll", + "middle_mcp_roll", + "ring_mcp_roll", + "pinky_mcp_roll", + ): + joint = payload["joints"][name] + assert joint["angle_rad"][joint["zero_command_u8"]] == pytest.approx(0.0) + assert joint["zero_angles"]["urdf_zero_offset_rad"] == 0.0 + assert payload["joints"]["pinky_mcp_pitch"]["zero_angles"] == { + "urdf_zero_offset_rad": 0.01 + } + assert payload["joints"]["index_mcp_pitch"]["zero_angles"] == { + "urdf_zero_offset_rad": 0.0 + } + + +@pytest.mark.skipif(not O30_SOURCE_URDF.is_file(), reason="O30 source URDF absent") +def test_o30_source_urdf_has_exact_active_joint_set_and_writer(tmp_path) -> None: + profile = O30_RIGHT_HAND_PROFILE + source_bytes = O30_SOURCE_URDF.read_bytes() + model = UrdfKinematicModel(O30_SOURCE_URDF) + + assert set(model.joints) == set(profile.active_joints) + assert "thumb_cmc_pitch" not in model.joints + destination = write_zero_corrected_urdf( + source_urdf=O30_SOURCE_URDF, + output_directory=tmp_path, + serial_number="O30_RIGHT_TEST", + offsets_rad={name: 0.0 for name in profile.active_joints}, + timestamp="20260814_120000", + ) + assert destination.is_file() + assert O30_SOURCE_URDF.read_bytes() == source_bytes + assert set(UrdfKinematicModel(destination).joints) == set(profile.active_joints) + + +@pytest.mark.skipif(not O30_SOURCE_URDF.is_file(), reason="O30 source URDF absent") +def test_o30_zero_solver_recovers_only_observable_offsets() -> None: + profile = O30_RIGHT_HAND_PROFILE + zero = get_zero_calibration_profile("right", "O30") + curves = { + name: _curve( + profile.baseline_command[profile.joint_specs[name].motor_index], + math.radians(55.0), + ) + for name in profile.measured_joints + } + motor_by_joint = { + name: spec.motor_index for name, spec in profile.joint_specs.items() + } + expected_degrees = { + "thumb_cmc_roll": 2.0, + "thumb_cmc_yaw": -3.0, + "thumb_mcp": 4.0, + "thumb_ip": 0.0, + "pinky_mcp_roll": 0.0, + "pinky_mcp_pitch": 1.2, + "pinky_pip": -1.0, + "pinky_dip": 0.0, + } + offsets = { + name: math.radians(expected_degrees[name]) + for name in zero.direct_zero_joints + } + model = UrdfKinematicModel(O30_SOURCE_URDF) + base_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) + base_translation = np.asarray([0.25, -0.12, 0.68]) + measurements = [] + for cycle in range(3): + for joint in zero.axis_joints: + state = tuple(float(value) for value in profile.baseline_command) + 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, + ) + 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=state, + plane_rms_m=0.0002, + radial_rms_m=0.0002, + rotation_circle_axis_difference_rad=math.radians(0.1), + pose_axis_line_rms_m=0.0001, + ) + ) + + result = solve_urdf_zero_offsets( + source_urdf=O30_SOURCE_URDF, + measurements=measurements, + curves=curves, + motor_by_joint=motor_by_joint, + hand_type="right", + hand_model="O30", + ) + + assert result.passed is True + for name, expected in expected_degrees.items(): + assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( + expected, abs=0.05 + ) + assert result.all_active_offsets_rad["index_mcp_roll"] == 0.0 + assert result.all_active_offsets_rad["index_mcp_pitch"] == 0.0 diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py index a956536..d137335 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py @@ -192,6 +192,39 @@ def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None: assert "整关节自动重采第2/3次" in _task_text(active) +def test_o30_status_shows_cycle_order_retry_execution_and_namespace() -> None: + payload = { + "state": "PAUSED", + "reason": "joint_fit_check_failed", + "progress": 0.112, + "scan_progress": 0.125, + "completed_sweeps": 6, + "total_sweeps": 48, + "executed_sweep_directions": 20, + "service_prefix": "/o30_calibration", + "active": { + "kind": "fit_failure", + "view": "front", + "motor_index": 0, + "joints": ["thumb_cmc_roll"], + "attempt": 3, + "fit_attempt_limit": 3, + "directions_to_rescan": 6, + "failures": [], + }, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "已启动20个方向（含自动重扫）" in text + assert "重扫不会重复增加计划进度" in text + assert "当前为第3/3次采集结果" in text + assert "/o30_calibration/resume" in text + assert "/g20_calibration/resume" not in text + + def test_motor_stall_reason_is_explained_in_chinese() -> None: reason, action = three_camera_reason_zh( "PAUSED", diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py index 72cf3d9..e01081c 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py @@ -1,9 +1,11 @@ import json import math +import time from dataclasses import replace from types import SimpleNamespace import numpy as np +from builtin_interfaces.msg import Time from scipy.spatial.transform import Rotation from g20_thumb_apriltag_calibration.core import ( @@ -11,6 +13,7 @@ from g20_thumb_apriltag_calibration.core import ( DIRECTION_INCREASING, ) from g20_thumb_apriltag_calibration.full_hand import ( + O30_RIGHT_HAND_PROFILE, RIGHT_HAND_PROFILE, SWEEP_SPECS, ) @@ -20,7 +23,9 @@ from g20_thumb_apriltag_calibration.three_camera_node import ( STATE_PREPARE_SWEEP, STATE_SWEEP, SweepItem, + _cycle_sequence_u8, _overall_progress, + _sweep_direction_order, ) @@ -49,6 +54,19 @@ def _frame(view: str, state_u8: list[float]) -> FrameObservation: ) +def test_o30_cycle_order_is_zero_to_255_and_back_without_changing_g20() -> None: + assert _sweep_direction_order(O30_RIGHT_HAND_PROFILE) == ( + DIRECTION_INCREASING, + DIRECTION_DECREASING, + ) + assert _cycle_sequence_u8(O30_RIGHT_HAND_PROFILE) == (0, 255, 0) + assert _sweep_direction_order(RIGHT_HAND_PROFILE) == ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + assert _cycle_sequence_u8(RIGHT_HAND_PROFILE) == (255, 0, 255) + + def test_right_thumb_pitch_requires_only_front_base_and_thumb_tag() -> None: profile = RIGHT_HAND_PROFILE spec = next(item for item in profile.sweep_specs if item.motor_index == 0) @@ -223,6 +241,185 @@ def test_joint_specific_zero_endpoint_deadbands() -> None: ) == 2.0 +def test_o30_endpoint_deadbands_are_model_specific() -> None: + thumb_mcp = next( + item for item in O30_RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 6 + ) + node = SimpleNamespace( + profile=O30_RIGHT_HAND_PROFILE, + endpoint_tolerance_u8=2.0, + o30_endpoint_tolerance_u8=4.0, + o30_thumb_cmc_roll_255_endpoint_tolerance_u8=9.0, + o30_index_mcp_roll_255_endpoint_tolerance_u8=8.0, + o30_thumb_mcp_zero_endpoint_tolerance_u8=8.0, + ) + + assert G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + node, 0, 255 + ) == 9.0 + assert G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + node, 0, 0 + ) == 4.0 + assert G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + node, 2, 255 + ) == 8.0 + assert G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + node, 2, 0 + ) == 4.0 + assert G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + node, 3, 205 + ) == 4.0 + assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, thumb_mcp, 0 + ) == 8.0 + assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, thumb_mcp, 255 + ) == 4.0 + + node.profile = RIGHT_HAND_PROFILE + assert G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + node, 3, 205 + ) == 2.0 + + +def test_o30_motion_profile_configures_sdk_ramp_and_minimum_speed() -> None: + messages: list[object] = [] + node = SimpleNamespace( + commands_enabled=True, + profile=O30_RIGHT_HAND_PROFILE, + hand_type="right", + commanded_speed_profile=(), + o30_position_mode_commanded=False, + o30_position_ramp_commanded=False, + o30_command_full_range_seconds=6.0, + speed_commanded_at=0.0, + setting_publisher=SimpleNamespace(publish=messages.append), + pending_command_u8=None, + ) + + G20ThreeCameraCalibrationNode._publish_speed_profile(node, [0] * 5) + + payloads = [json.loads(message.data) for message in messages] + assert [payload["setting_cmd"] for payload in payloads] == [ + "clear_move_time", + "set_position_ramp", + "set_speed", + ] + assert payloads[1]["params"]["full_range_seconds"] == 6.0 + assert payloads[2]["params"]["speed"] == [0] * 5 + assert node.o30_position_mode_commanded + assert node.o30_position_ramp_commanded + + G20ThreeCameraCalibrationNode._publish_speed_profile(node, [0] * 5) + assert len(messages) == 3 + + G20ThreeCameraCalibrationNode._restore_o30_position_mode(node) + restored = [json.loads(message.data)["setting_cmd"] for message in messages[-2:]] + assert restored == ["clear_position_ramp", "clear_move_time"] + assert not node.o30_position_ramp_commanded + + +def test_o30_position_command_waits_then_publishes_final_sdk_target() -> None: + messages: list[object] = [] + now = time.monotonic() + baseline = list(O30_RIGHT_HAND_PROFILE.baseline_command) + node = SimpleNamespace( + commands_enabled=True, + profile=O30_RIGHT_HAND_PROFILE, + speed_commanded_at=now, + speed_setting_settle_seconds=0.25, + pending_command_u8=None, + pending_command_not_before=0.0, + command_publisher=SimpleNamespace(publish=messages.append), + get_clock=lambda: SimpleNamespace( + now=lambda: SimpleNamespace(to_msg=lambda: Time()) + ), + ) + command = list(baseline) + command[0] = 255 + + G20ThreeCameraCalibrationNode._publish_command(node, command) + assert messages == [] + assert node.pending_command_u8 == tuple(command) + + G20ThreeCameraCalibrationNode._flush_pending_command(node, now + 0.1) + assert messages == [] + G20ThreeCameraCalibrationNode._flush_pending_command(node, now + 0.3) + assert len(messages) == 1 + assert list(messages[-1].position) == [float(value) for value in command] + + +def test_o30_baseline_accepts_observed_firmware_feedback() -> None: + baseline = list(O30_RIGHT_HAND_PROFILE.baseline_command) + state = [ + 0.0, 0.0, 248.0, 204.0, 161.0, 24.0, 7.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 3.0, 0.0, + ] + node = SimpleNamespace( + profile=O30_RIGHT_HAND_PROFILE, + baseline_command=tuple(baseline), + latest_state_u8=tuple(state), + endpoint_tolerance_u8=2.0, + o30_endpoint_tolerance_u8=4.0, + o30_index_mcp_roll_255_endpoint_tolerance_u8=8.0, + o30_thumb_mcp_zero_endpoint_tolerance_u8=8.0, + ) + + assert G20ThreeCameraCalibrationNode._baseline_reached(node) + details = G20ThreeCameraCalibrationNode._baseline_error_details(node) + assert details["motor_index"] == 4 + assert details["actual_u8"] == 161.0 + assert details["tolerance_u8"] == 4.0 + + state[4] = 160.0 + node.latest_state_u8 = tuple(state) + assert not G20ThreeCameraCalibrationNode._baseline_reached(node) + + state[4] = 161.0 + state[6] = 9.0 + node.latest_state_u8 = tuple(state) + assert not G20ThreeCameraCalibrationNode._baseline_reached(node) + + state[6] = 7.0 + state[2] = 246.0 + node.latest_state_u8 = tuple(state) + assert not G20ThreeCameraCalibrationNode._baseline_reached(node) + + +def test_o30_thumb_roll_255_accepts_observed_firmware_feedback() -> None: + thumb_roll = next( + item for item in O30_RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 0 + ) + state = list(O30_RIGHT_HAND_PROFILE.baseline_command) + state[0] = 247 + node = SimpleNamespace( + profile=O30_RIGHT_HAND_PROFILE, + latest_state_u8=tuple(float(value) for value in state), + baseline_command=O30_RIGHT_HAND_PROFILE.baseline_command, + endpoint_tolerance_u8=2.0, + o30_endpoint_tolerance_u8=4.0, + o30_thumb_cmc_roll_255_endpoint_tolerance_u8=9.0, + o30_index_mcp_roll_255_endpoint_tolerance_u8=8.0, + o30_thumb_mcp_zero_endpoint_tolerance_u8=8.0, + ) + node._endpoint_tolerance_for_spec = lambda spec, endpoint: ( + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, spec, endpoint + ) + ) + + assert G20ThreeCameraCalibrationNode._motion_command_reached( + node, thumb_roll, 255 + ) + state[0] = 245 + node.latest_state_u8 = tuple(float(value) for value in state) + assert not G20ThreeCameraCalibrationNode._motion_command_reached( + node, thumb_roll, 255 + ) + + def test_pinky_pip_command_zero_accepts_firmware_feedback_five() -> None: pinky_pip = next( item for item in RIGHT_HAND_PROFILE.sweep_specs @@ -730,6 +927,36 @@ def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> assert [event["retry"] for event in events] == [1, 2, 3] +def test_direction_retry_inside_fit_retry_requeues_same_direction(tmp_path) -> None: + spec = O30_RIGHT_HAND_PROFILE.sweep_specs[0] + failed = SweepItem(spec, 2, DIRECTION_DECREASING) + following = SweepItem(spec, 2, DIRECTION_INCREASING) + transitions: list[str] = [] + node = SimpleNamespace( + active_sweep=failed, + active_sweep_is_fit_retry=True, + retry_sweep_items=[following], + automatic_sweep_retry_limit=3, + sweep_retry_counts={}, + raw_path=tmp_path / "raw_samples.jsonl", + reason="", + views={}, + sweep_frames=[], + sweep_start_frames=[], + retry_speed_scales=(0.8, 0.6, 0.5), + retry_endpoint_hold_seconds=(0.75, 1.0, 1.25), + _begin_return_baseline=lambda after: transitions.append(after), + _pause=lambda reason: None, + ) + + G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( + node, "sweep_bin_gap_too_large" + ) + + assert node.retry_sweep_items == [failed, following] + assert transitions == ["retry_sweep"] + + def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) transitions: list[str] = []