G20四指单独标定(少末端tag)
This commit is contained in:
+3
-1
@@ -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
|
||||
@@ -92,3 +92,5 @@ candump-*
|
||||
# Local Codex/agent workspace metadata
|
||||
/.agents/
|
||||
/.codex/
|
||||
/.codebuddy/
|
||||
/.zcode/
|
||||
|
||||
@@ -1,5 +1,136 @@
|
||||
# G20 左右手 AprilTag 标定
|
||||
|
||||
## G20右手正式一键标定
|
||||
|
||||
固定三相机和15张Tag安装完成后,用户只运行:
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
|
||||
```
|
||||
|
||||
开发阶段若上一次会话失败,同一命令会自动校验硬件/几何哈希,并恢复已经
|
||||
完整提交的关节任务;失败中的当前任务始终丢弃重做,位于它后面但已经完整通过的
|
||||
独立任务仍会复用,不再因“连续前缀”限制整段重采。导入的任务会立即用与
|
||||
最终验收相同的硬门限复检(不含视口实时有效率):只以预警带余量通过的旧数据
|
||||
当场剔除并从其在扫掠顺序中的原始位置重采,避免全部任务采完后才在最终验收
|
||||
失败、把会话拉回靠前的关节。需要强制从第一个关节
|
||||
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
|
||||
一个完整同步任务断点;只有两边数据都完整时才复用。
|
||||
|
||||
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
|
||||
11个可求解主动关节URDF零位修正(`thumb_mcp`固定CAD零位)、
|
||||
17条实测命令曲线以及4条
|
||||
源URDF mimic派生DIP曲线发布。
|
||||
终端只显示中文进度和问题;失败时复制“请复制以下内容给开发者”块即可。
|
||||
|
||||
正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
|
||||
JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
|
||||
全部通过后更新。
|
||||
|
||||
## G20右手15-Tag底层调试入口
|
||||
|
||||
以下内容仅保留给旧会话回放和开发调试;正式一键命令只发布上面的精简
|
||||
schema v4 JSON,不再生成schema v5运行文件。
|
||||
|
||||
新布局用独立参数启用,原有左右手11-Tag流程仍默认使用
|
||||
`tag_layout:=legacy_11`,两套配置和结果schema互不覆盖:
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
|
||||
hand_type:=right \
|
||||
tag_layout:=g20_right_15 \
|
||||
serial_number:=G20_RIGHT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_expected_sha256:=<CAD负责人确认的G20右手源URDF_SHA256>
|
||||
```
|
||||
|
||||
15张Tag均为16 mm `tag36h11`。正面为
|
||||
`0,1,2,3,10,11,12,13`,侧面为
|
||||
`4,5,6,15,17`,上面为`8,9`;ID `7,14,16,18`不再粘贴。详细角色以
|
||||
`config/three_camera_tags_g20_right_15.yaml`为唯一软件配置源。程序执行4项拇指任务,
|
||||
以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、
|
||||
侧面PIP,共16个物理运动任务。同步roll只驱动电机一次,但两台相机仍分别拟合并通过
|
||||
各自的观测质量门限。四个DIP动态曲线由同次PIP实测曲线乘源URDF中受保护的mimic
|
||||
multiplier生成,不参与视觉拟合或视觉质量门限。每项正式四轮之前自动低速往返预检
|
||||
0/127/255可见性;
|
||||
|
||||
基准形态恢复完成后,程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
|
||||
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0,因此四指正面+侧面同步roll中
|
||||
允许ID 0暂时不可见,并使用本会话基准锁定值;运动连杆Tag仍必须实时可见,门限不
|
||||
放宽。终端用`锁`表示该固定参考有效,例如`正面[0锁,12✓]`,`✗`才表示需要处理的
|
||||
实时Tag。锁定后不得移动相机、手掌底座或整只手,否则缓存参考失效,必须重新启动
|
||||
标定。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计;
|
||||
任务结束后角色要求会切回预检全套标签,静止期帧不参与该门限,避免把
|
||||
采集质量良好的任务误判为可见性失败。
|
||||
|
||||
取消末端Tag后,四个PIP缺少下游DIP轴作为任意粘贴Tag条件下的绝对零位观察基准;
|
||||
因此PIP动态曲线仍为视觉实测,但PIP的URDF静态零位保留源CAD,不猜测写入。
|
||||
正式修正范围为拇指4个主动关节,以及四指各自的MCP roll/MCP pitch,共12个。
|
||||
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入,并按逆序分阶段退出;
|
||||
同类辅助电机(全部邻指滚转、全部PIP、全部MCP pitch)合并为同一个并行航点同时
|
||||
运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
||||
等已评审不变量保持不变,过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
|
||||
`parallel_pose_transitions`(默认true)置false可回退旧的逐电机顺序。
|
||||
同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行,只在任务切换时退出,
|
||||
不再每轮重复展开/弯曲辅助手指。跨手指组切换时,下一组避障姿态仍然需要、且
|
||||
当前已经在位(含反馈容差)的辅助电机保持原位,只有下一组不再使用的避障电机
|
||||
退回基准,避免"先展开回基准、马上又折回"的多余动作;已评审的
|
||||
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
|
||||
不超过8,会把非roll任务四轮正式速度最多提高到预检速度的1.5倍;否则保持原保守速度。
|
||||
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态:以`255→127`为标准物理
|
||||
零位,反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
||||
四轮间隙极差上限0.3°;其他关节仍使用严格的0.5°baseline回差门限。
|
||||
预检、正式四轮和拟合重扫始终使用速度5。
|
||||
正式roll的每个方向会在经过127时先到位保持0.5秒,再独立保存至少10帧静止Tag/反馈;
|
||||
方向分支检查和动态曲线的127相位都使用这两组双向静止数据,运动中经过127的帧不再
|
||||
替代静态保持姿态。
|
||||
前三轮只用于训练,第四轮完全留出;留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
||||
每轮PnP都清空帧间跟踪状态并重新执行8帧静态初始化,但同一任务第1轮
|
||||
已确立的端点相对姿态作为后3轮的分支锚点,防止独立初始化选到相反的
|
||||
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
||||
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`,只在逐帧IPPE双解中
|
||||
排除与MCP同步运动明显矛盾(残差超过7.5°)的ID3镜像候选。该先验不生成或缩放
|
||||
`thumb_ip`曲线;通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。
|
||||
|
||||
当前15-Tag产品流程发布精简schema v4:21条运行时曲线中,17条来自当前会话的
|
||||
视觉实测,四指DIP按源URDF受保护的mimic multiplier由同指PIP曲线派生。
|
||||
URDF零位字段只覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch`,共12个,
|
||||
其中`thumb_mcp`字段固定为0而不会修改源URDF;
|
||||
四指PIP、`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容,
|
||||
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合;方向、
|
||||
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
|
||||
相对侧相机视线倾斜约13°~20°,平面标签的单目IPPE姿态二义性会给侧视姿态引入
|
||||
数度的系统性"绕视线"偏差(亚像素重投影无法发现,会话20260820_105535实测
|
||||
前后轴向稳定相差11.4°),因此跨视角方向差超过0.75°融合门限时不再判定任务
|
||||
失败:程序记录`cross_view_roll_axis_diagnostic`诊断、跳过融合并采用可信的
|
||||
前视轴向;仅当差值超过粗错误兜底上限(`cross_view_roll_maximum_axis_difference_deg`
|
||||
默认15°或线距超过`cross_view_roll_maximum_axis_line_difference_mm`默认30mm,
|
||||
对应标签贴错连杆或标签松动)时才失败。前视侧摆连杆与侧视PIP连杆的轴线
|
||||
本身存在约21mm的系统性位置差(丝杠平移连杆),属预期现象。侧视校验通道
|
||||
(`*_mcp_roll_side`)的
|
||||
分支间隙跨轮极差上限放宽为
|
||||
`cross_view_roll_alias_maximum_branch_gap_range_deg`默认0.5°(绝对间隙1.5°上限
|
||||
不变),其`axis_pose_line_rms`降为诊断,不再作为准入门限;径向、平面、
|
||||
圆一致性等其余数据质量门限全部保留。侧面端视roll的圆轨迹方向已经受
|
||||
姿态轴约束,因此自由三维圆平面与姿态轴的夹角只保留诊断,不再被重复作为硬门限;
|
||||
径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、
|
||||
遮挡、PnP或跨机位检查失败时,只保留原始轨迹和`passed:false`诊断,不发布正式URDF。
|
||||
8个组合姿态仅保留为开发诊断,正式产品默认不执行。15-Tag布局的侧面每根手指只有一张
|
||||
PIP Tag,并不存在可独立验证的DIP Tag;同时轴线零位求解不提供适合绝对笛卡尔位置验收的
|
||||
手基座变换,因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
|
||||
`a609d521`验证过的完整四轮相对旋转曲线;全部实测关节均由隔离第四轮逐关节验收。
|
||||
现场需要区分某根手指的roll机构回差与单机位误差时,可设置
|
||||
`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次
|
||||
正面+侧面同步roll,共10个预检/正式方向;任一机位数据不足会重扫同一物理任务,
|
||||
双机位数据齐全后即使存在轴质量失败也不再自动重采,而是把失败项随双机位结果
|
||||
一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久
|
||||
锁定URDF发布,不能用`resume`转换成正式标定。
|
||||
schema v5明确声明曲线输入域为真实反馈u8;运行桥默认订阅
|
||||
`/g20/cb_right_hand_state`,并按反馈增减方向选择正程/反程曲线,停止时锁存最后运动
|
||||
方向。尚未观察到运动方向时使用`255→127`标准分支,不使用两个机械分支的平均值。
|
||||
schema v4继续兼容旧
|
||||
命令域。两者都只发布动态角度,不重复叠加已写入URDF的静态偏移。
|
||||
|
||||
## 三机位三维关节轴零位标定(schema v4)
|
||||
|
||||
正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机,只有
|
||||
@@ -177,7 +308,7 @@ Tag位置,接近轴向观察时改用相机图像平面相位并丢弃单目Pn
|
||||
|
||||
前两轮拟合,第三轮强制留出验证;轨迹与零位角度MAE必须≤1°、P95≤2°,三轮轴/零位
|
||||
差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF,并通过按三轮分组的
|
||||
95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
|
||||
训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。
|
||||
|
||||
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定:主动关节上限2.5°,被动
|
||||
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差,
|
||||
@@ -209,18 +340,22 @@ Tag位置,接近轴向观察时改用相机图像平面相位并丢弃单目Pn
|
||||
|
||||
生成修正URDF时只修改通过验收的主动关节 `origin.rpy`,不会修改任何关节的
|
||||
`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
|
||||
JSON;实测曲线即使略微越过CAD限位,也不能自动扩大URDF限位。
|
||||
JSON;任一曲线点越过CAD限位都会阻止正式发布,程序不会自动扩大URDF限位。
|
||||
|
||||
坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
|
||||
重置当前机位PnP、返回基准后重扫当前方向,最多3次;速度依次降为80%/60%/50%,
|
||||
端点保持延长到0.75/1.0/1.25秒,扫描超时按降速比例同步延长。若反馈在远离目标时
|
||||
连续8秒没有至少1个u8的进展,则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
|
||||
暂停,不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向,全局不一致才重扫
|
||||
完整关节,每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警,最终
|
||||
拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败,程序
|
||||
完整关节,每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色
|
||||
预警,但只要仍超过硬门限,就在当前关节立即使用剩余重试预算
|
||||
(`provisional_fit_warning_rescan`);第三次仍超限则当场暂停,不允许预警数据继续到
|
||||
后续关节。最终拟合仍按原硬门限验收,因此不会在全部任务采完后才回头重采靠前
|
||||
关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败,程序
|
||||
只暂停一次且不再自动重扫,防止重复运动;此时也拒绝`resume`形成死循环。其他可恢复
|
||||
失败在预算耗尽后才暂停,`resume`从最小失败单元继续,已通过数据保留。所有失败尝试
|
||||
仍保存在 `raw_samples.jsonl`。
|
||||
仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的
|
||||
PnP双簇行程,程序将它判为系统性分支失败并当场停止,不再浪费第3次全关节重扫。
|
||||
|
||||
每个新机位/Tag组合开始运动前,不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
|
||||
程序在静止端点联合8帧候选,按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
|
||||
@@ -273,20 +408,23 @@ ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试
|
||||
```text
|
||||
calibration_output/G20_LEFT_001/<时间戳>/
|
||||
g20_left_G20_LEFT_001_calibration.json
|
||||
src/.../g20_left/
|
||||
linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
|
||||
meshes/*.STL
|
||||
|
||||
calibration_output/G20_RIGHT_001/<时间戳>/
|
||||
g20_right_G20_RIGHT_001_calibration.json
|
||||
src/.../g20_right/
|
||||
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
|
||||
meshes/*.STL
|
||||
```
|
||||
|
||||
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
|
||||
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
|
||||
指定原始CAD文件生成,采用 `T_original × Rot(axis, offset)`,绝不叠加旧校准文件或
|
||||
覆盖原文件;只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
|
||||
改变,未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
|
||||
改变;当前15-Tag右手保留12个主动静态零位字段,其中11个由数据求解,`thumb_mcp`
|
||||
固定为原始CAD零位0;四指PIP虽保留字段但数值固定为0。
|
||||
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
|
||||
同一会话,保证会话内URDF可独立加载,并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
|
||||
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
|
||||
|
||||
完整 `raw_samples.jsonl` 已存在时,可以按当前算法离线重放,不连接相机、不发送电机
|
||||
@@ -315,7 +453,8 @@ ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.
|
||||
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
|
||||
```
|
||||
|
||||
默认订阅 `/cb_right_hand_control_cmd`,发布
|
||||
schema v5默认订阅 `/g20/cb_right_hand_state`;schema v4默认订阅
|
||||
`/g20/cb_right_hand_control_cmd`。两者均发布
|
||||
`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥,避免两个
|
||||
发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令,
|
||||
因此不会静默退回旧标定。
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
schema_version: 1
|
||||
model: G20
|
||||
side: right
|
||||
serial_number: G20_RIGHT_001
|
||||
can_interface: can0
|
||||
output_root: calibration_output
|
||||
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
camera_name: hikrobot_front_DB2163742
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
camera_name: hikrobot_side_DB2163749
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
camera_name: hikrobot_top_DB2163739
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||
|
||||
artifacts:
|
||||
source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
|
||||
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: aa0a1498a210ef36f20d59bd4fdc612a01fd09eb5a2bc1c0b8d84c05a44d5c5a
|
||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
||||
calibration_config_sha256: 80758d0a240a7dea1426d4fad688b09eac8360560a1d5870d709b7bb103d3280
|
||||
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml
|
||||
tag_config_sha256: 7ce50be80dfccb3529c34d5bd601090b9d378e2bf6abcef2d5b93dc6a4da99fd
|
||||
|
||||
release:
|
||||
# Each task already contains three training cycles plus an isolated fourth
|
||||
# holdout, so a second complete hardware session duplicates hours of motion.
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -16,14 +16,24 @@ g20_calibration:
|
||||
normal_calibration_speed: 15
|
||||
index_roll_calibration_speed: 5
|
||||
index_flex_calibration_speed: 10
|
||||
# 15-Tag产品预检仍使用上面保守速度;只有正反预检都留出至少双倍正式分箱余量,
|
||||
# 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束,
|
||||
# 始终保持速度5;任一方向采样余量不足也保持原速度。
|
||||
adaptive_formal_speed_enabled: true
|
||||
adaptive_formal_speed_max_scale: 1.5
|
||||
adaptive_formal_speed_minimum_bins: 64
|
||||
adaptive_formal_speed_maximum_bin_gap: 8
|
||||
speed_setting_settle_seconds: 0.25
|
||||
|
||||
# tag36h11检测角点围成的黑色正方形实测为16 mm;不包含外围白边。
|
||||
tag_size_m: 0.016
|
||||
repetitions: 3
|
||||
# 15-Tag产品正式零位使用前三轮训练、最后一轮完全留出;旧11-Tag仍读取repetitions=3。
|
||||
g20_right_19_repetitions: 4
|
||||
preflight_frames: 60
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_detection_hz: 15.0
|
||||
minimum_feedback_hz: 25.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
@@ -36,9 +46,15 @@ g20_calibration:
|
||||
pnp_tracker_reset_seconds: 5.0
|
||||
# 标定任务不再用第一帧决定平面Tag的IPPE分支;静止端点联合8帧选择整组最稳定解。
|
||||
pnp_group_initialization_frames: 8
|
||||
# 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致;用此先验消除静态IPPE镜像双解。
|
||||
# 侧面当前任务所需Tag在初始化端点的贴面法向应一致;用此先验消除静态IPPE镜像双解。
|
||||
pnp_group_normal_alignment_scale_deg: 5.0
|
||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时,用源URDF mimic
|
||||
# 关系辅助选择IPPE分支;若全部候选超限则退回纯视觉,绝不丢帧,也不生成、
|
||||
# 缩放或替代被动IP的自身Tag实测曲线。
|
||||
thumb_ip_pnp_coupling_multiplier: 1.03
|
||||
thumb_ip_pnp_coupling_scale_deg: 3.0
|
||||
thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
|
||||
top_pnp_invalid_reset_seconds: 1.0
|
||||
# 三维位姿必须与实测20通道状态严格按时间戳配对。
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
@@ -54,17 +70,22 @@ g20_calibration:
|
||||
# 约束三维圆,不让单目平面Tag的深度噪声自由决定转轴方向。
|
||||
axis_maximum_rotation_circle_difference_deg: 1.0
|
||||
# 单轴模型残差与跨轮重复误差分开判定:主动刚性关节要求更严;被动耦合
|
||||
# 关节允许可重复的非理想单轴分量,但仍须通过0.75°跨轮轴差及第三轮留出。
|
||||
# 关节允许可重复的非理想单轴分量,但仍须通过0.75°跨轮轴差及最终轮留出。
|
||||
active_maximum_rotation_orthogonal_rms_deg: 2.5
|
||||
passive_maximum_rotation_orthogonal_rms_deg: 7.5
|
||||
zero_maximum_axis_cycle_difference_deg: 0.75
|
||||
# 零位无法改变父子轴夹角;超过该值属于CAD/PnP几何错误,不能吸收到零位。
|
||||
zero_maximum_axis_cone_mismatch_deg: 5.0
|
||||
zero_maximum_observability_condition_number: 10000000000.0
|
||||
zero_maximum_offset_deg: 20.0
|
||||
# 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线,静态零位固定为CAD 0。
|
||||
# 四指MCP roll/pitch绝对静态零偏的保护范围;PIP仍保留CAD静态零位。
|
||||
zero_finger_maximum_offset_deg: 3.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区,但反馈
|
||||
# 必须已经稳定;大残差仍由机械卡滞保护处理。
|
||||
steady_checkpoint_command_feedback_tolerance_u8: 8.0
|
||||
steady_checkpoint_maximum_feedback_range_u8: 2.0
|
||||
# 电机10在命令0时实测会稳定反馈为4;该0端使用±4。
|
||||
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
|
||||
# 右手电机10在命令255时多次实测稳定反馈为250;仅右手该端点使用±5。
|
||||
@@ -72,12 +93,18 @@ g20_calibration:
|
||||
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5;仅其0端使用±5。
|
||||
pinky_pip_zero_endpoint_tolerance_u8: 5.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
# roll零位127必须从两个方向到位并静止采集,禁止用运动中经过127的帧判回差。
|
||||
baseline_hold_seconds: 0.5
|
||||
minimum_baseline_hold_frames: 10
|
||||
# 15-Tag产品每项正式四轮前先做一次低速往返,端点Tag稳定至少2秒。
|
||||
task_precheck_hold_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
# 反馈在远离目标时连续8秒没有至少1个u8的进展,按机械卡滞立即暂停;
|
||||
# 启动宽限1秒后,反馈连续2秒没有至少1个u8的进展,按机械卡滞立即暂停;
|
||||
# 这类故障不进入遮挡/超时的三次自动重扫。
|
||||
motor_stall_timeout_seconds: 8.0
|
||||
# 低速5也应持续产生反馈进展;5秒无进展即停,减少机构持续顶死时间。
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
motor_stall_startup_grace_seconds: 1.0
|
||||
motor_stall_minimum_progress_u8: 1.0
|
||||
invalid_timeout_seconds: 3.0
|
||||
minimum_sweep_frames: 40
|
||||
@@ -85,15 +112,18 @@ g20_calibration:
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
# 可恢复的采样失败自动重扫当前方向;超过次数才暂停等待人工处理。
|
||||
automatic_sweep_retry_limit: 3
|
||||
automatic_sweep_retry_limit: 2
|
||||
# 轨迹拟合失败优先只重扫失败轮次;零位/URDF模型失败不重复运动。
|
||||
automatic_fit_retry_limit: 2
|
||||
automatic_motion_retry_limit: 2
|
||||
# 留空为正式标定;设为pinky/ring/middle/index时只采该指正面+侧面roll,
|
||||
# 即使正面baseline回差失败也继续完成侧面对照,并永久锁定本会话URDF发布。
|
||||
cross_view_roll_diagnostic_finger: ""
|
||||
# 过程检查允许25%的黄色预警带,最终验收仍使用下面的严格门限。
|
||||
provisional_warning_ratio: 1.25
|
||||
retry_minimum_speed: 3
|
||||
retry_speed_scales: [0.8, 0.6, 0.5]
|
||||
retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
|
||||
retry_speed_scales: [0.8, 0.6]
|
||||
retry_endpoint_hold_seconds: [0.75, 1.0]
|
||||
|
||||
trajectory_maximum_plane_rms_m: 0.004
|
||||
trajectory_maximum_radial_rms_m: 0.004
|
||||
@@ -106,15 +136,40 @@ g20_calibration:
|
||||
trajectory_maximum_cycle_travel_difference_deg: 3.0
|
||||
passive_maximum_cycle_travel_difference_deg: 10.0
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
maximum_hysteresis_deg: 5.0
|
||||
# 旧布局仍用连续扫描正反程差门限;15-Tag产品的连续运动包含速度相关滞后,
|
||||
# 由方向曲线和最终留出验证建模,不再重复硬判。其绝对正反程门禁使用下面
|
||||
# 的九点稳态command_maximum_direction_gap_deg。
|
||||
maximum_hysteresis_deg: 2.0
|
||||
# 15-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
|
||||
# 不超过0.5°;四指roll例外:127以255→127为唯一物理零位,反向分支
|
||||
# 保留实测偏差,并改为检查分支间隙上限及跨轮稳定性。
|
||||
baseline_maximum_hysteresis_deg: 0.5
|
||||
directional_zero_maximum_branch_gap_deg: 2.0
|
||||
directional_zero_maximum_branch_gap_range_deg: 0.3
|
||||
cross_view_roll_maximum_branch_gap_difference_deg: 0.3
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
passive_maximum_hysteresis_deg: 7.5
|
||||
passive_maximum_hysteresis_deg: 2.0
|
||||
# 九点姿态先按实测反馈重新对齐,再用此门限检查固有机械回差;请求命令域
|
||||
# 的两条运行曲线仍原样保留固件方向死区,不能把command/feedback差算成回差。
|
||||
command_maximum_direction_gap_deg: 2.0
|
||||
|
||||
# 默认无额外随机动作;第三轮扫描始终作为不可关闭的留出验证。
|
||||
# 默认无额外随机动作;15-Tag产品最终一轮始终作为不可关闭的留出验证。
|
||||
validation_enabled: false
|
||||
# 第四轮留出求解后必须再走8个固定安全组合姿态;三机位规定Tag全部可见
|
||||
# 且实测20通道到位才允许发布。只保存Tag位姿,不保存原始图像。
|
||||
# Developer diagnostic only. The 15-Tag product has one side PIP Tag per
|
||||
# finger and no independent DIP Tag, while the formal fourth sweep cycle
|
||||
# already provides an isolated per-joint holdout.
|
||||
combination_validation_enabled: false
|
||||
combination_validation_frames: 10
|
||||
combination_maximum_position_p95_m: 0.003
|
||||
combination_maximum_orientation_p95_deg: 2.0
|
||||
validation_command_count: 3
|
||||
validation_frames: 10
|
||||
validation_seed: 20260804
|
||||
validation_timeout_seconds: 20.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
# 15-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
|
||||
maximum_validation_error_deg: 3.0
|
||||
zero_maximum_confidence_half_width_deg: 1.5
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2, 3, 10, 11, 12, 13]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 15, 17]
|
||||
frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [8, 9]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.016, 0.016]
|
||||
+89
-10
@@ -1,8 +1,13 @@
|
||||
"""Map G20 u8 commands to URDF joint angles using one calibration JSON.
|
||||
"""Map G20 u8 feedback to URDF joint angles using one calibration JSON.
|
||||
|
||||
The static encoder-zero corrections in ``zero_angles`` are already baked into
|
||||
the corrected URDF joint origins. This bridge therefore publishes only the
|
||||
dynamic ``angle_rad`` values and never adds the static offsets a second time.
|
||||
|
||||
Schema-v5 trajectories are fitted against timestamp-synchronised hardware
|
||||
feedback, not controller set-points. They must therefore be queried with the
|
||||
SDK ``hand_state`` topic. The retained schema-v4 path is command-indexed for
|
||||
backwards compatibility only.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -16,7 +21,11 @@ import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .full_hand import get_hand_calibration_profile, validate_compact_payload
|
||||
from .full_hand import (
|
||||
get_hand_calibration_profile,
|
||||
infer_compact_payload_layout,
|
||||
validate_compact_payload,
|
||||
)
|
||||
|
||||
|
||||
G20_COMMAND_NAMES: tuple[str, ...] = (
|
||||
@@ -71,7 +80,7 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = (
|
||||
|
||||
|
||||
class CalibratedCommandMapper:
|
||||
"""Validated, side-specific lookup from a G20 command to URDF radians."""
|
||||
"""Validated, side-specific lookup from G20 u8 values to URDF radians."""
|
||||
|
||||
def __init__(
|
||||
self, payload: Mapping[str, Any], *, expected_side: str | None = None
|
||||
@@ -86,9 +95,20 @@ 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)
|
||||
layout_id = infer_compact_payload_layout(payload)
|
||||
profile = get_hand_calibration_profile(side, layout_id)
|
||||
self.side = side
|
||||
self.layout_id = layout_id
|
||||
self.serial_number = str(payload["serial_number"])
|
||||
schema_version = int(payload["schema_version"])
|
||||
self.input_domain = str(
|
||||
payload.get(
|
||||
"curve_input_domain",
|
||||
"command_u8" if schema_version == 4 else "",
|
||||
)
|
||||
)
|
||||
if self.input_domain not in {"command_u8", "feedback_u8"}:
|
||||
raise ValueError("calibration curve_input_domain is invalid")
|
||||
self._motor_by_joint = {
|
||||
name: int(profile.joint_specs[name].motor_index)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
@@ -100,6 +120,27 @@ class CalibratedCommandMapper:
|
||||
)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._decreasing_curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name].get(
|
||||
"decreasing_rad", payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._increasing_curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name].get(
|
||||
"increasing_rad", payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._previous_by_motor: dict[int, float] = {}
|
||||
self._direction_by_motor: dict[int, str] = {}
|
||||
self.direction_deadband_u8 = 0.5
|
||||
|
||||
@staticmethod
|
||||
def _command_index(value: float) -> int:
|
||||
@@ -133,10 +174,34 @@ class CalibratedCommandMapper:
|
||||
)
|
||||
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
|
||||
)
|
||||
direction_by_motor: dict[int, str | None] = {}
|
||||
for motor, value in enumerate(command):
|
||||
previous = self._previous_by_motor.get(motor)
|
||||
direction = self._direction_by_motor.get(motor)
|
||||
if previous is not None:
|
||||
if value > previous + self.direction_deadband_u8:
|
||||
direction = "increasing"
|
||||
elif value < previous - self.direction_deadband_u8:
|
||||
direction = "decreasing"
|
||||
direction_by_motor[motor] = direction
|
||||
result: list[float] = []
|
||||
for name in G20_URDF_JOINT_NAMES:
|
||||
motor = self._motor_by_joint[name]
|
||||
direction = direction_by_motor[motor]
|
||||
curves = (
|
||||
self._increasing_curves
|
||||
if direction == "increasing"
|
||||
else self._decreasing_curves
|
||||
if direction == "decreasing"
|
||||
else self._curves
|
||||
)
|
||||
result.append(curves[name][indices[motor]])
|
||||
for motor, value in enumerate(command):
|
||||
self._previous_by_motor[motor] = value
|
||||
direction = direction_by_motor[motor]
|
||||
if direction is not None:
|
||||
self._direction_by_motor[motor] = direction
|
||||
return tuple(result)
|
||||
|
||||
|
||||
def load_calibrated_command_mapper(
|
||||
@@ -149,6 +214,17 @@ def load_calibrated_command_mapper(
|
||||
return CalibratedCommandMapper(payload, expected_side=expected_side)
|
||||
|
||||
|
||||
def default_input_topic(hand_type: str, input_domain: str) -> str:
|
||||
side = str(hand_type).lower()
|
||||
if side not in {"left", "right"}:
|
||||
raise ValueError("hand_type must be left or right")
|
||||
if input_domain == "feedback_u8":
|
||||
return f"/g20/cb_{side}_hand_state"
|
||||
if input_domain == "command_u8":
|
||||
return f"/g20/cb_{side}_hand_control_cmd"
|
||||
raise ValueError("calibration curve_input_domain is invalid")
|
||||
|
||||
|
||||
class CalibratedJointStateBridge(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("g20_calibrated_joint_state_bridge")
|
||||
@@ -168,7 +244,9 @@ class CalibratedJointStateBridge(Node):
|
||||
)
|
||||
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.input_topic = input_topic or default_input_topic(
|
||||
hand_type, self.mapper.input_domain
|
||||
)
|
||||
self.output_topic = (
|
||||
output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
|
||||
)
|
||||
@@ -179,7 +257,8 @@ class CalibratedJointStateBridge(Node):
|
||||
self._last_error = ""
|
||||
self.get_logger().info(
|
||||
f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
|
||||
f"{self.input_topic} -> {self.output_topic}"
|
||||
f"{self.input_topic} ({self.mapper.input_domain}) -> "
|
||||
f"{self.output_topic}"
|
||||
)
|
||||
|
||||
def _command_callback(self, command: JointState) -> None:
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
+775
-81
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,553 @@
|
||||
"""One-command product runner for G20_RIGHT_001 calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from datetime import datetime
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
import traceback
|
||||
from typing import Any, Mapping
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
||||
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
|
||||
from .product import ProductConfig, load_product_config, sha256_file
|
||||
from .publication import atomic_session_pointer, finalize_session_artifacts
|
||||
|
||||
|
||||
EXIT_PASS = 0
|
||||
EXIT_QUALITY = 2
|
||||
EXIT_SAFETY = 3
|
||||
STATUS_TIMEOUT_SECONDS = 90.0
|
||||
|
||||
|
||||
class CalibrationMonitor(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("g20_calibration_product_runner")
|
||||
self.latest_status: dict[str, Any] = {}
|
||||
self.last_status_at = time.monotonic()
|
||||
self.start_requested = False
|
||||
self.start_future: Any = None
|
||||
self.abort_future: Any = None
|
||||
self.create_subscription(String, "/g20_calibration/status", self._status, 10)
|
||||
self.start_client = self.create_client(Trigger, "/g20_calibration/start")
|
||||
self.abort_client = self.create_client(Trigger, "/g20_calibration/abort")
|
||||
|
||||
def _status(self, message: String) -> None:
|
||||
try:
|
||||
payload = json.loads(message.data)
|
||||
except (TypeError, json.JSONDecodeError):
|
||||
return
|
||||
if isinstance(payload, dict):
|
||||
self.latest_status = payload
|
||||
self.last_status_at = time.monotonic()
|
||||
|
||||
def maybe_start(self) -> None:
|
||||
if self.start_requested or self.latest_status.get("state") != "WAIT_START":
|
||||
return
|
||||
if not self.start_client.service_is_ready():
|
||||
self.start_client.wait_for_service(timeout_sec=0.05)
|
||||
return
|
||||
self.start_requested = True
|
||||
self.start_future = self.start_client.call_async(Trigger.Request())
|
||||
|
||||
def abort(self) -> None:
|
||||
if not self.abort_client.service_is_ready():
|
||||
self.abort_client.wait_for_service(timeout_sec=1.0)
|
||||
if self.abort_client.service_is_ready():
|
||||
self.abort_future = self.abort_client.call_async(Trigger.Request())
|
||||
|
||||
|
||||
class ProgressConsole:
|
||||
def __init__(self, serial_number: str) -> None:
|
||||
self.serial_number = serial_number
|
||||
self.estimator = ProgressEstimator.start()
|
||||
self.last_text = ""
|
||||
self.last_issue = ""
|
||||
|
||||
def update(self, status: Mapping[str, Any]) -> None:
|
||||
text = render_progress_zh(self.serial_number, status, self.estimator)
|
||||
if text == self.last_text:
|
||||
return
|
||||
self.last_text = text
|
||||
if sys.stdout.isatty():
|
||||
sys.stdout.write("\x1b[2J\x1b[H" + text + "\n")
|
||||
sys.stdout.flush()
|
||||
else:
|
||||
print(text, flush=True)
|
||||
reason = str(status.get("reason", ""))
|
||||
if reason.startswith("automatic_retry_") and reason != self.last_issue:
|
||||
self.last_issue = reason
|
||||
active = status.get("active", {})
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
f"⚠ 当前任务出现问题:{reason.removeprefix('automatic_retry_')}",
|
||||
f"系统处理:只重扫当前任务(第 {active.get('automatic_retry_count', 1)}/2 次)",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
|
||||
|
||||
def _default_product_config() -> Path:
|
||||
try:
|
||||
installed = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
) / "config" / "g20_right_product.yaml"
|
||||
if installed.is_file():
|
||||
return installed
|
||||
except Exception:
|
||||
pass
|
||||
return (
|
||||
Path.cwd()
|
||||
/ "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml"
|
||||
).resolve()
|
||||
|
||||
|
||||
def _launch_command(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
) -> list[str]:
|
||||
values = {
|
||||
"hand_type": "right",
|
||||
"tag_layout": "g20_right_15",
|
||||
"serial_number": config.serial_number,
|
||||
"can_interface": config.can_interface,
|
||||
"session_dir": str(session),
|
||||
"output_root": str(config.output_root),
|
||||
"camera_extrinsics_file": str(config.camera_extrinsics),
|
||||
"source_urdf_path": str(config.source_urdf),
|
||||
"source_urdf_expected_sha256": config.source_urdf_sha256,
|
||||
"corrected_urdf_output_dir": str(session),
|
||||
"calibration_config": str(config.calibration_config),
|
||||
"tag_config": str(config.tag_config),
|
||||
"commands_enabled": "true",
|
||||
"start_cameras": "true",
|
||||
"start_sdk": "true",
|
||||
"record_bag": "false",
|
||||
"validation_enabled": "false",
|
||||
}
|
||||
if resume_from is not None:
|
||||
values["resume_raw_samples_path"] = str(
|
||||
resume_from / "raw_samples.jsonl"
|
||||
)
|
||||
for view, camera in config.cameras.items():
|
||||
values[f"{view}_camera_serial"] = camera["serial_number"]
|
||||
values[f"{view}_camera_name"] = camera["camera_name"]
|
||||
values[f"{view}_camera_info_url"] = camera["camera_info"]
|
||||
return [
|
||||
"ros2",
|
||||
"launch",
|
||||
"g20_thumb_apriltag_calibration",
|
||||
"three_camera_calibration.launch.py",
|
||||
*(f"{name}:={value}" for name, value in values.items()),
|
||||
]
|
||||
|
||||
|
||||
def _stop_stack(process: subprocess.Popen[Any]) -> None:
|
||||
if process.poll() is not None:
|
||||
return
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGINT)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
try:
|
||||
process.wait(timeout=15.0)
|
||||
except subprocess.TimeoutExpired:
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
try:
|
||||
process.wait(timeout=5.0)
|
||||
except subprocess.TimeoutExpired:
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
process.wait(timeout=5.0)
|
||||
|
||||
|
||||
def _write_trace(log_path: Path, error: BaseException) -> None:
|
||||
with log_path.open("a", encoding="utf-8") as stream:
|
||||
stream.write("\n[one-command exception]\n")
|
||||
traceback.print_exception(type(error), error, error.__traceback__, file=stream)
|
||||
|
||||
|
||||
def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None:
|
||||
monitor.abort()
|
||||
deadline = time.monotonic() + float(timeout_seconds)
|
||||
while time.monotonic() < deadline and rclpy.ok():
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
if monitor.latest_status.get("state") == "ABORTED":
|
||||
return
|
||||
|
||||
|
||||
def _run_hardware_session(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
) -> tuple[dict[str, Any], int]:
|
||||
session.mkdir(parents=True, exist_ok=False)
|
||||
(session / "raw_samples.jsonl").touch()
|
||||
log_path = session / "calibration.log"
|
||||
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
|
||||
atomic_session_pointer(config.session_root, "latest_attempt", session)
|
||||
monitor = CalibrationMonitor()
|
||||
console = ProgressConsole(config.serial_number)
|
||||
process: subprocess.Popen[Any] | None = None
|
||||
latest_status: dict[str, Any] = {
|
||||
"state": "PREFLIGHT",
|
||||
"reason": "starting_ros_stack",
|
||||
"progress": 0.0,
|
||||
"views": {},
|
||||
"feedback_hz": 0.0,
|
||||
}
|
||||
exit_code = EXIT_QUALITY
|
||||
try:
|
||||
process = subprocess.Popen(
|
||||
_launch_command(config, session, resume_from=resume_from),
|
||||
cwd=config.workspace,
|
||||
stdout=log_stream,
|
||||
stderr=subprocess.STDOUT,
|
||||
text=True,
|
||||
start_new_session=True,
|
||||
)
|
||||
launched_at = time.monotonic()
|
||||
last_render = 0.0
|
||||
while True:
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
if monitor.latest_status:
|
||||
latest_status = monitor.latest_status
|
||||
monitor.maybe_start()
|
||||
now = time.monotonic()
|
||||
if now - last_render >= 0.5:
|
||||
console.update(latest_status)
|
||||
last_render = now
|
||||
if monitor.start_future is not None and monitor.start_future.done():
|
||||
response = monitor.start_future.result()
|
||||
if response is None or not response.success:
|
||||
message = "start service failed" if response is None else response.message
|
||||
raise RuntimeError(f"CFG-START-008:{message}")
|
||||
monitor.start_future = None
|
||||
state = str(latest_status.get("state", ""))
|
||||
if state == "COMPLETE":
|
||||
exit_code = EXIT_PASS
|
||||
break
|
||||
if state in {"PAUSED", "ABORTED"}:
|
||||
reason = str(latest_status.get("reason", "calibration_paused"))
|
||||
exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY
|
||||
if state == "PAUSED" and "stall" not in reason:
|
||||
# Ordinary quality failures return to the reviewed baseline
|
||||
# before the process tree is stopped. Mechanical stalls
|
||||
# deliberately skip this path and keep the current pose.
|
||||
failure_status = dict(latest_status)
|
||||
_request_safe_abort(monitor)
|
||||
latest_status = failure_status
|
||||
break
|
||||
if process.poll() is not None:
|
||||
raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}")
|
||||
if (
|
||||
not monitor.latest_status
|
||||
and now - launched_at > STATUS_TIMEOUT_SECONDS
|
||||
):
|
||||
raise RuntimeError("CAM-STATUS-202:no calibration status received")
|
||||
if (
|
||||
monitor.latest_status
|
||||
and now - monitor.last_status_at > STATUS_TIMEOUT_SECONDS
|
||||
):
|
||||
raise RuntimeError("MOTION-COMM-303:calibration status stopped")
|
||||
except KeyboardInterrupt as error:
|
||||
latest_status["state"] = "ABORTED"
|
||||
latest_status["reason"] = "operator_abort"
|
||||
_request_safe_abort(monitor)
|
||||
_write_trace(log_path, error)
|
||||
exit_code = EXIT_SAFETY
|
||||
except BaseException as error:
|
||||
latest_status["state"] = "PAUSED"
|
||||
latest_status["reason"] = str(error)
|
||||
_write_trace(log_path, error)
|
||||
exit_code = EXIT_QUALITY
|
||||
finally:
|
||||
if process is not None:
|
||||
_stop_stack(process)
|
||||
monitor.destroy_node()
|
||||
log_stream.flush()
|
||||
os.fsync(log_stream.fileno())
|
||||
log_stream.close()
|
||||
|
||||
if exit_code != EXIT_PASS:
|
||||
_, block = build_failure_report(
|
||||
config,
|
||||
session,
|
||||
latest_status,
|
||||
reason=str(latest_status.get("reason", "unknown_failure")),
|
||||
)
|
||||
print(block, flush=True)
|
||||
return latest_status, exit_code
|
||||
|
||||
|
||||
def _startup_failure_block(path: Path, error: BaseException) -> str:
|
||||
return "\n".join(
|
||||
[
|
||||
"========== 请复制以下内容给开发者 ==========",
|
||||
"结果:FAIL",
|
||||
"错误代码:CFG-PRODUCT-001",
|
||||
"失败阶段:启动静态预检",
|
||||
f"问题:{error}",
|
||||
f"产品配置:{path}",
|
||||
"自动处理:未启动相机、SDK或机械手运动",
|
||||
"建议:复制本诊断块给开发者,不要手工修改哈希绕过检查。",
|
||||
"========== 复制结束 ==========",
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
||||
"""Return the newest compatible failed attempt, never a passed session.
|
||||
|
||||
Do not trust only ``latest_attempt``. A process interrupted during the
|
||||
device-only startup gate may have already moved that pointer while still
|
||||
containing no ``session_start`` checkpoint. In that case walk backwards
|
||||
to the preceding usable failed session instead of throwing away hours of
|
||||
completed tasks.
|
||||
"""
|
||||
root = config.session_root
|
||||
try:
|
||||
resolved_root = root.resolve(strict=True)
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
candidates: list[Path] = []
|
||||
pointer = config.session_root / "latest_attempt"
|
||||
if pointer.exists():
|
||||
try:
|
||||
candidates.append(pointer.resolve(strict=True))
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
candidates.extend(
|
||||
sorted(
|
||||
(
|
||||
path
|
||||
for path in root.iterdir()
|
||||
if path.is_dir() and not path.name.startswith("latest_")
|
||||
),
|
||||
key=lambda path: path.name,
|
||||
reverse=True,
|
||||
)
|
||||
)
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
passed_pointer = config.session_root / "latest_passed"
|
||||
passed: Path | None = None
|
||||
if passed_pointer.exists():
|
||||
try:
|
||||
passed = passed_pointer.resolve(strict=True)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
seen: set[Path] = set()
|
||||
for unresolved in candidates:
|
||||
try:
|
||||
candidate = unresolved.resolve(strict=True)
|
||||
except OSError:
|
||||
continue
|
||||
if candidate in seen:
|
||||
continue
|
||||
seen.add(candidate)
|
||||
if candidate.parent != resolved_root or not candidate.is_dir():
|
||||
continue
|
||||
# A failed attempt older than the current formal release is stale and
|
||||
# must not seed a new independent calibration.
|
||||
if passed is not None and candidate.name <= passed.name:
|
||||
continue
|
||||
raw_path = candidate / "raw_samples.jsonl"
|
||||
if not raw_path.is_file():
|
||||
continue
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
summary: dict[str, Any] | None = None
|
||||
if summary_path.is_file():
|
||||
try:
|
||||
loaded = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError):
|
||||
continue
|
||||
if not isinstance(loaded, dict) or loaded.get("result") != "FAIL":
|
||||
continue
|
||||
summary = loaded
|
||||
hashes = summary.get("hashes", {})
|
||||
if not isinstance(hashes, Mapping):
|
||||
continue
|
||||
if (
|
||||
str(hashes.get("source_urdf_sha256", ""))
|
||||
!= config.source_urdf_sha256
|
||||
or str(hashes.get("camera_extrinsics_sha256", ""))
|
||||
!= config.camera_extrinsics_sha256
|
||||
):
|
||||
continue
|
||||
start: dict[str, Any] | None = None
|
||||
try:
|
||||
with raw_path.open("r", encoding="utf-8") as stream:
|
||||
for line in stream:
|
||||
if not line.strip():
|
||||
continue
|
||||
value = json.loads(line)
|
||||
if (
|
||||
isinstance(value, dict)
|
||||
and value.get("kind") == "session_start"
|
||||
):
|
||||
start = value
|
||||
break
|
||||
except (OSError, json.JSONDecodeError):
|
||||
continue
|
||||
if (
|
||||
start is None
|
||||
or start.get("hand_type") != "right"
|
||||
or start.get("tag_layout") != "g20_right_15"
|
||||
or start.get("source_urdf_sha256")
|
||||
!= config.source_urdf_sha256
|
||||
):
|
||||
continue
|
||||
if summary is None:
|
||||
# Ctrl+C can terminate the ROS launch tree before the wrapper gets
|
||||
# a chance to create calibration_summary_zh.json. The immutable
|
||||
# checkpoint itself is enough to resume only after independently
|
||||
# proving that its external geometry still matches the product.
|
||||
try:
|
||||
checkpoint_extrinsics = Path(
|
||||
str(start["camera_extrinsics_file"])
|
||||
).expanduser().resolve(strict=True)
|
||||
if sha256_file(checkpoint_extrinsics) != (
|
||||
config.camera_extrinsics_sha256
|
||||
):
|
||||
continue
|
||||
except (KeyError, OSError, ValueError):
|
||||
continue
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
def run(
|
||||
config_path: str | Path,
|
||||
*,
|
||||
workspace: str | Path | None = None,
|
||||
preflight_only: bool = False,
|
||||
allow_resume: bool = True,
|
||||
) -> int:
|
||||
path = Path(config_path).expanduser().resolve()
|
||||
try:
|
||||
# Resolve every file and camera identity before allowing a hardware
|
||||
# process to start. A second load enables the real CAN existence gate.
|
||||
config = load_product_config(path, workspace=workspace, check_can=False)
|
||||
load_product_config(path, workspace=workspace, check_can=True)
|
||||
except BaseException as error:
|
||||
print(_startup_failure_block(path, error), flush=True)
|
||||
return EXIT_QUALITY
|
||||
if preflight_only:
|
||||
print("PASS:产品文件、相机内外参、15张Tag配置和CAN接口静态预检通过。")
|
||||
return EXIT_PASS
|
||||
|
||||
config.session_root.mkdir(parents=True, exist_ok=True)
|
||||
resume_candidate = (
|
||||
_automatic_resume_candidate(config) if allow_resume else None
|
||||
)
|
||||
if resume_candidate is not None:
|
||||
print(
|
||||
"检测到兼容的失败会话,将恢复已完整通过的关节任务:"
|
||||
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
|
||||
flush=True,
|
||||
)
|
||||
maximum_sessions = config.required_independent_passes
|
||||
for pass_index in range(maximum_sessions):
|
||||
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session = config.session_root / stamp
|
||||
while session.exists():
|
||||
time.sleep(1.0)
|
||||
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session = config.session_root / stamp
|
||||
if maximum_sessions > 1:
|
||||
print(f"正式标定复验:第 {pass_index + 1}/{maximum_sessions} 次", flush=True)
|
||||
status, code = _run_hardware_session(
|
||||
config,
|
||||
session,
|
||||
resume_from=(resume_candidate if pass_index == 0 else None),
|
||||
)
|
||||
if code != EXIT_PASS:
|
||||
return code
|
||||
try:
|
||||
summary, release_ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
)
|
||||
except BaseException as error:
|
||||
_write_trace(session / "calibration.log", error)
|
||||
status = dict(status)
|
||||
status["state"] = "PAUSED"
|
||||
status["reason"] = f"PUB-ARTIFACT-601:{error}"
|
||||
_, block = build_failure_report(config, session, status, reason=status["reason"])
|
||||
print(block, flush=True)
|
||||
return EXIT_QUALITY
|
||||
if release_ready:
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
"PASS:G20右手标定、URDF修正和独立复验全部通过。",
|
||||
f"正式结果:{config.session_root / 'latest_passed'}",
|
||||
f"JSON:{session / f'g20_right_{config.serial_number}_calibration.json'}",
|
||||
f"URDF:{summary['artifacts']['urdf']}",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
return EXIT_PASS
|
||||
print("本次会话质量PASS;正在自动执行第二次独立完整复验。", flush=True)
|
||||
return EXIT_QUALITY
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
parser = argparse.ArgumentParser(description="G20右手一键精密标定")
|
||||
parser.add_argument("--config", default=str(_default_product_config()))
|
||||
parser.add_argument("--workspace", default=None)
|
||||
parser.add_argument("--preflight-only", action="store_true")
|
||||
parser.add_argument(
|
||||
"--no-resume",
|
||||
action="store_true",
|
||||
help="忽略失败会话,从第一个关节开始全新采集",
|
||||
)
|
||||
arguments = parser.parse_args(args)
|
||||
configure_fastdds_large_image_transport()
|
||||
ros_log_dir = Path(
|
||||
os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs")
|
||||
)
|
||||
ros_log_dir.mkdir(parents=True, exist_ok=True)
|
||||
rclpy.init()
|
||||
try:
|
||||
code = run(
|
||||
arguments.config,
|
||||
workspace=arguments.workspace,
|
||||
preflight_only=arguments.preflight_only,
|
||||
allow_resume=not arguments.no_resume,
|
||||
)
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,394 @@
|
||||
"""Stable Chinese progress and copy/paste diagnostics for non-expert users."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
import time
|
||||
from typing import Any, Mapping
|
||||
|
||||
from .product import ProductConfig
|
||||
from .storage import atomic_write_json
|
||||
from .three_camera_diagnostics import JOINT_NAMES_ZH, STATE_NAMES_ZH, three_camera_reason_zh
|
||||
|
||||
|
||||
def _duration(seconds: float | None) -> str:
|
||||
if seconds is None or not math.isfinite(seconds) or seconds < 0.0:
|
||||
return "计算中"
|
||||
value = int(round(seconds))
|
||||
return f"{value // 60}分{value % 60:02d}秒"
|
||||
|
||||
|
||||
def _task_tag_id_status(views: Mapping[str, Any]) -> str:
|
||||
"""Render every currently required Tag ID with its live visibility."""
|
||||
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
|
||||
ordered_views = [
|
||||
*[name for name in ("front", "side", "top") if name in views],
|
||||
*sorted(name for name in views if name not in labels),
|
||||
]
|
||||
groups: list[str] = []
|
||||
for name in ordered_views:
|
||||
item = views.get(name, {})
|
||||
if not isinstance(item, Mapping):
|
||||
continue
|
||||
required = sorted({int(value) for value in item.get("required_tag_ids", [])})
|
||||
if not required:
|
||||
continue
|
||||
visible = {int(value) for value in item.get("detected_tag_ids", [])}
|
||||
locked = {
|
||||
int(value)
|
||||
for value in item.get("locked_reference_tag_ids", [])
|
||||
}
|
||||
ids = ",".join(
|
||||
f"{tag_id}{'锁' if tag_id in locked else '✓' if tag_id in visible else '✗'}"
|
||||
for tag_id in required
|
||||
)
|
||||
groups.append(f"{labels.get(name, name)}[{ids}]")
|
||||
if not groups:
|
||||
return ""
|
||||
return (
|
||||
" 当前任务ID:"
|
||||
+ " ".join(groups)
|
||||
+ "(✓实时可见/锁=基准锁定/✗不可用)"
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class ProgressEstimator:
|
||||
started_at: float
|
||||
last_progress: float = 0.0
|
||||
last_progress_at: float = 0.0
|
||||
seconds_per_fraction: float | None = None
|
||||
|
||||
@classmethod
|
||||
def start(cls) -> "ProgressEstimator":
|
||||
now = time.monotonic()
|
||||
return cls(started_at=now, last_progress_at=now)
|
||||
|
||||
def remaining(self, progress: float, now: float | None = None) -> float | None:
|
||||
current = time.monotonic() if now is None else float(now)
|
||||
value = float(max(0.0, min(1.0, progress)))
|
||||
delta = value - self.last_progress
|
||||
elapsed = current - self.last_progress_at
|
||||
if delta >= 0.002 and elapsed > 0.0:
|
||||
estimate = elapsed / delta
|
||||
self.seconds_per_fraction = (
|
||||
estimate
|
||||
if self.seconds_per_fraction is None
|
||||
else 0.8 * self.seconds_per_fraction + 0.2 * estimate
|
||||
)
|
||||
self.last_progress = value
|
||||
self.last_progress_at = current
|
||||
if value >= 1.0:
|
||||
return 0.0
|
||||
if self.seconds_per_fraction is not None:
|
||||
return max(0.0, self.seconds_per_fraction * (1.0 - value))
|
||||
if value >= 0.02:
|
||||
return max(0.0, (current - self.started_at) * (1.0 - value) / value)
|
||||
return None
|
||||
|
||||
|
||||
def render_progress_zh(
|
||||
serial_number: str,
|
||||
status: Mapping[str, Any],
|
||||
estimator: ProgressEstimator,
|
||||
) -> str:
|
||||
progress = float(status.get("progress", 0.0))
|
||||
active = status.get("active", {})
|
||||
if not isinstance(active, Mapping):
|
||||
active = {}
|
||||
state = str(status.get("state", "PREFLIGHT"))
|
||||
stage = STATE_NAMES_ZH.get(state, state)
|
||||
preflight_mode = str(status.get("preflight_mode", ""))
|
||||
if preflight_mode == "device_only_before_baseline":
|
||||
stage = "设备连接预检"
|
||||
elif preflight_mode == "recovering_baseline":
|
||||
stage = "安全恢复基准形态"
|
||||
elif preflight_mode == "baseline_tags_after_recovery":
|
||||
stage = "基准姿态标签预检"
|
||||
elif str(status.get("reason", "")) == (
|
||||
"holding_same_finger_clearance_before_next_task"
|
||||
):
|
||||
stage = "保持避让姿态,切换同指下一项"
|
||||
elif str(status.get("reason", "")) == (
|
||||
"waiting_for_task_tags_at_sweep_start"
|
||||
):
|
||||
stage = "已到扫描起点,等待任务Tag"
|
||||
joints = active.get("joints", [])
|
||||
joint = "/".join(JOINT_NAMES_ZH.get(str(name), str(name)) for name in joints)
|
||||
if str(active.get("task_name", "")).endswith("_roll_multiview") and joints:
|
||||
joint = (
|
||||
JOINT_NAMES_ZH.get(str(joints[0]), str(joints[0]))
|
||||
+ "(正面+侧面同步)"
|
||||
)
|
||||
if not joint:
|
||||
joint = str(active.get("label_zh", "")) or (
|
||||
"等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
|
||||
)
|
||||
direction = {
|
||||
"decreasing": "递减",
|
||||
"increasing": "递增",
|
||||
}.get(str(active.get("direction", "")), "-")
|
||||
cycle = active.get("cycle", "-")
|
||||
repetitions = active.get("repetitions", 4)
|
||||
requested = active.get(
|
||||
"current_motion_target_u8",
|
||||
active.get("command_u8", active.get("pose_name", "-")),
|
||||
)
|
||||
actual = active.get("actual_u8", "-")
|
||||
views = status.get("views", {})
|
||||
if not isinstance(views, Mapping):
|
||||
views = {}
|
||||
ready_cameras = sum(
|
||||
bool(item.get("camera_info_valid") and item.get("camera_extrinsics_valid"))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
required_tags = sum(
|
||||
len(item.get("required_tag_ids", []))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
visible_tags = sum(
|
||||
len(
|
||||
{
|
||||
*item.get("detected_tag_ids", []),
|
||||
*item.get("locked_reference_tag_ids", []),
|
||||
}
|
||||
)
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
locked_tag_count = sum(
|
||||
len(item.get("locked_reference_tag_ids", []))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
configured_tags = sum(
|
||||
len(item.get("configured_tag_ids", item.get("required_tag_ids", [])))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
visible_configured_tags = sum(
|
||||
len(
|
||||
item.get(
|
||||
"visible_configured_tag_ids",
|
||||
item.get("detected_tag_ids", []),
|
||||
)
|
||||
)
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
occlusion_allowed = bool(
|
||||
state in {"PREFLIGHT", "WAIT_START"}
|
||||
or active.get("pose_name")
|
||||
)
|
||||
if preflight_mode == "device_only_before_baseline":
|
||||
tag_status = (
|
||||
f"当前 {visible_configured_tags}/{configured_tags}"
|
||||
"(基准恢复后检查)"
|
||||
)
|
||||
elif preflight_mode == "recovering_baseline":
|
||||
tag_status = (
|
||||
f"当前 {visible_configured_tags}/{configured_tags}"
|
||||
"(恢复中不作为门槛)"
|
||||
)
|
||||
elif occlusion_allowed:
|
||||
tag_status = (
|
||||
f"当前 {visible_configured_tags}/{configured_tags}(允许遮挡) "
|
||||
f"本阶段必需 {visible_tags}/{required_tags}"
|
||||
)
|
||||
else:
|
||||
tag_status = (
|
||||
f"{visible_tags}/{required_tags} 有效"
|
||||
+ (f"(含锁定 {locked_tag_count})" if locked_tag_count else "")
|
||||
)
|
||||
task_tag_ids = (
|
||||
_task_tag_id_status(views)
|
||||
if state
|
||||
in {
|
||||
"PREPARE_SWEEP",
|
||||
"SWEEP",
|
||||
"VALIDATION_MOVE",
|
||||
"VALIDATION_CAPTURE",
|
||||
}
|
||||
else ""
|
||||
)
|
||||
retry = int(active.get("automatic_retry_count", 0))
|
||||
fit_attempt = int(active.get("fit_attempt", 1))
|
||||
fit_attempt_limit = int(active.get("fit_attempt_limit", 1))
|
||||
feedback_hz = float(status.get("feedback_hz", 0.0))
|
||||
eta = (
|
||||
"等待Tag"
|
||||
if str(status.get("reason", ""))
|
||||
== "waiting_for_task_tags_at_sweep_start"
|
||||
else _duration(estimator.remaining(progress))
|
||||
)
|
||||
lines = [
|
||||
f"[{serial_number}] 标定中 {progress * 100:5.1f}% 预计剩余 {eta}",
|
||||
f"阶段:{stage}(第 {cycle}/{repetitions} 轮)",
|
||||
f"任务:{joint} 命令/反馈:{requested}/{actual} 方向:{direction}",
|
||||
f"Tag:{tag_status}{task_tag_ids} 相机:{ready_cameras}/3 正常 反馈:{feedback_hz:.1f} Hz",
|
||||
f"质量:有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry} 次",
|
||||
]
|
||||
if fit_attempt_limit > 1:
|
||||
lines.append(
|
||||
f"拟合:整关节第 {fit_attempt}/{fit_attempt_limit} 次尝试;"
|
||||
"每次包含完整四轮双向扫描"
|
||||
)
|
||||
resume = status.get("resume", {})
|
||||
if isinstance(resume, Mapping) and resume.get("used"):
|
||||
lines.append(
|
||||
"断点:已恢复 "
|
||||
f"{int(resume.get('completed_task_count', 0))}/"
|
||||
f"{int(resume.get('total_task_count', 16))} 个完整任务;"
|
||||
"失败任务已丢弃并重新采集"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, str]:
|
||||
value = str(reason)
|
||||
active = status.get("active", {})
|
||||
if not isinstance(active, Mapping):
|
||||
active = {}
|
||||
views = status.get("views", {})
|
||||
missing = [
|
||||
tag
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
for tag in item.get("missing_tag_ids", [])
|
||||
] if isinstance(views, Mapping) else []
|
||||
group_pnp_reasons = [
|
||||
str(item.get("group_pnp_reason"))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping) and item.get("group_pnp_reason")
|
||||
] if isinstance(views, Mapping) else []
|
||||
if value.startswith("CFG-"):
|
||||
return value, "产品配置或文件预检失败", "不要移动相机;复制本诊断块给开发者。"
|
||||
if "motor_state_stalled" in value:
|
||||
return "MOTION-STALL-301", "电机反馈停止向目标推进,程序已保持当前位置", "先检查机械卡阻,未排除前不要重复强推。"
|
||||
if any(
|
||||
token in value
|
||||
for token in (
|
||||
"sweep_missing_endpoint_bin",
|
||||
"sweep_bins_too_few",
|
||||
"sweep_bin_gap_too_large",
|
||||
"task_precheck_missing_command_127",
|
||||
)
|
||||
):
|
||||
return "OBS-SAMPLE-104", "当前任务的有效视觉轨迹不完整", "根据诊断中的关节、机位和缺失区间处理遮挡或反光后重新运行。"
|
||||
if "synchronised" in value and not missing and group_pnp_reasons:
|
||||
return "CAM-GEOMETRY-201", "Tag可见,但整组PnP候选持续被几何检查拒绝", "不要调整Tag;复制本诊断块给开发者检查PnP候选选择。"
|
||||
if missing or "tag" in value or "detection" in value or "synchronised" in value:
|
||||
return "OBS-TAG-103", f"所需Tag不可用或持续丢失:{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。"
|
||||
if "state" in value and ("timeout" in value or "lost" in value):
|
||||
return "MOTION-COMM-302", "机械手反馈中断或频率不足", "检查CAN接口和机械手供电后重新运行。"
|
||||
if "camera" in value or "pnp" in value or "extrinsics" in value:
|
||||
return "CAM-GEOMETRY-201", "相机、内外参或位姿求解未通过", "确认三台相机没有移动,然后复制本诊断块给开发者。"
|
||||
if "checkpoint" in value:
|
||||
return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留;复制本诊断块给开发者检查采集状态机。"
|
||||
if "fit" in value or "trajectory" in value or "axis" in value:
|
||||
return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限;复制本诊断块给开发者分析原始样本。"
|
||||
if "validation" in value or "combination" in value or "quality" in value or "zero" in value:
|
||||
return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布;复制本诊断块给开发者。"
|
||||
if "publish" in value or "artifact" in value or "URDF" in value:
|
||||
return "PUB-ARTIFACT-601", "结果文件校验或原子发布失败", "原始URDF未被覆盖;复制本诊断块给开发者。"
|
||||
return "PUB-UNEXPECTED-699", "标定程序出现未分类异常", "复制本诊断块给开发者,完整堆栈已写入calibration.log。"
|
||||
|
||||
|
||||
def build_failure_report(
|
||||
config: ProductConfig,
|
||||
session: str | Path,
|
||||
status: Mapping[str, Any],
|
||||
*,
|
||||
reason: str,
|
||||
) -> tuple[dict[str, Any], str]:
|
||||
directory = Path(session).resolve()
|
||||
code, problem, suggestion = classify_error(reason, status)
|
||||
active = status.get("active", {})
|
||||
if not isinstance(active, Mapping):
|
||||
active = {}
|
||||
else:
|
||||
active = dict(active)
|
||||
views = status.get("views", {})
|
||||
if isinstance(views, Mapping):
|
||||
group_pnp_reasons = {
|
||||
str(view): str(item.get("group_pnp_reason"))
|
||||
for view, item in views.items()
|
||||
if isinstance(item, Mapping) and item.get("group_pnp_reason")
|
||||
}
|
||||
if group_pnp_reasons:
|
||||
active["group_pnp_reasons"] = group_pnp_reasons
|
||||
try:
|
||||
explanation, automatic_action = three_camera_reason_zh(
|
||||
str(status.get("state", "")), reason, active
|
||||
)
|
||||
except Exception:
|
||||
explanation, automatic_action = problem, "已停止本次发布并保留全部诊断数据"
|
||||
camera_state = {
|
||||
view: (
|
||||
"OK"
|
||||
if isinstance(item, Mapping)
|
||||
and item.get("camera_info_valid", item.get("ready"))
|
||||
and item.get("camera_extrinsics_valid", item.get("ready"))
|
||||
and item.get("stream_alive", item.get("ready"))
|
||||
else "NOT_READY"
|
||||
)
|
||||
for view, item in (views.items() if isinstance(views, Mapping) else [])
|
||||
}
|
||||
session_id = f"{config.serial_number}_{directory.name}"
|
||||
metrics = {
|
||||
"valid_frames": active.get("valid_frames"),
|
||||
"sample": active.get("sample", {}),
|
||||
"automatic_retry_count": active.get("automatic_retry_count", 0),
|
||||
}
|
||||
payload: dict[str, Any] = {
|
||||
"schema_version": 1,
|
||||
"serial_number": config.serial_number,
|
||||
"session_id": session_id,
|
||||
"result": "FAIL",
|
||||
"error_code": code,
|
||||
"stage": str(status.get("state", "startup")),
|
||||
"reason": str(reason),
|
||||
"problem_zh": problem,
|
||||
"explanation_zh": explanation,
|
||||
"automatic_action_zh": automatic_action,
|
||||
"suggestion_zh": suggestion,
|
||||
"metrics": metrics,
|
||||
"camera_state": camera_state,
|
||||
"feedback_hz": status.get("feedback_hz", 0.0),
|
||||
"hashes": {
|
||||
"product_config_sha256": __import__("hashlib").sha256(config.path.read_bytes()).hexdigest(),
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
},
|
||||
"session_dir": str(directory),
|
||||
"quality": {"passed": False},
|
||||
}
|
||||
atomic_write_json(directory / "calibration_summary_zh.json", payload)
|
||||
block = "\n".join(
|
||||
[
|
||||
"========== 请复制以下内容给开发者 ==========",
|
||||
f"会话编号:{session_id}",
|
||||
"结果:FAIL",
|
||||
f"错误代码:{code}",
|
||||
f"失败阶段:{payload['stage']}",
|
||||
f"问题:{problem}",
|
||||
f"详细说明:{explanation}",
|
||||
f"自动处理:{automatic_action}",
|
||||
f"关键指标:{json.dumps(metrics, ensure_ascii=False, separators=(',', ':'))}",
|
||||
f"相机状态:{json.dumps(camera_state, ensure_ascii=False, separators=(',', ':'))}",
|
||||
f"反馈状态:{float(payload['feedback_hz'] or 0.0):.1f} Hz",
|
||||
f"配置哈希:{payload['hashes']['product_config_sha256']}",
|
||||
f"外参哈希:{config.camera_extrinsics_sha256}",
|
||||
f"源 URDF 哈希:{config.source_urdf_sha256}",
|
||||
f"会话目录:{directory}",
|
||||
f"建议:{suggestion}",
|
||||
"========== 复制结束 ==========",
|
||||
]
|
||||
)
|
||||
return payload, block
|
||||
@@ -477,6 +477,11 @@ def select_static_rigid_group_initialization(
|
||||
relative_translation_scale_m: float,
|
||||
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
|
||||
normal_alignment_scale_rad: float = math.radians(5.0),
|
||||
task_reference_pairs: Mapping[
|
||||
tuple[str, str], tuple[Rotation, np.ndarray]
|
||||
] | None = None,
|
||||
task_reference_rotation_scale_rad: float = math.radians(1.0),
|
||||
task_reference_translation_scale_m: float = 0.01,
|
||||
) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
|
||||
"""Select a static multi-Tag IPPE branch path in bounded time.
|
||||
|
||||
@@ -497,13 +502,18 @@ def select_static_rigid_group_initialization(
|
||||
normal_pair_names = tuple(
|
||||
(str(first), str(second)) for first, second in normal_alignment_pairs
|
||||
)
|
||||
task_references = dict(task_reference_pairs or {})
|
||||
if not frames:
|
||||
raise ValueError("at least one PnP frame is required")
|
||||
if not role_names or len(set(role_names)) != len(role_names):
|
||||
raise ValueError("roles must be non-empty and unique")
|
||||
if any(
|
||||
parent not in role_names or child not in role_names
|
||||
for parent, child in (*pair_names, *normal_pair_names)
|
||||
for parent, child in (
|
||||
*pair_names,
|
||||
*normal_pair_names,
|
||||
*task_references,
|
||||
)
|
||||
):
|
||||
raise ValueError("geometry pairs must reference roles")
|
||||
scales = (
|
||||
@@ -513,6 +523,8 @@ def select_static_rigid_group_initialization(
|
||||
float(relative_rotation_scale_rad),
|
||||
float(relative_translation_scale_m),
|
||||
float(normal_alignment_scale_rad),
|
||||
float(task_reference_rotation_scale_rad),
|
||||
float(task_reference_translation_scale_m),
|
||||
)
|
||||
if min(scales) <= 0.0:
|
||||
raise ValueError("static initialization scales must be positive")
|
||||
@@ -523,6 +535,8 @@ def select_static_rigid_group_initialization(
|
||||
relative_rotation_scale,
|
||||
relative_translation_scale,
|
||||
normal_scale,
|
||||
task_reference_rotation_scale,
|
||||
task_reference_translation_scale,
|
||||
) = scales
|
||||
|
||||
combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
|
||||
@@ -578,6 +592,16 @@ def select_static_rigid_group_initialization(
|
||||
+ float(np.linalg.norm(translation - reference_translation))
|
||||
/ relative_translation_scale
|
||||
)
|
||||
for pair, (task_rotation, task_translation) in task_references.items():
|
||||
rotation, translation = _relative_pose(
|
||||
combination[pair[0]], combination[pair[1]]
|
||||
)
|
||||
score += (
|
||||
float((task_rotation.inv() * rotation).magnitude())
|
||||
/ task_reference_rotation_scale
|
||||
+ float(np.linalg.norm(translation - task_translation))
|
||||
/ task_reference_translation_scale
|
||||
)
|
||||
return float(score)
|
||||
|
||||
best_total = float("inf")
|
||||
@@ -628,6 +652,9 @@ def select_static_rigid_group_initialization(
|
||||
quality = dict(quality)
|
||||
quality["total_cost"] = float(best_total)
|
||||
quality["initialization_search"] = "static_reference"
|
||||
quality["task_reference_used"] = (
|
||||
"true" if task_references else "false"
|
||||
)
|
||||
return selected_path, quality
|
||||
|
||||
|
||||
@@ -1013,6 +1040,13 @@ class SquareTagGroupPoseTracker:
|
||||
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
|
||||
normal_alignment_scale_rad: float = math.radians(5.0),
|
||||
maximum_normal_alignment_rad: float | None = None,
|
||||
return_reference_rotation_scale_rad: float = math.radians(1.0),
|
||||
return_reference_maximum_command_gap_u8: int = 8,
|
||||
coupled_rotation_pairs: Sequence[
|
||||
tuple[str, str, str, str, float]
|
||||
] = (),
|
||||
coupled_rotation_scale_rad: float = math.radians(3.0),
|
||||
maximum_coupled_rotation_residual_rad: float | None = None,
|
||||
) -> None:
|
||||
self.roles = tuple(str(role) for role in roles)
|
||||
self.adjacent_pairs = tuple(
|
||||
@@ -1023,6 +1057,22 @@ class SquareTagGroupPoseTracker:
|
||||
(str(first), str(second))
|
||||
for first, second in normal_alignment_pairs
|
||||
)
|
||||
self.coupled_rotation_pairs = tuple(
|
||||
(
|
||||
str(driver_parent),
|
||||
str(driver_child),
|
||||
str(follower_parent),
|
||||
str(follower_child),
|
||||
float(multiplier),
|
||||
)
|
||||
for (
|
||||
driver_parent,
|
||||
driver_child,
|
||||
follower_parent,
|
||||
follower_child,
|
||||
multiplier,
|
||||
) in coupled_rotation_pairs
|
||||
)
|
||||
if not self.roles or len(set(self.roles)) != len(self.roles):
|
||||
raise ValueError("roles must be non-empty and unique")
|
||||
if any(
|
||||
@@ -1054,6 +1104,20 @@ class SquareTagGroupPoseTracker:
|
||||
if maximum_normal_alignment_rad is None
|
||||
else float(maximum_normal_alignment_rad)
|
||||
)
|
||||
self.return_reference_rotation_scale_rad = float(
|
||||
return_reference_rotation_scale_rad
|
||||
)
|
||||
self.return_reference_maximum_command_gap_u8 = int(
|
||||
return_reference_maximum_command_gap_u8
|
||||
)
|
||||
self.coupled_rotation_scale_rad = float(
|
||||
coupled_rotation_scale_rad
|
||||
)
|
||||
self.maximum_coupled_rotation_residual_rad = (
|
||||
None
|
||||
if maximum_coupled_rotation_residual_rad is None
|
||||
else float(maximum_coupled_rotation_residual_rad)
|
||||
)
|
||||
reset_seconds = float(reset_after_seconds)
|
||||
if min(
|
||||
self.maximum_pose_jump_rad,
|
||||
@@ -1062,6 +1126,8 @@ class SquareTagGroupPoseTracker:
|
||||
self.relative_translation_scale_m,
|
||||
self.reprojection_scale_px,
|
||||
self.normal_alignment_scale_rad,
|
||||
self.return_reference_rotation_scale_rad,
|
||||
self.coupled_rotation_scale_rad,
|
||||
reset_seconds,
|
||||
) <= 0.0:
|
||||
raise ValueError("group tracking scales must be positive")
|
||||
@@ -1069,11 +1135,33 @@ class SquareTagGroupPoseTracker:
|
||||
raise ValueError("reprojection_weight must be non-negative")
|
||||
if self.initialization_frames < 1:
|
||||
raise ValueError("initialization_frames must be positive")
|
||||
if self.return_reference_maximum_command_gap_u8 < 0:
|
||||
raise ValueError(
|
||||
"return reference maximum command gap must be non-negative"
|
||||
)
|
||||
if (
|
||||
self.maximum_normal_alignment_rad is not None
|
||||
and self.maximum_normal_alignment_rad <= 0.0
|
||||
):
|
||||
raise ValueError("maximum normal alignment must be positive")
|
||||
if any(
|
||||
role not in self.roles
|
||||
for coupling in self.coupled_rotation_pairs
|
||||
for role in coupling[:4]
|
||||
):
|
||||
raise ValueError("coupled rotation pairs must reference roles")
|
||||
if any(
|
||||
multiplier <= 0.0
|
||||
for *_, multiplier in self.coupled_rotation_pairs
|
||||
):
|
||||
raise ValueError("coupled rotation multipliers must be positive")
|
||||
if (
|
||||
self.maximum_coupled_rotation_residual_rad is not None
|
||||
and self.maximum_coupled_rotation_residual_rad <= 0.0
|
||||
):
|
||||
raise ValueError(
|
||||
"maximum coupled rotation residual must be positive"
|
||||
)
|
||||
self.reset_after_ns = int(reset_seconds * 1_000_000_000)
|
||||
self._previous: dict[str, SquareTagPose] = {}
|
||||
self._previous_stamp_ns: int | None = None
|
||||
@@ -1083,22 +1171,200 @@ class SquareTagGroupPoseTracker:
|
||||
self._initial_stamps_ns: list[int] = []
|
||||
self.last_initialization_quality: dict[str, float | str] = {}
|
||||
self.branch_correction_counts: dict[str, int] = {}
|
||||
self._decreasing_relative_rotations: dict[
|
||||
int, dict[tuple[str, str], Rotation]
|
||||
] = {}
|
||||
self._coupled_reference_rotations: dict[
|
||||
tuple[str, str], Rotation
|
||||
] = {}
|
||||
self._task_reference_relative_poses: dict[
|
||||
tuple[str, str], tuple[Rotation, np.ndarray]
|
||||
] = {}
|
||||
|
||||
def reset(self) -> None:
|
||||
def reset(self, *, preserve_task_reference: bool = False) -> None:
|
||||
self._previous.clear()
|
||||
self._previous_stamp_ns = None
|
||||
self._initial_candidates.clear()
|
||||
self._initial_stamps_ns.clear()
|
||||
self.last_initialization_quality.clear()
|
||||
self.branch_correction_counts.clear()
|
||||
self._decreasing_relative_rotations.clear()
|
||||
self._coupled_reference_rotations.clear()
|
||||
if not preserve_task_reference:
|
||||
self._task_reference_relative_poses.clear()
|
||||
|
||||
def _task_reference_cost(
|
||||
self, combination: Mapping[str, SquareTagPose]
|
||||
) -> float:
|
||||
residual = 0.0
|
||||
for pair, (expected_rotation, expected_translation) in (
|
||||
self._task_reference_relative_poses.items()
|
||||
):
|
||||
rotation, translation = _relative_pose(
|
||||
combination[pair[0]], combination[pair[1]]
|
||||
)
|
||||
residual += (
|
||||
float((expected_rotation.inv() * rotation).magnitude())
|
||||
/ self.return_reference_rotation_scale_rad
|
||||
+ float(np.linalg.norm(translation - expected_translation))
|
||||
/ self.relative_translation_scale_m
|
||||
)
|
||||
return residual
|
||||
|
||||
def _coupled_rotation_residuals(
|
||||
self, combination: Mapping[str, SquareTagPose]
|
||||
) -> tuple[float, ...]:
|
||||
if not self.coupled_rotation_pairs:
|
||||
return ()
|
||||
residuals: list[float] = []
|
||||
for (
|
||||
driver_parent,
|
||||
driver_child,
|
||||
follower_parent,
|
||||
follower_child,
|
||||
multiplier,
|
||||
) in self.coupled_rotation_pairs:
|
||||
driver_pair = (driver_parent, driver_child)
|
||||
follower_pair = (follower_parent, follower_child)
|
||||
if (
|
||||
driver_pair not in self._coupled_reference_rotations
|
||||
or follower_pair not in self._coupled_reference_rotations
|
||||
):
|
||||
return ()
|
||||
driver_rotation = _relative_pose(
|
||||
combination[driver_parent], combination[driver_child]
|
||||
)[0]
|
||||
follower_rotation = _relative_pose(
|
||||
combination[follower_parent], combination[follower_child]
|
||||
)[0]
|
||||
driver_travel = (
|
||||
self._coupled_reference_rotations[driver_pair].inv()
|
||||
* driver_rotation
|
||||
).magnitude()
|
||||
follower_travel = (
|
||||
self._coupled_reference_rotations[follower_pair].inv()
|
||||
* follower_rotation
|
||||
).magnitude()
|
||||
residuals.append(
|
||||
abs(float(follower_travel) - multiplier * float(driver_travel))
|
||||
)
|
||||
return tuple(residuals)
|
||||
|
||||
def _informative_coupled_rotation_costs(
|
||||
self,
|
||||
combinations: Sequence[Mapping[str, SquareTagPose]],
|
||||
) -> tuple[float, ...]:
|
||||
"""Return branch costs only while the weak coupling prior is credible.
|
||||
|
||||
The URDF mimic ratio is useful for distinguishing two planar-IPPE
|
||||
branches, but it is not measurement truth for a passive joint. Once
|
||||
every otherwise viable combination disagrees with that ratio, using
|
||||
it would bias the measured curve (and previously rejected every
|
||||
frame). In that case fall back to visual continuity for this frame.
|
||||
"""
|
||||
residuals = tuple(
|
||||
self._coupled_rotation_residuals(combination)
|
||||
for combination in combinations
|
||||
)
|
||||
if not residuals or not any(residuals):
|
||||
return tuple(0.0 for _ in combinations)
|
||||
if (
|
||||
self.maximum_coupled_rotation_residual_rad is not None
|
||||
and not any(
|
||||
values
|
||||
and max(values)
|
||||
<= self.maximum_coupled_rotation_residual_rad
|
||||
for values in residuals
|
||||
)
|
||||
):
|
||||
return tuple(0.0 for _ in combinations)
|
||||
return tuple(
|
||||
sum(values) / self.coupled_rotation_scale_rad
|
||||
for values in residuals
|
||||
)
|
||||
|
||||
def _return_reference(
|
||||
self, command_u8: int | None
|
||||
) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]:
|
||||
if command_u8 is None or not self._decreasing_relative_rotations:
|
||||
return {}
|
||||
command = int(command_u8)
|
||||
nearest = min(
|
||||
self._decreasing_relative_rotations,
|
||||
key=lambda candidate: abs(candidate - command),
|
||||
)
|
||||
if (
|
||||
abs(nearest - command)
|
||||
> self.return_reference_maximum_command_gap_u8
|
||||
):
|
||||
return {}
|
||||
references = self._decreasing_relative_rotations[nearest]
|
||||
commands = sorted(self._decreasing_relative_rotations)
|
||||
axes: dict[tuple[str, str], np.ndarray | None] = {}
|
||||
for pair in self.adjacent_pairs:
|
||||
endpoint_delta = (
|
||||
self._decreasing_relative_rotations[commands[-1]][pair].inv()
|
||||
* self._decreasing_relative_rotations[commands[0]][pair]
|
||||
).as_rotvec()
|
||||
norm = float(np.linalg.norm(endpoint_delta))
|
||||
axes[pair] = (
|
||||
None
|
||||
if norm < math.radians(5.0)
|
||||
else endpoint_delta / norm
|
||||
)
|
||||
return {
|
||||
pair: (rotation, axes[pair])
|
||||
for pair, rotation in references.items()
|
||||
}
|
||||
|
||||
def _return_reference_cost(
|
||||
self,
|
||||
combination: Mapping[str, SquareTagPose],
|
||||
reference: Mapping[
|
||||
tuple[str, str], tuple[Rotation, np.ndarray | None]
|
||||
],
|
||||
) -> float:
|
||||
residual = 0.0
|
||||
for pair, (expected, motion_axis) in reference.items():
|
||||
vector = (
|
||||
expected.inv()
|
||||
* _relative_pose(
|
||||
combination[pair[0]], combination[pair[1]]
|
||||
)[0]
|
||||
).as_rotvec()
|
||||
if motion_axis is not None:
|
||||
# The outbound trajectory identifies the physical one-DOF
|
||||
# motion axis. Do not penalize return travel along that axis:
|
||||
# it may contain real mechanical hysteresis that calibration
|
||||
# must measure. A planar-IPPE mirror branch appears primarily
|
||||
# as a large orthogonal tilt and is rejected by this residual.
|
||||
vector = vector - motion_axis * float(vector @ motion_axis)
|
||||
residual += float(np.linalg.norm(vector))
|
||||
return residual / self.return_reference_rotation_scale_rad
|
||||
|
||||
def select(
|
||||
self,
|
||||
candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
|
||||
*,
|
||||
stamp_ns: int,
|
||||
trajectory_command_u8: int | None = None,
|
||||
trajectory_direction: str | None = None,
|
||||
) -> tuple[dict[str, SquareTagPose] | None, str]:
|
||||
"""Return one mutually consistent pose for every configured role."""
|
||||
direction = (
|
||||
None
|
||||
if trajectory_direction is None
|
||||
else str(trajectory_direction)
|
||||
)
|
||||
if direction not in {None, "decreasing", "increasing"}:
|
||||
raise ValueError(
|
||||
"trajectory_direction must be decreasing or increasing"
|
||||
)
|
||||
return_reference = (
|
||||
self._return_reference(trajectory_command_u8)
|
||||
if direction == "increasing"
|
||||
else {}
|
||||
)
|
||||
candidate_lists = [
|
||||
tuple(candidates_by_role.get(role, ()))
|
||||
for role in self.roles
|
||||
@@ -1175,6 +1441,15 @@ class SquareTagGroupPoseTracker:
|
||||
normal_alignment_scale_rad=(
|
||||
self.normal_alignment_scale_rad
|
||||
),
|
||||
task_reference_pairs=(
|
||||
self._task_reference_relative_poses
|
||||
),
|
||||
task_reference_rotation_scale_rad=(
|
||||
self.return_reference_rotation_scale_rad
|
||||
),
|
||||
task_reference_translation_scale_m=(
|
||||
self.relative_translation_scale_m
|
||||
),
|
||||
)
|
||||
)
|
||||
selected = selected_path[-1]
|
||||
@@ -1195,23 +1470,31 @@ class SquareTagGroupPoseTracker:
|
||||
):
|
||||
return None, "group_normal_alignment"
|
||||
else:
|
||||
coupling_costs = self._informative_coupled_rotation_costs(
|
||||
combinations
|
||||
)
|
||||
selected = min(
|
||||
combinations,
|
||||
key=lambda combination: (
|
||||
zip(combinations, coupling_costs),
|
||||
key=lambda item: (
|
||||
sum(
|
||||
pose.reprojection_error_px
|
||||
for pose in combination.values()
|
||||
for pose in item[0].values()
|
||||
)
|
||||
/ self.reprojection_scale_px
|
||||
+ sum(
|
||||
_normal_alignment_rad(
|
||||
combination[first], combination[second]
|
||||
item[0][first], item[0][second]
|
||||
)
|
||||
for first, second in self.normal_alignment_pairs
|
||||
)
|
||||
/ self.normal_alignment_scale_rad
|
||||
+ self._return_reference_cost(
|
||||
item[0], return_reference
|
||||
)
|
||||
+ item[1]
|
||||
+ self._task_reference_cost(item[0])
|
||||
),
|
||||
)
|
||||
)[0]
|
||||
maximum_alignment = max(
|
||||
(
|
||||
_normal_alignment_rad(
|
||||
@@ -1238,7 +1521,7 @@ class SquareTagGroupPoseTracker:
|
||||
)
|
||||
for pair in self.adjacent_pairs
|
||||
}
|
||||
scored: list[tuple[float, dict[str, SquareTagPose]]] = []
|
||||
base_scored: list[tuple[float, dict[str, SquareTagPose]]] = []
|
||||
for combination in combinations:
|
||||
absolute_rotation_motion = 0.0
|
||||
absolute_translation_motion = 0.0
|
||||
@@ -1305,11 +1588,23 @@ class SquareTagGroupPoseTracker:
|
||||
+ relative_translation_motion
|
||||
/ self.relative_translation_scale_m
|
||||
+ self.reprojection_weight * reprojection_penalty
|
||||
+ self._return_reference_cost(
|
||||
combination, return_reference
|
||||
)
|
||||
)
|
||||
scored.append((float(score), combination))
|
||||
base_scored.append((float(score), combination))
|
||||
|
||||
if not scored:
|
||||
if not base_scored:
|
||||
return None, "group_pose_jump"
|
||||
coupling_costs = self._informative_coupled_rotation_costs(
|
||||
[combination for _, combination in base_scored]
|
||||
)
|
||||
scored = [
|
||||
(base_score + coupling_cost, combination)
|
||||
for (base_score, combination), coupling_cost in zip(
|
||||
base_scored, coupling_costs
|
||||
)
|
||||
]
|
||||
selected = min(scored, key=lambda item: item[0])[1]
|
||||
|
||||
aligned: dict[str, SquareTagPose] = {}
|
||||
@@ -1343,4 +1638,39 @@ class SquareTagGroupPoseTracker:
|
||||
|
||||
self._previous = aligned
|
||||
self._previous_stamp_ns = stamp
|
||||
if (
|
||||
direction == "decreasing"
|
||||
and trajectory_command_u8 is not None
|
||||
and not self._coupled_reference_rotations
|
||||
):
|
||||
for (
|
||||
driver_parent,
|
||||
driver_child,
|
||||
follower_parent,
|
||||
follower_child,
|
||||
_multiplier,
|
||||
) in self.coupled_rotation_pairs:
|
||||
for pair in (
|
||||
(driver_parent, driver_child),
|
||||
(follower_parent, follower_child),
|
||||
):
|
||||
self._coupled_reference_rotations[pair] = _relative_pose(
|
||||
aligned[pair[0]], aligned[pair[1]]
|
||||
)[0]
|
||||
if direction == "decreasing" and trajectory_command_u8 is not None:
|
||||
self._decreasing_relative_rotations[
|
||||
int(trajectory_command_u8)
|
||||
] = {
|
||||
pair: _relative_pose(
|
||||
aligned[pair[0]], aligned[pair[1]]
|
||||
)[0]
|
||||
for pair in self.adjacent_pairs
|
||||
}
|
||||
if not self._task_reference_relative_poses:
|
||||
self._task_reference_relative_poses = {
|
||||
pair: _relative_pose(
|
||||
aligned[pair[0]], aligned[pair[1]]
|
||||
)
|
||||
for pair in self.adjacent_pairs
|
||||
}
|
||||
return dict(aligned), ""
|
||||
|
||||
@@ -0,0 +1,237 @@
|
||||
"""Immutable G20 right-hand product identity and one-command preflight."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import hashlib
|
||||
from pathlib import Path
|
||||
import re
|
||||
from typing import Any, Mapping
|
||||
|
||||
import yaml
|
||||
|
||||
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
def sha256_file(path: str | Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with Path(path).open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _mapping(value: Any, name: str) -> Mapping[str, Any]:
|
||||
if not isinstance(value, Mapping):
|
||||
raise ValueError(f"{name} must be a mapping")
|
||||
return value
|
||||
|
||||
|
||||
def _resolve_path(value: Any, *, workspace: Path, name: str) -> Path:
|
||||
text = str(value).strip()
|
||||
if not text:
|
||||
raise ValueError(f"{name} is required")
|
||||
candidate = Path(text).expanduser()
|
||||
if not candidate.is_absolute():
|
||||
candidate = workspace / candidate
|
||||
return candidate.resolve()
|
||||
|
||||
|
||||
def _camera_info_fingerprint(path: Path) -> str:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
payload = _mapping(yaml.safe_load(stream), str(path))
|
||||
|
||||
def values(key: str) -> list[float]:
|
||||
item = _mapping(payload.get(key, {}), key)
|
||||
return [float(value) for value in item.get("data", [])]
|
||||
|
||||
return camera_info_fingerprint(
|
||||
width=int(payload["image_width"]),
|
||||
height=int(payload["image_height"]),
|
||||
camera_matrix=values("camera_matrix"),
|
||||
distortion=values("distortion_coefficients"),
|
||||
rectification=values("rectification_matrix"),
|
||||
projection=values("projection_matrix"),
|
||||
)
|
||||
|
||||
|
||||
def _tag_ids(path: Path) -> set[int]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
payload = _mapping(yaml.safe_load(stream), str(path))
|
||||
result: set[int] = set()
|
||||
for node in payload.values():
|
||||
parameters = _mapping(_mapping(node, "tag node").get("ros__parameters"), "ros__parameters")
|
||||
tag = _mapping(parameters.get("tag"), "tag")
|
||||
ids = [int(value) for value in tag.get("ids", [])]
|
||||
if result.intersection(ids):
|
||||
raise ValueError("Tag IDs must be unique across the three views")
|
||||
result.update(ids)
|
||||
return result
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProductConfig:
|
||||
path: Path
|
||||
workspace: Path
|
||||
serial_number: str
|
||||
can_interface: str
|
||||
source_urdf: Path
|
||||
source_urdf_sha256: str
|
||||
camera_extrinsics: Path
|
||||
camera_extrinsics_sha256: str
|
||||
calibration_config: Path
|
||||
calibration_config_sha256: str
|
||||
tag_config: Path
|
||||
tag_config_sha256: str
|
||||
output_root: Path
|
||||
cameras: Mapping[str, Mapping[str, str]]
|
||||
required_independent_passes: int
|
||||
static_repeatability_rad: float
|
||||
|
||||
@property
|
||||
def session_root(self) -> Path:
|
||||
return self.output_root / self.serial_number
|
||||
|
||||
|
||||
def load_product_config(
|
||||
path: str | Path,
|
||||
*,
|
||||
workspace: str | Path | None = None,
|
||||
check_can: bool = True,
|
||||
) -> ProductConfig:
|
||||
"""Load and fully verify the fixed G20_RIGHT_001 product configuration."""
|
||||
source = Path(path).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
raise ValueError(f"product config does not exist: {source}")
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
raw = _mapping(yaml.safe_load(stream), str(source))
|
||||
if int(raw.get("schema_version", -1)) != 1:
|
||||
raise ValueError("product config schema_version must be 1")
|
||||
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
|
||||
serial = str(raw.get("serial_number", ""))
|
||||
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
||||
raise ValueError("serial_number is invalid")
|
||||
if raw.get("model") != "G20" or raw.get("side") != "right":
|
||||
raise ValueError("product config must describe the G20 right hand")
|
||||
can_interface = str(raw.get("can_interface", "")).strip()
|
||||
if not can_interface:
|
||||
raise ValueError("can_interface is required")
|
||||
if check_can and not (Path("/sys/class/net") / can_interface).exists():
|
||||
raise ValueError(f"CAN interface does not exist: {can_interface}")
|
||||
|
||||
artifacts = _mapping(raw.get("artifacts"), "artifacts")
|
||||
source_urdf = _resolve_path(
|
||||
artifacts.get("source_urdf"), workspace=root, name="source_urdf"
|
||||
)
|
||||
extrinsics = _resolve_path(
|
||||
artifacts.get("camera_extrinsics"),
|
||||
workspace=root,
|
||||
name="camera_extrinsics",
|
||||
)
|
||||
calibration_config = _resolve_path(
|
||||
artifacts.get("calibration_config"),
|
||||
workspace=root,
|
||||
name="calibration_config",
|
||||
)
|
||||
tag_config = _resolve_path(
|
||||
artifacts.get("tag_config"), workspace=root, name="tag_config"
|
||||
)
|
||||
for name, candidate in (
|
||||
("source URDF", source_urdf),
|
||||
("camera extrinsics", extrinsics),
|
||||
("calibration config", calibration_config),
|
||||
("Tag config", tag_config),
|
||||
):
|
||||
if not candidate.is_file():
|
||||
raise ValueError(f"{name} does not exist: {candidate}")
|
||||
|
||||
expected_source_hash = str(artifacts.get("source_urdf_sha256", "")).lower()
|
||||
expected_extrinsics_hash = str(
|
||||
artifacts.get("camera_extrinsics_sha256", "")
|
||||
).lower()
|
||||
expected_tag_hash = str(artifacts.get("tag_config_sha256", "")).lower()
|
||||
expected_calibration_hash = str(
|
||||
artifacts.get("calibration_config_sha256", "")
|
||||
).lower()
|
||||
hashes = (
|
||||
("source URDF", source_urdf, expected_source_hash),
|
||||
("camera extrinsics", extrinsics, expected_extrinsics_hash),
|
||||
("Tag config", tag_config, expected_tag_hash),
|
||||
("calibration config", calibration_config, expected_calibration_hash),
|
||||
)
|
||||
for name, candidate, expected in hashes:
|
||||
if re.fullmatch(r"[0-9a-f]{64}", expected) is None:
|
||||
raise ValueError(f"{name} expected SHA-256 is invalid")
|
||||
actual = sha256_file(candidate)
|
||||
if actual != expected:
|
||||
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
|
||||
|
||||
expected_tag_ids = set(range(19)) - {7, 14, 16, 18}
|
||||
if _tag_ids(tag_config) != expected_tag_ids:
|
||||
raise ValueError(
|
||||
"g20_right_15 Tag config must contain exactly IDs "
|
||||
"0,1,2,3,4,5,6,8,9,10,11,12,13,15,17"
|
||||
)
|
||||
|
||||
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
||||
if set(camera_raw) != set(VIEWS):
|
||||
raise ValueError("cameras must contain front/side/top")
|
||||
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
||||
cameras: dict[str, dict[str, str]] = {}
|
||||
serials: set[str] = set()
|
||||
for view in VIEWS:
|
||||
item = _mapping(camera_raw[view], f"cameras.{view}")
|
||||
camera_serial = str(item.get("serial_number", ""))
|
||||
info_path = _resolve_path(
|
||||
item.get("camera_info"), workspace=root, name=f"{view}.camera_info"
|
||||
)
|
||||
if not info_path.is_file():
|
||||
raise ValueError(f"{view} camera info does not exist: {info_path}")
|
||||
if camera_serial in serials:
|
||||
raise ValueError("the three camera serial numbers must be unique")
|
||||
serials.add(camera_serial)
|
||||
identity = loaded_extrinsics.cameras[view]
|
||||
fingerprint = _camera_info_fingerprint(info_path)
|
||||
if camera_serial != identity.serial_number:
|
||||
raise ValueError(f"{view} camera serial does not match extrinsics")
|
||||
if fingerprint != identity.intrinsics_sha256:
|
||||
raise ValueError(f"{view} camera intrinsics do not match extrinsics")
|
||||
cameras[view] = {
|
||||
"serial_number": camera_serial,
|
||||
"camera_info": str(info_path),
|
||||
"camera_name": str(item.get("camera_name", f"hikrobot_{view}_{camera_serial}")),
|
||||
}
|
||||
|
||||
output_root = _resolve_path(
|
||||
raw.get("output_root", "calibration_output"),
|
||||
workspace=root,
|
||||
name="output_root",
|
||||
)
|
||||
release = _mapping(raw.get("release", {}), "release")
|
||||
passes = int(release.get("required_independent_passes", 2))
|
||||
if passes not in {1, 2}:
|
||||
raise ValueError("required_independent_passes must be 1 or 2")
|
||||
repeatability_deg = float(release.get("static_repeatability_deg", 1.0))
|
||||
if not 0.0 < repeatability_deg <= 5.0:
|
||||
raise ValueError("static_repeatability_deg must be in (0, 5]")
|
||||
return ProductConfig(
|
||||
path=source,
|
||||
workspace=root,
|
||||
serial_number=serial,
|
||||
can_interface=can_interface,
|
||||
source_urdf=source_urdf,
|
||||
source_urdf_sha256=expected_source_hash,
|
||||
camera_extrinsics=extrinsics,
|
||||
camera_extrinsics_sha256=expected_extrinsics_hash,
|
||||
calibration_config=calibration_config,
|
||||
calibration_config_sha256=expected_calibration_hash,
|
||||
tag_config=tag_config,
|
||||
tag_config_sha256=expected_tag_hash,
|
||||
output_root=output_root,
|
||||
cameras=cameras,
|
||||
required_independent_passes=passes,
|
||||
static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0,
|
||||
)
|
||||
@@ -0,0 +1,569 @@
|
||||
"""Validate and atomically publish one inseparable G20 JSON/URDF session."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime, timezone
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
from typing import Any, Mapping, Sequence
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from .full_hand import (
|
||||
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
||||
G20_RIGHT_19_LAYOUT,
|
||||
MIMIC_DERIVED_FINGER_DIPS,
|
||||
get_hand_calibration_profile,
|
||||
validate_compact_payload,
|
||||
)
|
||||
from .product import ProductConfig, sha256_file
|
||||
from .storage import atomic_write_json
|
||||
from .urdf_zero import get_zero_calibration_profile
|
||||
|
||||
|
||||
PASSIVE_JOINTS = frozenset(
|
||||
{"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
||||
)
|
||||
ACTIVE_ZERO_JOINTS = frozenset(
|
||||
get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
|
||||
)
|
||||
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
|
||||
) - ACTIVE_ZERO_JOINTS
|
||||
|
||||
|
||||
def _load_json(path: Path) -> dict[str, Any]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
payload = json.load(stream)
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError(f"{path} must contain a JSON object")
|
||||
return payload
|
||||
|
||||
|
||||
def _joint_blocks(text: str) -> dict[str, str]:
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
return {match.group("name"): match.group(0) for match in pattern.finditer(text)}
|
||||
|
||||
|
||||
def _mask_origin_rpy(block: str) -> str:
|
||||
return re.sub(
|
||||
r"(<origin\b[^>]*\brpy\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_RPY__\2",
|
||||
block,
|
||||
count=1,
|
||||
)
|
||||
|
||||
|
||||
def _mask_active_origin_rpy_fields(text: str) -> str:
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
|
||||
def replace(match: re.Match[str]) -> str:
|
||||
block = match.group(0)
|
||||
if match.group("name") not in ACTIVE_ZERO_JOINTS:
|
||||
return block
|
||||
masked = _mask_origin_rpy(block)
|
||||
if masked == block:
|
||||
raise ValueError(
|
||||
f"active joint {match.group('name')} has no origin.rpy"
|
||||
)
|
||||
return masked
|
||||
|
||||
return pattern.sub(replace, text)
|
||||
|
||||
|
||||
def _triplet(value: str) -> np.ndarray:
|
||||
result = np.asarray([float(item) for item in value.split()], dtype=float)
|
||||
if result.shape != (3,) or not np.all(np.isfinite(result)):
|
||||
raise ValueError(f"invalid URDF triplet: {value}")
|
||||
return result
|
||||
|
||||
|
||||
def _joint_elements(path: str | Path) -> dict[str, ET.Element]:
|
||||
return {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(Path(path)).getroot().findall("joint")
|
||||
}
|
||||
|
||||
|
||||
def _verify_expected_origin_offsets(
|
||||
source: str | Path,
|
||||
corrected: str | Path,
|
||||
expected_offsets_rad: Mapping[str, float],
|
||||
) -> None:
|
||||
offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()}
|
||||
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
|
||||
not math.isfinite(value) for value in offsets.values()
|
||||
):
|
||||
raise ValueError("expected offsets must contain 12 finite static-zero values")
|
||||
before = _joint_elements(source)
|
||||
after = _joint_elements(corrected)
|
||||
maximum_rotation_error = 0.0
|
||||
for name, original_joint in before.items():
|
||||
corrected_joint = after[name]
|
||||
original_origin = original_joint.find("origin")
|
||||
corrected_origin = corrected_joint.find("origin")
|
||||
if original_origin is None or corrected_origin is None:
|
||||
if original_origin is not corrected_origin:
|
||||
raise ValueError(f"corrected URDF changed origin presence for {name}")
|
||||
continue
|
||||
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
|
||||
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
|
||||
if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0):
|
||||
raise ValueError(f"corrected URDF changed origin.xyz for {name}")
|
||||
original_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
corrected_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
expected_rotation = original_rotation
|
||||
if name in offsets:
|
||||
axis_node = original_joint.find("axis")
|
||||
axis = _triplet(
|
||||
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
|
||||
)
|
||||
norm = float(np.linalg.norm(axis))
|
||||
if norm <= 1.0e-12:
|
||||
raise ValueError(f"source URDF joint {name} has a degenerate axis")
|
||||
expected_rotation = expected_rotation * Rotation.from_rotvec(
|
||||
axis / norm * offsets[name]
|
||||
)
|
||||
error = float((expected_rotation.inv() * corrected_rotation).magnitude())
|
||||
maximum_rotation_error = max(maximum_rotation_error, error)
|
||||
# Schema v4 intentionally stores zero offsets at eight decimal places.
|
||||
# Older sessions wrote the corrected URDF from the full-precision solve,
|
||||
# so comparing that file with the published JSON necessarily permits half
|
||||
# of one last-place unit. This is about 2.9e-7 degrees and is far below
|
||||
# any calibration or URDF numerical significance.
|
||||
if maximum_rotation_error > 5.1e-9:
|
||||
raise ValueError(
|
||||
"corrected URDF origin.rpy does not match the published zero offsets"
|
||||
)
|
||||
|
||||
|
||||
def verify_corrected_urdf(
|
||||
source: str | Path,
|
||||
corrected: str | Path,
|
||||
*,
|
||||
expected_offsets_rad: Mapping[str, float] | None = None,
|
||||
) -> tuple[str, ...]:
|
||||
"""Prove that only active-joint origin.rpy attributes changed.
|
||||
|
||||
Passive joint blocks, including their mimic elements, are compared as raw
|
||||
UTF-8 text so formatting and numeric spelling are protected as well.
|
||||
"""
|
||||
source_text = Path(source).read_text(encoding="utf-8")
|
||||
corrected_text = Path(corrected).read_text(encoding="utf-8")
|
||||
before = _joint_blocks(source_text)
|
||||
after = _joint_blocks(corrected_text)
|
||||
if set(before) != set(after):
|
||||
raise ValueError("corrected URDF changed the joint set")
|
||||
# Text patchers may normalize only the final newline. It has no URDF/XML
|
||||
# semantics; every joint block and every non-rpy byte remains guarded
|
||||
# below.
|
||||
if _mask_active_origin_rpy_fields(source_text).rstrip("\r\n") != (
|
||||
_mask_active_origin_rpy_fields(corrected_text).rstrip("\r\n")
|
||||
):
|
||||
raise ValueError("corrected URDF changed content outside active origin.rpy")
|
||||
changed: list[str] = []
|
||||
for name in before:
|
||||
if before[name] == after[name]:
|
||||
continue
|
||||
if name not in ACTIVE_ZERO_JOINTS:
|
||||
raise ValueError(f"corrected URDF changed protected joint {name}")
|
||||
if _mask_origin_rpy(before[name]) != _mask_origin_rpy(after[name]):
|
||||
raise ValueError(f"corrected URDF changed more than origin.rpy for {name}")
|
||||
changed.append(name)
|
||||
for name in PASSIVE_JOINTS:
|
||||
if before.get(name) != after.get(name):
|
||||
raise ValueError(f"passive joint/mimic block changed: {name}")
|
||||
if expected_offsets_rad is not None:
|
||||
_verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
|
||||
return tuple(sorted(changed))
|
||||
|
||||
|
||||
def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
|
||||
"""Return every session-local mesh after proving it resolves safely."""
|
||||
path = Path(urdf).expanduser().resolve()
|
||||
root = ET.parse(path).getroot()
|
||||
resources: dict[str, Path] = {}
|
||||
for mesh in root.findall(".//mesh"):
|
||||
filename = str(mesh.get("filename", "")).strip()
|
||||
if not filename or "://" in filename or filename.startswith("package:"):
|
||||
continue
|
||||
relative = Path(filename)
|
||||
if relative.is_absolute() or ".." in relative.parts:
|
||||
raise ValueError(f"URDF has an unsafe local mesh path: {filename}")
|
||||
resolved = (path.parent / relative).resolve()
|
||||
try:
|
||||
resolved.relative_to(path.parent)
|
||||
except ValueError as error:
|
||||
raise ValueError(f"URDF mesh escapes the session: {filename}") from error
|
||||
if not resolved.is_file():
|
||||
raise ValueError(f"URDF mesh resource is missing: {filename}")
|
||||
resources[relative.as_posix()] = resolved
|
||||
return dict(sorted(resources.items()))
|
||||
|
||||
|
||||
def validate_runtime_curves_against_urdf_limits(
|
||||
payload: Mapping[str, Any], source_urdf: str | Path
|
||||
) -> None:
|
||||
"""Reject a runtime curve whose commanded q leaves a CAD safety limit."""
|
||||
validate_compact_payload(payload)
|
||||
source_joints = _joint_elements(source_urdf)
|
||||
for name, calibration in payload["joints"].items():
|
||||
joint = source_joints.get(str(name))
|
||||
if joint is None:
|
||||
raise ValueError(f"source URDF is missing runtime joint {name}")
|
||||
limit = joint.find("limit")
|
||||
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
|
||||
raise ValueError(f"source URDF joint {name} has no finite position limit")
|
||||
lower = float(limit.get("lower"))
|
||||
upper = float(limit.get("upper"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||
raise ValueError(f"source URDF joint {name} has invalid position limits")
|
||||
curve = np.asarray(calibration["angle_rad"], dtype=float)
|
||||
minimum = float(np.min(curve))
|
||||
maximum = float(np.max(curve))
|
||||
tolerance = 1.0e-7
|
||||
if minimum < lower - tolerance or maximum > upper + tolerance:
|
||||
raise ValueError(
|
||||
f"runtime curve exceeds source URDF limit for {name}: "
|
||||
f"[{minimum:.9g}, {maximum:.9g}] not within "
|
||||
f"[{lower:.9g}, {upper:.9g}]"
|
||||
)
|
||||
|
||||
|
||||
def clamp_compact_payload_to_urdf_limits(
|
||||
payload: Mapping[str, Any], source_urdf: str | Path
|
||||
) -> tuple[dict[str, Any], dict[str, int]]:
|
||||
"""Return a schema-preserving runtime payload bounded by CAD limits."""
|
||||
result = copy.deepcopy(dict(payload))
|
||||
validate_compact_payload(result)
|
||||
source_joints = _joint_elements(source_urdf)
|
||||
clipped_by_joint: dict[str, int] = {}
|
||||
for name, calibration in result["joints"].items():
|
||||
joint = source_joints.get(str(name))
|
||||
limit = None if joint is None else joint.find("limit")
|
||||
if (
|
||||
limit is None
|
||||
or limit.get("lower") is None
|
||||
or limit.get("upper") is None
|
||||
):
|
||||
raise ValueError(f"source URDF joint {name} has no finite position limit")
|
||||
lower = float(limit.get("lower"))
|
||||
upper = float(limit.get("upper"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||
raise ValueError(f"source URDF joint {name} has invalid position limits")
|
||||
source = np.asarray(calibration["angle_rad"], dtype=float)
|
||||
bounded = np.clip(source, lower, upper)
|
||||
count = int(np.count_nonzero(bounded != source))
|
||||
if count:
|
||||
clipped_by_joint[str(name)] = count
|
||||
calibration["angle_rad"] = [
|
||||
round(float(value), 8) for value in bounded
|
||||
]
|
||||
validate_compact_payload(result)
|
||||
return result, clipped_by_joint
|
||||
|
||||
|
||||
def build_mujoco_validation_commands(
|
||||
baseline: Sequence[int],
|
||||
) -> dict[str, Any]:
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 commands")
|
||||
base = [int(value) for value in baseline]
|
||||
if any(not 0 <= value <= 255 for value in base):
|
||||
raise ValueError("baseline commands must be in [0, 255]")
|
||||
|
||||
def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]:
|
||||
values = list(base)
|
||||
for index, value in changes.items():
|
||||
values[int(index)] = int(value)
|
||||
return {"name": name, "label_zh": label, "command_u8": values}
|
||||
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"topic": "/g20/cb_right_hand_control_cmd",
|
||||
"serial_number": "G20_RIGHT_001",
|
||||
"poses": [
|
||||
pose("all_open", "全开", {}),
|
||||
pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}),
|
||||
pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}),
|
||||
pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}),
|
||||
pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}),
|
||||
pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}),
|
||||
pose(
|
||||
"half_grip",
|
||||
"四指半握",
|
||||
{1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160},
|
||||
),
|
||||
pose(
|
||||
"light_pinch",
|
||||
"轻捏",
|
||||
{0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176},
|
||||
),
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path:
|
||||
parent = Path(root).resolve()
|
||||
target = Path(session).resolve()
|
||||
if target.parent != parent:
|
||||
raise ValueError("session pointer target must be a direct child of the serial root")
|
||||
if not target.is_dir():
|
||||
raise ValueError(f"session directory does not exist: {target}")
|
||||
if name not in {"latest_attempt", "latest_passed"}:
|
||||
raise ValueError("unsupported session pointer name")
|
||||
parent.mkdir(parents=True, exist_ok=True)
|
||||
destination = parent / name
|
||||
temporary = parent / f".{name}.{os.getpid()}.tmp"
|
||||
if temporary.is_symlink():
|
||||
temporary.unlink()
|
||||
elif temporary.exists():
|
||||
raise ValueError(f"temporary pointer path is occupied: {temporary}")
|
||||
os.symlink(target.name, temporary, target_is_directory=True)
|
||||
os.replace(temporary, destination)
|
||||
return destination
|
||||
|
||||
|
||||
def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||
validate_compact_payload(payload)
|
||||
result = {
|
||||
name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"])
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
}
|
||||
if set(result) != ACTIVE_ZERO_JOINTS or any(
|
||||
not math.isfinite(value) for value in result.values()
|
||||
):
|
||||
raise ValueError("payload does not contain 12 finite observable zero offsets")
|
||||
return result
|
||||
|
||||
|
||||
def compare_session_offsets(
|
||||
first: Mapping[str, Any],
|
||||
second: Mapping[str, Any],
|
||||
*,
|
||||
maximum_difference_rad: float,
|
||||
) -> dict[str, float]:
|
||||
left = active_offsets(first)
|
||||
right = active_offsets(second)
|
||||
differences = {name: abs(left[name] - right[name]) for name in sorted(left)}
|
||||
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
|
||||
if failed:
|
||||
details = ", ".join(
|
||||
f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items()
|
||||
)
|
||||
raise ValueError(f"independent-session static-zero mismatch: {details}")
|
||||
return differences
|
||||
|
||||
|
||||
def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]:
|
||||
directory = Path(session).resolve()
|
||||
urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf"))
|
||||
if len(urdfs) != 1:
|
||||
raise ValueError("session must contain exactly one corrected URDF")
|
||||
return {
|
||||
"json": directory / f"g20_right_{serial_number}_calibration.json",
|
||||
"urdf": urdfs[0],
|
||||
"summary": directory / "calibration_summary_zh.json",
|
||||
"commands": directory / "mujoco_validation_commands.json",
|
||||
"raw": directory / "raw_samples.jsonl",
|
||||
"log": directory / "calibration.log",
|
||||
}
|
||||
|
||||
|
||||
def find_compatible_prior_session(
|
||||
config: ProductConfig,
|
||||
current: str | Path,
|
||||
current_payload: Mapping[str, Any],
|
||||
) -> tuple[Path | None, dict[str, float]]:
|
||||
current_path = Path(current).resolve()
|
||||
for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True):
|
||||
if candidate.resolve() == current_path:
|
||||
continue
|
||||
json_path = candidate / f"g20_right_{config.serial_number}_calibration.json"
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
if not json_path.is_file() or not summary_path.is_file():
|
||||
continue
|
||||
try:
|
||||
summary = _load_json(summary_path)
|
||||
if not bool(summary.get("quality", {}).get("passed")):
|
||||
continue
|
||||
payload = _load_json(json_path)
|
||||
differences = compare_session_offsets(
|
||||
payload,
|
||||
current_payload,
|
||||
maximum_difference_rad=config.static_repeatability_rad,
|
||||
)
|
||||
except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError):
|
||||
continue
|
||||
return candidate, differences
|
||||
return None, {}
|
||||
|
||||
|
||||
def _verify_combination_validation(combination: Mapping[str, Any]) -> None:
|
||||
# The formal product uses the independently held-out fourth sweep cycle.
|
||||
# The optional eight-pose check is a developer diagnostic because the
|
||||
# 15-Tag layout has no DIP Tags and the axis-line zero solve does not
|
||||
# establish an absolute Cartesian hand-base transform. When explicitly
|
||||
# enabled, retain its strict coverage and error checks.
|
||||
if not bool(combination.get("enabled")):
|
||||
return
|
||||
position_p95 = float(combination.get("position_p95_m", float("inf")))
|
||||
orientation_p95 = float(
|
||||
combination.get("orientation_p95_rad", float("inf"))
|
||||
)
|
||||
required = {str(value) for value in combination.get("required_targets", ())}
|
||||
expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS)
|
||||
observations = combination.get("observation_counts")
|
||||
validations = combination.get("validation_counts")
|
||||
if required != expected:
|
||||
raise ValueError("combination validation has the wrong required-target set")
|
||||
if not isinstance(observations, Mapping) or not isinstance(validations, Mapping):
|
||||
raise ValueError("combination validation is missing per-target coverage")
|
||||
missing_observations = sorted(
|
||||
key for key in expected if int(observations.get(key, 0)) < 2
|
||||
)
|
||||
missing_validations = sorted(
|
||||
key for key in expected if int(validations.get(key, 0)) < 1
|
||||
)
|
||||
if missing_observations or missing_validations:
|
||||
raise ValueError(
|
||||
"combination validation target coverage is incomplete: "
|
||||
f"observations={','.join(missing_observations) or '-'}; "
|
||||
f"validations={','.join(missing_validations) or '-'}"
|
||||
)
|
||||
if (
|
||||
not bool(combination.get("completed"))
|
||||
or int(combination.get("completed_poses", 0)) != 8
|
||||
or position_p95 > 0.003
|
||||
or orientation_p95 > math.radians(2.0)
|
||||
):
|
||||
raise ValueError(
|
||||
"eight-pose four-finger combination validation is incomplete or failed"
|
||||
)
|
||||
|
||||
|
||||
def finalize_session_artifacts(
|
||||
config: ProductConfig,
|
||||
session: str | Path,
|
||||
*,
|
||||
node_status: Mapping[str, Any],
|
||||
) -> tuple[dict[str, Any], bool]:
|
||||
"""Create diagnostics and update latest_passed only after every guard."""
|
||||
directory = Path(session).resolve()
|
||||
paths = session_artifact_paths(directory, config.serial_number)
|
||||
for name in ("json", "urdf", "raw", "log"):
|
||||
if not paths[name].is_file():
|
||||
raise ValueError(f"session is missing {name}: {paths[name]}")
|
||||
payload = _load_json(paths["json"])
|
||||
validate_compact_payload(payload)
|
||||
if payload.get("schema_version") != 4 or payload.get("side") != "right":
|
||||
raise ValueError("runtime JSON is not the compact right-hand schema v4")
|
||||
if not bool(payload.get("quality", {}).get("passed")):
|
||||
raise ValueError("runtime JSON quality is not passed")
|
||||
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
|
||||
payload, config.source_urdf
|
||||
)
|
||||
if clipped_runtime_joints:
|
||||
# A node from before runtime-limit saturation may already have written
|
||||
# an otherwise valid commit candidate. Normalize it before hashing
|
||||
# and publication so completed sessions can be recovered offline.
|
||||
atomic_write_json(paths["json"], payload)
|
||||
combination = node_status.get("combination_validation")
|
||||
if not isinstance(combination, Mapping):
|
||||
raise ValueError("node status is missing combination validation")
|
||||
_verify_combination_validation(combination)
|
||||
offsets = active_offsets(payload)
|
||||
changed_joints = verify_corrected_urdf(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
)
|
||||
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
|
||||
mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
|
||||
mesh_hashes = {
|
||||
name: sha256_file(path) for name, path in mesh_resources.items()
|
||||
}
|
||||
commands = build_mujoco_validation_commands(payload["baseline_command_u8"])
|
||||
atomic_write_json(paths["commands"], commands)
|
||||
|
||||
prior, differences = find_compatible_prior_session(config, directory, payload)
|
||||
release_ready = config.required_independent_passes == 1 or prior is not None
|
||||
quality = dict(payload["quality"])
|
||||
summary: dict[str, Any] = {
|
||||
"schema_version": 1,
|
||||
"serial_number": config.serial_number,
|
||||
"session_id": f"{config.serial_number}_{directory.name}",
|
||||
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
|
||||
"quality": quality,
|
||||
"runtime_limit_clipped_bins": clipped_runtime_joints,
|
||||
"runtime_curve_domain": "requested_command_u8",
|
||||
"static_zero_calibrated_joints": sorted(ACTIVE_ZERO_JOINTS),
|
||||
"retained_active_urdf_zero_joints": sorted(
|
||||
RETAINED_ACTIVE_ZERO_JOINTS
|
||||
),
|
||||
"retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
|
||||
"mimic_derived_dynamic_joints": sorted(MIMIC_DERIVED_FINGER_DIPS),
|
||||
"changed_urdf_joint_origins": list(changed_joints),
|
||||
"hashes": {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
"corrected_urdf_sha256": sha256_file(paths["urdf"]),
|
||||
"mesh_resources_sha256": mesh_hashes,
|
||||
"calibration_json_sha256": sha256_file(paths["json"]),
|
||||
"raw_samples_sha256": sha256_file(paths["raw"]),
|
||||
},
|
||||
"holdout": {
|
||||
"training_cycles": [0, 1, 2],
|
||||
"validation_cycle": 3,
|
||||
"isolated": True,
|
||||
},
|
||||
"combination_validation": dict(combination),
|
||||
"formal_release": {
|
||||
"passed": release_ready,
|
||||
"required_independent_passes": config.required_independent_passes,
|
||||
"comparison_session": None if prior is None else prior.name,
|
||||
"maximum_static_difference_deg": (
|
||||
None if not differences else math.degrees(max(differences.values()))
|
||||
),
|
||||
},
|
||||
"node_status": dict(node_status),
|
||||
"created_at_utc": datetime.now(timezone.utc).isoformat(),
|
||||
"artifacts": {name: path.name for name, path in paths.items()},
|
||||
}
|
||||
atomic_write_json(paths["summary"], summary)
|
||||
# Recompute hashes after all files exist and validate the pair once more
|
||||
# immediately before the one atomic publication operation.
|
||||
if sha256_file(config.source_urdf) != config.source_urdf_sha256:
|
||||
raise ValueError("source URDF changed during calibration")
|
||||
verify_corrected_urdf(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
)
|
||||
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
|
||||
final_mesh_hashes = {
|
||||
name: sha256_file(path)
|
||||
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
||||
}
|
||||
if final_mesh_hashes != mesh_hashes:
|
||||
raise ValueError("URDF mesh resources changed during publication")
|
||||
if release_ready:
|
||||
atomic_session_pointer(config.session_root, "latest_passed", directory)
|
||||
return summary, release_ready
|
||||
+195
-12
@@ -48,6 +48,10 @@ JOINT_NAMES_ZH = {
|
||||
"pinky_pip": "小指PIP",
|
||||
"pinky_dip": "小指DIP(被动)",
|
||||
"thumb_cmc_yaw": "拇指CMC侧摆",
|
||||
"index_mcp_roll_side": "食指MCP侧摆(侧面校验)",
|
||||
"middle_mcp_roll_side": "中指MCP侧摆(侧面校验)",
|
||||
"ring_mcp_roll_side": "无名指MCP侧摆(侧面校验)",
|
||||
"pinky_mcp_roll_side": "小指MCP侧摆(侧面校验)",
|
||||
}
|
||||
|
||||
|
||||
@@ -86,6 +90,12 @@ def _task_text(active: Mapping[str, Any]) -> str:
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}"
|
||||
)
|
||||
if active.get("kind") == "cross_view_roll_diagnostic":
|
||||
return (
|
||||
f"{active.get('finger', '?')}侧摆跨机位诊断完成:"
|
||||
f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°,"
|
||||
f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°"
|
||||
)
|
||||
if active.get("kind") == "validation":
|
||||
return (
|
||||
f"{view}机位,随机复测,电机{active.get('motor_index')},"
|
||||
@@ -132,10 +142,19 @@ def three_camera_reason_zh(
|
||||
context = fields[1] if len(fields) > 1 else "unknown"
|
||||
error_match = re.search(r"error_u8=([0-9.]+)", reason)
|
||||
error = error_match.group(1) if error_match else "未知"
|
||||
timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason)
|
||||
timeout_value = active.get("timeout_seconds")
|
||||
if timeout_value is None and timeout_match is not None:
|
||||
timeout_value = float(timeout_match.group(1))
|
||||
duration = (
|
||||
f"连续{float(timeout_value):g}秒"
|
||||
if timeout_value is not None
|
||||
else "在规定时间内"
|
||||
)
|
||||
motor = active.get("motor_index")
|
||||
if motor is not None:
|
||||
return (
|
||||
f"电机{motor}反馈连续8秒没有向目标推进;目标"
|
||||
f"电机{motor}反馈{duration}没有向目标推进;目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8,"
|
||||
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
|
||||
@@ -144,12 +163,29 @@ def three_camera_reason_zh(
|
||||
"重启;若仍在变化或有摩擦,则先排查机械问题,不要反复resume强推。",
|
||||
)
|
||||
return (
|
||||
f"电机反馈连续8秒没有向目标推进;停止位置距目标{error}个u8"
|
||||
f"电机反馈{duration}没有向目标推进;停止位置距目标{error}个u8"
|
||||
f"(阶段={context})。程序已保持当前位置,防止机械碰撞或摩擦加重。",
|
||||
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
|
||||
"正常端点,应配置该电机专用端点容差后重启标定;不要反复调用resume强推。",
|
||||
)
|
||||
|
||||
base_reason, separator, reason_detail = reason.partition(":")
|
||||
if base_reason in {
|
||||
"sweep_missing_endpoint_bin",
|
||||
"sweep_bins_too_few",
|
||||
"sweep_bin_gap_too_large",
|
||||
"task_precheck_missing_command_127",
|
||||
"task_precheck_detection_rate_too_low",
|
||||
"synchronised_tag_state_timeout",
|
||||
}:
|
||||
reason = base_reason
|
||||
detail_label = (
|
||||
JOINT_NAMES_ZH.get(reason_detail, reason_detail)
|
||||
if separator and reason_detail
|
||||
else ""
|
||||
)
|
||||
detail_prefix = f"{detail_label}:" if detail_label else ""
|
||||
|
||||
if "URDF zero offset reached the configured" in reason:
|
||||
bound_match = re.search(
|
||||
r"configured\s+([0-9.]+)\s+degree bound", reason
|
||||
@@ -174,7 +210,7 @@ def three_camera_reason_zh(
|
||||
if reason == "sweep_missing_endpoint_bin":
|
||||
missing_text = "、".join(str(value) for value in missing) or "0或255"
|
||||
return (
|
||||
f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据,但缺少"
|
||||
f"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据,但缺少"
|
||||
f"电机端点{missing_text}附近的有效分箱;采样到的实际电机范围为"
|
||||
f"{sample_range},端点容差为±{tolerance}。这通常表示电机虽然运动到"
|
||||
"端点,但该时刻没有同时取得有效Tag图像和电机状态。",
|
||||
@@ -183,29 +219,64 @@ def three_camera_reason_zh(
|
||||
)
|
||||
if reason == "sweep_bins_too_few":
|
||||
return (
|
||||
f"有效电机分箱只有{sample.get('bin_count', 0)}个,要求至少"
|
||||
f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个,要求至少"
|
||||
f"{sample.get('minimum_bin_count', '?')}个;当前采样范围{sample_range}。",
|
||||
"检查Tag连续识别和电机状态频率,修正后调用resume重新扫描当前方向。",
|
||||
)
|
||||
if reason == "sweep_bin_gap_too_large":
|
||||
gap_start = sample.get("maximum_bin_gap_start_u8")
|
||||
gap_end = sample.get("maximum_bin_gap_end_u8")
|
||||
gap_range = (
|
||||
""
|
||||
if gap_start is None or gap_end is None
|
||||
else f"({gap_start}→{gap_end})"
|
||||
)
|
||||
return (
|
||||
f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')},"
|
||||
f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
|
||||
f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')},"
|
||||
f"{gap_range}允许值不超过"
|
||||
f"{sample.get('allowed_maximum_bin_gap', '?')}。",
|
||||
"检查运动中Tag是否间歇丢失;修正遮挡、反光或对焦后调用resume。",
|
||||
)
|
||||
if reason == "synchronised_tag_state_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
if isinstance(group_reasons, Mapping) and group_reasons:
|
||||
reason_text = "、".join(
|
||||
f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}"
|
||||
for view, value in group_reasons.items()
|
||||
)
|
||||
return (
|
||||
f"{detail_prefix}Tag仍可见且反馈正常,但连续图像帧被整组PnP几何检查拒绝"
|
||||
f"({reason_text}),因此无法与电机状态形成有效轨迹帧。",
|
||||
"不要调整或反复粘贴Tag;保留当前会话并复制诊断块给开发者检查PnP分支逻辑。",
|
||||
)
|
||||
return (
|
||||
"运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
"按时间戳配对”的图像帧。",
|
||||
"查看下面活动机位的缺失Tag,确认状态话题仍在更新;修正后调用resume,"
|
||||
"程序会重扫当前方向。",
|
||||
)
|
||||
if reason == "task_precheck_missing_command_127":
|
||||
return (
|
||||
f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。",
|
||||
"检查中位姿态的Tag遮挡和反光;程序只会重扫当前物理任务。",
|
||||
)
|
||||
if reason == "task_precheck_detection_rate_too_low":
|
||||
return (
|
||||
f"{detail_prefix}低速预检的有效Tag识别率低于门限。",
|
||||
"检查该机位当前任务Tag的遮挡、反光和对焦;程序只会重扫当前物理任务。",
|
||||
)
|
||||
if reason == "sweep_start_position_timeout":
|
||||
return (
|
||||
f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
|
||||
f"{active.get('start_u8')},当前实际值{_format_u8(active.get('actual_u8'))}。",
|
||||
"检查CAN、机械手使能和是否存在机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "sweep_start_tag_timeout":
|
||||
return (
|
||||
"被测电机已经到达扫描起点,但当前任务所需的实时运动Tag没有形成足够的"
|
||||
"同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”,不会触发此错误。",
|
||||
"只检查标记为✗的实时运动Tag、反光和外部遮挡;不要移动相机或手掌底座。",
|
||||
)
|
||||
if reason == "sweep_timeout":
|
||||
return (
|
||||
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
|
||||
@@ -226,7 +297,7 @@ def three_camera_reason_zh(
|
||||
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
||||
"检查当前机位Tag可见性后调用resume。",
|
||||
)
|
||||
if reason == "joint_fit_check_failed":
|
||||
if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
|
||||
metric_names = {
|
||||
"plane_rms_mm": "平面拟合RMS",
|
||||
"radial_rms_mm": "圆半径拟合RMS",
|
||||
@@ -237,6 +308,9 @@ def three_camera_reason_zh(
|
||||
"arc_deg": "实测圆弧",
|
||||
"monotonic_correction_deg": "最大单调修正",
|
||||
"hysteresis_deg": "最大正反程差",
|
||||
"baseline_hysteresis_deg": "baseline正反程关节角差",
|
||||
"baseline_directional_gap_deg": "baseline方向分支间隙",
|
||||
"baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差",
|
||||
"cycle_travel_range_deg": "三轮行程差",
|
||||
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
|
||||
"axis_plane_rms_mm": "三维圆轴向RMS",
|
||||
@@ -259,6 +333,9 @@ def three_camera_reason_zh(
|
||||
"arc_deg": "°",
|
||||
"monotonic_correction_deg": "°",
|
||||
"hysteresis_deg": "°",
|
||||
"baseline_hysteresis_deg": "°",
|
||||
"baseline_directional_gap_deg": "°",
|
||||
"baseline_directional_gap_range_deg": "°",
|
||||
"cycle_travel_range_deg": "°",
|
||||
"rotation_orthogonal_rms_deg": "°",
|
||||
"axis_plane_rms_mm": "mm",
|
||||
@@ -288,9 +365,14 @@ def three_camera_reason_zh(
|
||||
)
|
||||
cycle_travel = failure.get("cycle_travel_deg", [])
|
||||
if cycle_travel:
|
||||
detail += "(三轮=" + "/".join(
|
||||
detail += "(各轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_travel
|
||||
) + ")"
|
||||
cycle_values = failure.get("cycle_values_deg", [])
|
||||
if cycle_values:
|
||||
detail += "(各轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_values
|
||||
) + ")"
|
||||
details.append(detail)
|
||||
else:
|
||||
cycle = failure.get("cycle")
|
||||
@@ -300,11 +382,30 @@ def three_camera_reason_zh(
|
||||
f"{failure.get('reason', '未知原因')}"
|
||||
)
|
||||
detail_text = ";".join(details) or "当前关节的轨迹拟合未通过"
|
||||
directional_gap_failure = any(
|
||||
str(failure.get("metric", "")).startswith(
|
||||
"baseline_directional_gap"
|
||||
)
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
if reason == "joint_fit_systematic_failure":
|
||||
suggestion = (
|
||||
"各轮重复出现同一模型冲突,继续运动不会改善;程序已禁止自动重扫。"
|
||||
"请直接复制诊断块给开发者,不要放宽门限。"
|
||||
)
|
||||
else:
|
||||
suggestion = (
|
||||
"方向分支已由软件保留,不要放宽门限;请检查传动回差或高支架刚度,"
|
||||
"处理后重新执行一键标定命令,程序会从最近可靠断点继续。"
|
||||
if directional_gap_failure
|
||||
else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令;"
|
||||
"程序只清除当前失败关节的数据并重扫"
|
||||
f"{active.get('directions_to_rescan', 6)}个方向,"
|
||||
"不需要手工调用ROS服务。"
|
||||
)
|
||||
return (
|
||||
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
|
||||
"修正Tag位置、遮挡或机械行程后调用"
|
||||
"/g20_calibration/resume;程序只清除当前失败关节的数据"
|
||||
f"并重扫{active.get('directions_to_rescan', 6)}个方向,不要调用start。",
|
||||
suggestion,
|
||||
)
|
||||
if reason == "zero_model_validation_failed":
|
||||
reason_names = {
|
||||
@@ -345,11 +446,37 @@ def three_camera_reason_zh(
|
||||
"该类稳定模型失败不能靠重复运动修复,程序不会自动重扫;"
|
||||
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
|
||||
)
|
||||
if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪;此阶段不以Tag可见性阻止基准恢复。",
|
||||
"保持机械手运动范围无障碍;设备就绪后系统会先安全恢复基准形态,再检查掌部Tag。",
|
||||
)
|
||||
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查;全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "waiting_for_baseline_tags_after_recovery":
|
||||
return (
|
||||
"机械手已经稳定恢复到基准形态,正在用新采集的画面确认三台相机各自的固定掌部Tag。",
|
||||
"若某个掌部Tag持续缺失,只调整遮挡手指或检查Tag固定情况,不要移动相机和手掌底座。",
|
||||
)
|
||||
if reason == "locking_fixed_base_references":
|
||||
return (
|
||||
"基准形态Tag预检已通过,正在把三台相机的固定掌部Tag稳健锁定为本会话参考。",
|
||||
"无需操作;锁定完成后允许任务姿态遮挡固定掌部Tag。",
|
||||
)
|
||||
if reason == "waiting_for_task_tags_at_sweep_start":
|
||||
return (
|
||||
"电机已到扫描起点,正在等待当前任务的实时运动Tag;显示为“锁”的固定掌部Tag"
|
||||
"允许被手指遮挡。",
|
||||
"只检查标记为✗的实时运动Tag;若均为✓或锁,程序会自动开始运动。",
|
||||
)
|
||||
if reason == "call_start_for_baseline_recovery":
|
||||
return (
|
||||
"相机数据和机械手反馈已就绪,等待一键程序触发安全基准恢复。",
|
||||
"保持机械手运动范围无障碍;程序会自动开始,无需手工调用ROS服务。",
|
||||
)
|
||||
if reason == "call_start":
|
||||
return (
|
||||
"三机位预检已经通过,等待操作员确认开始。",
|
||||
@@ -361,14 +488,62 @@ def three_camera_reason_zh(
|
||||
return "操作员终止了本次标定,程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
|
||||
if reason == "collecting_timestamp_synchronised_tag_centres":
|
||||
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作,保持相机、标签和底座不动。"
|
||||
if reason == "collecting_dedicated_baseline_hold":
|
||||
return (
|
||||
"正在从当前方向到达关节baseline并静止采集Tag与电机反馈;这批数据单独用于回差验收。",
|
||||
"无需操作,保持相机、标签和底座不动。",
|
||||
)
|
||||
if reason == "steady checkpoint target is missing":
|
||||
return (
|
||||
"首轮稳态检查点已经到达最终端点,但采集状态没有及时切换到端点完成阶段。",
|
||||
"程序已停止发布并保留已采样数据;这是软件状态切换问题,不需要调整相机、Tag或机械手。",
|
||||
)
|
||||
if reason == "cross_view_roll_front_failure_deferred":
|
||||
return (
|
||||
"正面侧摆回差不合格已保留,诊断模式将继续采集同一手指的侧面数据。",
|
||||
"无需操作;该诊断会锁定URDF发布。",
|
||||
)
|
||||
if reason == "cross_view_roll_diagnostic_complete":
|
||||
interpretation = str(active.get("interpretation", ""))
|
||||
explanations = {
|
||||
"both_views_confirm_direction_dependent_pose": (
|
||||
"正面和侧面都确认了方向相关姿态,优先判断为roll输出机构或共同下游链的真实回差。"
|
||||
),
|
||||
"front_only_difference_check_roll_tag_bracket_or_front_pnp": (
|
||||
"只有正面差异超限,优先检查roll Tag高支架刚度和正面PnP。"
|
||||
),
|
||||
"side_only_difference_check_side_tag_chain_or_side_pnp": (
|
||||
"只有侧面差异超限,优先检查侧面Tag链和侧面PnP。"
|
||||
),
|
||||
"both_views_within_formal_hysteresis_limit": (
|
||||
"两个机位的静止回差均满足正式门限。"
|
||||
),
|
||||
}
|
||||
return (
|
||||
explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。")
|
||||
+ " 本次为诊断会话,不会生成或发布URDF。",
|
||||
"保存当前状态和raw_samples.jsonl;根据两机位结论处理后重新启动正式标定。",
|
||||
)
|
||||
if reason == "capturing_random_validation_pose":
|
||||
return "正在当前随机命令位置采集复测数据。", "无需操作,保持设备不动。"
|
||||
if reason in {"calibration_passed", "calibration_complete"}:
|
||||
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
||||
if reason == "quality_failed":
|
||||
return "标定流程完成,但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
||||
if reason == "combination_pose_prediction_failed":
|
||||
return (
|
||||
"单关节、零位和URDF几何验证已通过,但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
|
||||
"程序会在原姿态重新初始化PnP并自动复测;若最终仍失败,请把raw_samples.jsonl中的"
|
||||
"combination_validation_failure记录交给开发者,不要重新采集16个单关节任务。",
|
||||
)
|
||||
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
|
||||
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
|
||||
if reason == "holding_same_finger_clearance_before_next_task":
|
||||
return (
|
||||
"同一根手指的上一项已经完成;相邻手指继续保持当前避让姿态,只调整"
|
||||
"被测关节以衔接下一项。",
|
||||
"无需操作,不要手动展开正在避让的手指。",
|
||||
)
|
||||
if state == "RETURN_BASELINE":
|
||||
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
@@ -431,6 +606,14 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}~"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
detection_frames = int(active.get("detection_frames", 0))
|
||||
if detection_frames:
|
||||
lines.append(
|
||||
"本方向Tag检出:"
|
||||
f"{float(active.get('detection_rate', 0.0)):.1%}"
|
||||
f"({active.get('detection_valid_frames', 0)}/"
|
||||
f"{detection_frames}帧)"
|
||||
)
|
||||
auxiliary = active.get("auxiliary_motors", [])
|
||||
if auxiliary:
|
||||
lines.append(
|
||||
|
||||
+5770
-353
File diff suppressed because it is too large
Load Diff
@@ -634,6 +634,7 @@ def _fit_joint_curve(
|
||||
values: Sequence[float],
|
||||
*,
|
||||
endpoint_reference: Mapping[str, Sequence[float]] | None = None,
|
||||
preserve_direction_offset: bool = False,
|
||||
) -> tuple[dict[str, Any], float, float]:
|
||||
by_direction: dict[str, list[list[float]]] = {
|
||||
direction: [[] for _ in range(256)] for direction in DIRECTIONS
|
||||
@@ -669,9 +670,11 @@ def _fit_joint_curve(
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
raw -= raw[-1]
|
||||
if not preserve_direction_offset:
|
||||
raw -= raw[-1]
|
||||
projected_samples = isotonic_nonincreasing(raw)
|
||||
projected_samples -= projected_samples[-1]
|
||||
if not preserve_direction_offset:
|
||||
projected_samples -= projected_samples[-1]
|
||||
maximum_correction = max(
|
||||
maximum_correction,
|
||||
float(np.max(np.abs(projected_samples - raw))),
|
||||
@@ -681,7 +684,8 @@ def _fit_joint_curve(
|
||||
commands.astype(float),
|
||||
projected_samples,
|
||||
)
|
||||
curve -= curve[255]
|
||||
if not preserve_direction_offset:
|
||||
curve -= curve[255]
|
||||
if endpoint_reference is not None:
|
||||
curve = _regularize_coupled_zero_tail(
|
||||
curve,
|
||||
@@ -695,7 +699,8 @@ def _fit_joint_curve(
|
||||
increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float)
|
||||
hysteresis = float(np.max(np.abs(decreasing - increasing)))
|
||||
combined = 0.5 * (decreasing + increasing)
|
||||
combined -= combined[255]
|
||||
if not preserve_direction_offset:
|
||||
combined -= combined[255]
|
||||
return (
|
||||
{
|
||||
"angle_rad": [round(float(value), 8) for value in combined],
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,4 +1,4 @@
|
||||
"""Publish calibrated G20 URDF joint angles from raw u8 commands."""
|
||||
"""Publish calibrated G20 URDF angles from raw command/feedback u8 values."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
import hashlib
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
@@ -41,6 +42,28 @@ def _launch_stack(context):
|
||||
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise RuntimeError("hand_type must be left or right")
|
||||
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
|
||||
if tag_layout not in {"legacy_11", "g20_right_15"}:
|
||||
raise RuntimeError("tag_layout must be legacy_11 or g20_right_15")
|
||||
if tag_layout == "g20_right_15" and hand_type != "right":
|
||||
raise RuntimeError("g20_right_15 requires hand_type:=right")
|
||||
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
tag_config = (
|
||||
Path(requested_tag_config).expanduser().resolve()
|
||||
if requested_tag_config
|
||||
else package_share
|
||||
/ "config"
|
||||
/ (
|
||||
"three_camera_tags_g20_right_15.yaml"
|
||||
if tag_layout == "g20_right_15"
|
||||
else "three_camera_tags.yaml"
|
||||
)
|
||||
)
|
||||
if not tag_config.is_file():
|
||||
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
||||
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"
|
||||
@@ -52,6 +75,20 @@ def _launch_stack(context):
|
||||
)
|
||||
if not source_urdf.is_file():
|
||||
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
|
||||
expected_source_hash = LaunchConfiguration(
|
||||
"source_urdf_expected_sha256"
|
||||
).perform(context).strip().lower()
|
||||
if tag_layout == "g20_right_15":
|
||||
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
||||
raise RuntimeError(
|
||||
"g20_right_15 requires source_urdf_expected_sha256 confirmed "
|
||||
"by the CAD/hardware owner"
|
||||
)
|
||||
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
|
||||
if actual_source_hash != expected_source_hash:
|
||||
raise RuntimeError(
|
||||
"source_urdf_expected_sha256 does not match source_urdf_path"
|
||||
)
|
||||
|
||||
hand_serial = LaunchConfiguration("serial_number").perform(context)
|
||||
if (
|
||||
@@ -163,7 +200,7 @@ def _launch_stack(context):
|
||||
name="apriltag",
|
||||
namespace=detector_namespace,
|
||||
parameters=[
|
||||
LaunchConfiguration("tag_config"),
|
||||
str(tag_config),
|
||||
{
|
||||
"detector.decimate": ParameterValue(
|
||||
LaunchConfiguration("apriltag_decimate"),
|
||||
@@ -234,7 +271,11 @@ def _launch_stack(context):
|
||||
{
|
||||
"serial_number": hand_serial,
|
||||
"hand_type": hand_type,
|
||||
"tag_layout": tag_layout,
|
||||
"session_dir": str(session_dir),
|
||||
"resume_raw_samples_path": LaunchConfiguration(
|
||||
"resume_raw_samples_path"
|
||||
),
|
||||
# The SDK performs roughly 25 synchronous CAN queries whenever
|
||||
# cb_<side>_hand_info has a subscriber. Calibration only used
|
||||
# that topic to display a speed diagnostic, while those reads
|
||||
@@ -246,6 +287,9 @@ def _launch_stack(context):
|
||||
"camera_extrinsics_file"
|
||||
),
|
||||
"source_urdf_path": str(source_urdf),
|
||||
"source_urdf_expected_sha256": LaunchConfiguration(
|
||||
"source_urdf_expected_sha256"
|
||||
),
|
||||
"corrected_urdf_output_dir": LaunchConfiguration(
|
||||
"corrected_urdf_output_dir"
|
||||
),
|
||||
@@ -267,6 +311,13 @@ def _launch_stack(context):
|
||||
LaunchConfiguration("index_flex_calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"adaptive_formal_speed_enabled": ParameterValue(
|
||||
LaunchConfiguration("adaptive_formal_speed_enabled"),
|
||||
value_type=bool,
|
||||
),
|
||||
"cross_view_roll_diagnostic_finger": LaunchConfiguration(
|
||||
"cross_view_roll_diagnostic_finger"
|
||||
),
|
||||
"validation_enabled": ParameterValue(
|
||||
LaunchConfiguration("validation_enabled"), value_type=bool
|
||||
),
|
||||
@@ -300,7 +351,7 @@ def _launch_stack(context):
|
||||
return [
|
||||
LogInfo(
|
||||
msg=(
|
||||
f"G20 {hand_type} three-camera session: {session_dir}; "
|
||||
f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; "
|
||||
f"source_urdf={source_urdf}"
|
||||
)
|
||||
),
|
||||
@@ -342,6 +393,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
DeclareLaunchArgument("hand_type", default_value="left"),
|
||||
DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument(
|
||||
"front_camera_serial", default_value="DB2163742"
|
||||
@@ -387,6 +439,12 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument(
|
||||
"index_flex_calibration_speed", default_value="10"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"adaptive_formal_speed_enabled", default_value="true"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"cross_view_roll_diagnostic_finger", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument("validation_enabled", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_extrinsics_file",
|
||||
@@ -397,6 +455,9 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument(
|
||||
"source_urdf_path", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"source_urdf_expected_sha256", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"corrected_urdf_output_dir", default_value=""
|
||||
),
|
||||
@@ -409,6 +470,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
default_value=str(Path.cwd() / "calibration_output"),
|
||||
),
|
||||
DeclareLaunchArgument("session_dir", default_value=""),
|
||||
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config",
|
||||
default_value=str(
|
||||
@@ -417,9 +479,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "three_camera_tags.yaml"
|
||||
),
|
||||
default_value="",
|
||||
),
|
||||
OpaqueFunction(function=_launch_stack),
|
||||
]
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
<package format="3">
|
||||
<name>g20_thumb_apriltag_calibration</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Three-view Hikrobot AprilTag calibration for the complete left G20 hand.</description>
|
||||
<description>One-command three-camera AprilTag calibration and zero-URDF correction for the G20 right hand.</description>
|
||||
<maintainer email="support@linker-robotics.com">lxp</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
|
||||
@@ -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="One-command three-camera AprilTag calibration for the G20 right hand",
|
||||
license="MIT",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
@@ -64,6 +64,10 @@ setup(
|
||||
"g20_thumb_apriltag_calibration."
|
||||
"calibrated_joint_state_bridge:main"
|
||||
),
|
||||
(
|
||||
"calibrate_g20_right = "
|
||||
"g20_thumb_apriltag_calibration.one_command:main"
|
||||
),
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -6,10 +6,16 @@ from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
|
||||
G20_COMMAND_NAMES,
|
||||
G20_URDF_JOINT_NAMES,
|
||||
CalibratedCommandMapper,
|
||||
default_input_topic,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
JointCurveFit,
|
||||
build_compact_payload,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
get_zero_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
def _payload(side: str = "right") -> dict:
|
||||
@@ -85,3 +91,87 @@ 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_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> None:
|
||||
profile = get_hand_calibration_profile("right", "g20_right_15")
|
||||
zero = get_zero_calibration_profile("right", "g20_right_15")
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127] * 4
|
||||
fits = {}
|
||||
diagnostics = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
zero_command = baseline[spec.motor_index]
|
||||
average = tuple(
|
||||
0.001 * (zero_command - value) for value in range(256)
|
||||
)
|
||||
decreasing = tuple(1.1 * value for value in average)
|
||||
increasing = tuple(0.9 * value for value in average)
|
||||
fits[name] = JointCurveFit(
|
||||
average,
|
||||
decreasing,
|
||||
increasing,
|
||||
{},
|
||||
0.0,
|
||||
0.0,
|
||||
{"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
|
||||
)
|
||||
diagnostics[name] = {
|
||||
"cycle_travel_rad": [0.25] * 4,
|
||||
"cycle_travel_range_rad": 0.0,
|
||||
"baseline_hysteresis_by_cycle_rad": [0.0] * 4,
|
||||
"holdout_cycle": 3,
|
||||
"holdout_cycle_mae_rad": 0.0,
|
||||
"holdout_cycle_max_rad": 0.0,
|
||||
}
|
||||
payload = build_compact_payload(
|
||||
serial_number="TEST_RIGHT_V5",
|
||||
measured_fits=fits,
|
||||
urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
|
||||
validation_errors_rad=[0.0],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
side="right",
|
||||
layout_id="g20_right_15",
|
||||
zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
|
||||
zero_cycle_offsets_rad={
|
||||
name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
|
||||
},
|
||||
zero_observers=zero.offset_observer_joint,
|
||||
artifact_hashes={
|
||||
"source_urdf_sha256": "0" * 64,
|
||||
"camera_extrinsics_sha256": "1" * 64,
|
||||
"corrected_urdf_sha256": "2" * 64,
|
||||
},
|
||||
cross_view_roll_metrics={
|
||||
f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
},
|
||||
joint_dynamic_diagnostics=diagnostics,
|
||||
zero_geometry_diagnostics={
|
||||
"training_cycles": (0, 1, 2),
|
||||
"validation_cycle": 3,
|
||||
"axis_line_rms_m": 0.0,
|
||||
"validation_line_error_by_joint_m": {},
|
||||
},
|
||||
)
|
||||
mapper = CalibratedCommandMapper(payload, expected_side="right")
|
||||
command = [255.0] * 20
|
||||
command[0] = 100.0
|
||||
mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command)))
|
||||
|
||||
assert mapper.input_domain == "command_u8"
|
||||
assert mapper.layout_id == "g20_right_15"
|
||||
# midpoint(1.1, 0.9) is the original 0.001-rad/u8 curve.
|
||||
assert mapped["thumb_cmc_pitch"] == pytest.approx(0.155)
|
||||
assert payload["joints"]["pinky_dip"]["passive"] is True
|
||||
assert "source_joint" not in payload["joints"]["pinky_dip"]
|
||||
|
||||
|
||||
def test_default_topics_use_the_g20_sdk_namespace() -> None:
|
||||
assert default_input_topic("right", "command_u8") == (
|
||||
"/g20/cb_right_hand_control_cmd"
|
||||
)
|
||||
assert default_input_topic("right", "feedback_u8") == (
|
||||
"/g20/cb_right_hand_state"
|
||||
)
|
||||
|
||||
@@ -80,6 +80,36 @@ def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
|
||||
]["ros__parameters"]["tag"]["frames"][0] == "side_base"
|
||||
|
||||
|
||||
def test_right_15_tag_config_matches_the_physical_layout() -> None:
|
||||
tags = yaml.safe_load(
|
||||
(
|
||||
PACKAGE_ROOT
|
||||
/ "config"
|
||||
/ "three_camera_tags_g20_right_15.yaml"
|
||||
).read_text()
|
||||
)
|
||||
expected = {
|
||||
"front": [0, 1, 2, 3, 10, 11, 12, 13],
|
||||
"side": [4, 5, 6, 15, 17],
|
||||
"top": [8, 9],
|
||||
}
|
||||
all_ids: set[int] = set()
|
||||
for view, ids in expected.items():
|
||||
parameters = tags[
|
||||
f"/g20_calibration/{view}/apriltag/apriltag"
|
||||
]["ros__parameters"]
|
||||
assert parameters["tag"]["ids"] == ids
|
||||
assert parameters["tag"]["sizes"] == [0.016] * len(ids)
|
||||
all_ids.update(ids)
|
||||
assert all_ids == set(range(19)) - {7, 14, 16, 18}
|
||||
side_frames = tags[
|
||||
"/g20_calibration/side/apriltag/apriltag"
|
||||
]["ros__parameters"]["tag"]["frames"]
|
||||
assert side_frames == [
|
||||
"side_base", "ring_pip", "pinky_pip", "middle_pip", "index_pip",
|
||||
]
|
||||
|
||||
|
||||
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()
|
||||
@@ -114,14 +144,35 @@ 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["adaptive_formal_speed_enabled"] is True
|
||||
assert parameters["adaptive_formal_speed_max_scale"] == 1.5
|
||||
assert parameters["adaptive_formal_speed_minimum_bins"] == 64
|
||||
assert parameters["adaptive_formal_speed_maximum_bin_gap"] == 8
|
||||
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
|
||||
assert parameters["pnp_group_initialization_frames"] == 8
|
||||
assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0
|
||||
assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0
|
||||
assert parameters["thumb_ip_pnp_coupling_multiplier"] == 1.03
|
||||
assert parameters["thumb_ip_pnp_coupling_scale_deg"] == 3.0
|
||||
assert parameters["thumb_ip_pnp_maximum_coupling_residual_deg"] == 7.5
|
||||
assert parameters["baseline_hold_seconds"] == 0.5
|
||||
assert parameters["minimum_baseline_hold_frames"] == 10
|
||||
assert parameters["directional_zero_maximum_branch_gap_deg"] == 2.0
|
||||
assert parameters["directional_zero_maximum_branch_gap_range_deg"] == 0.3
|
||||
assert (
|
||||
parameters["cross_view_roll_maximum_branch_gap_difference_deg"]
|
||||
== 0.3
|
||||
)
|
||||
assert parameters["cross_view_roll_diagnostic_finger"] == ""
|
||||
assert parameters["repetitions"] == 3
|
||||
assert parameters["g20_right_19_repetitions"] >= 4
|
||||
assert parameters["validation_enabled"] is False
|
||||
assert parameters["combination_validation_enabled"] is False
|
||||
assert parameters["combination_validation_frames"] >= 10
|
||||
assert parameters["combination_maximum_position_p95_m"] <= 0.003
|
||||
assert parameters["combination_maximum_orientation_p95_deg"] <= 2.0
|
||||
assert parameters["minimum_detection_rate"] == 0.95
|
||||
assert parameters["minimum_detection_hz"] == 15.0
|
||||
assert parameters["minimum_state_span_u8"] >= 240.0
|
||||
@@ -130,14 +181,15 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
|
||||
assert parameters["minimum_sweep_bins"] >= 32
|
||||
assert parameters["maximum_bin_gap"] <= 16
|
||||
assert parameters["automatic_sweep_retry_limit"] == 3
|
||||
assert parameters["automatic_sweep_retry_limit"] == 2
|
||||
assert parameters["automatic_fit_retry_limit"] == 2
|
||||
assert parameters["motor_stall_timeout_seconds"] >= 5.0
|
||||
assert parameters["motor_stall_timeout_seconds"] == 2.0
|
||||
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
|
||||
assert parameters["motor_stall_minimum_progress_u8"] == 1.0
|
||||
assert parameters["automatic_motion_retry_limit"] == 2
|
||||
assert parameters["provisional_warning_ratio"] == 1.25
|
||||
assert parameters["retry_speed_scales"] == [0.8, 0.6, 0.5]
|
||||
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25]
|
||||
assert parameters["retry_speed_scales"] == [0.8, 0.6]
|
||||
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0]
|
||||
assert parameters["position_timeout_seconds"] >= 20.0
|
||||
assert parameters["maximum_state_image_skew_ms"] <= 50.0
|
||||
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
|
||||
@@ -147,6 +199,7 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5
|
||||
assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75
|
||||
assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0
|
||||
assert parameters["zero_maximum_observability_condition_number"] >= 1.0
|
||||
assert parameters["zero_maximum_offset_deg"] <= 20.0
|
||||
assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
|
||||
assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
|
||||
@@ -155,7 +208,13 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0
|
||||
assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0
|
||||
assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0
|
||||
assert parameters["passive_maximum_hysteresis_deg"] <= 7.5
|
||||
assert parameters["maximum_hysteresis_deg"] <= 2.0
|
||||
assert parameters["passive_maximum_hysteresis_deg"] <= 2.0
|
||||
assert parameters["command_maximum_direction_gap_deg"] <= 2.0
|
||||
assert parameters["maximum_validation_mae_deg"] <= 1.0
|
||||
assert parameters["maximum_validation_p95_deg"] <= 2.0
|
||||
assert parameters["maximum_validation_error_deg"] <= 3.0
|
||||
assert parameters["zero_maximum_confidence_half_width_deg"] <= 1.5
|
||||
for view in ("front", "side", "top"):
|
||||
assert parameters[f"{view}_camera_info_topic"].startswith(
|
||||
f"/g20_calibration/{view}/"
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
import math
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
@@ -8,18 +9,26 @@ from g20_thumb_apriltag_calibration.full_hand import (
|
||||
ACTIVE_JOINTS,
|
||||
IMAGE_TRAJECTORY_JOINTS,
|
||||
JOINT_SPECS,
|
||||
JointCurveFit,
|
||||
LEFT_HAND_PROFILE,
|
||||
MEASURED_JOINTS,
|
||||
PASSIVE_JOINTS,
|
||||
RIGHT_HAND_PROFILE,
|
||||
RIGHT_19_HAND_PROFILE,
|
||||
SPLAY_JOINTS,
|
||||
SWEEP_SPECS,
|
||||
THREE_CAMERA_BASELINE_COMMAND,
|
||||
VIEW_TAGS,
|
||||
build_calibration_motion_command,
|
||||
build_calibration_preparation_waypoints,
|
||||
build_calibration_return_waypoints,
|
||||
build_calibration_speed_profile,
|
||||
build_compact_payload,
|
||||
build_full_hand_command,
|
||||
clamp_runtime_fits_to_urdf_limits,
|
||||
center_splay_curve,
|
||||
compare_cross_view_roll_curves,
|
||||
derive_mimic_passive_fits,
|
||||
fit_joint_center_curve,
|
||||
fit_joint_image_curve,
|
||||
fit_measured_joint_curve,
|
||||
@@ -30,6 +39,375 @@ from g20_thumb_apriltag_calibration.full_hand import (
|
||||
)
|
||||
|
||||
|
||||
def test_runtime_fit_saturates_at_source_urdf_limit() -> None:
|
||||
values = tuple(np.linspace(0.0, 2.0, 256))
|
||||
fit = JointCurveFit(
|
||||
angle_rad=values,
|
||||
decreasing_rad=values,
|
||||
increasing_rad=values,
|
||||
circle={},
|
||||
maximum_monotonic_correction_rad=0.0,
|
||||
maximum_hysteresis_rad=0.0,
|
||||
quality={},
|
||||
)
|
||||
|
||||
bounded = clamp_runtime_fits_to_urdf_limits(
|
||||
Path(__file__).resolve().parents[3]
|
||||
/ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/"
|
||||
"hands/linker_hand/g20_right/linkerhand_g20_right.urdf",
|
||||
{"thumb_cmc_yaw": fit},
|
||||
)["thumb_cmc_yaw"]
|
||||
|
||||
assert max(bounded.angle_rad) == pytest.approx(1.57)
|
||||
assert max(bounded.decreasing_rad) == pytest.approx(1.57)
|
||||
assert bounded.circle["runtime_urdf_limit_clipped_bins"]["angle_rad"] > 0
|
||||
|
||||
|
||||
def test_right_15_profile_has_exact_layout_tasks_and_derived_dips() -> None:
|
||||
profile = get_hand_calibration_profile("right", "g20_right_15")
|
||||
assert profile is RIGHT_19_HAND_PROFILE
|
||||
assert len(profile.sweep_specs) == 16
|
||||
assert len(profile.measured_joints) == 17
|
||||
assert len(profile.record_joints) == 21
|
||||
assert list(profile.view_tags["front"].values()) == [
|
||||
0, 1, 2, 3, 10, 11, 12, 13
|
||||
]
|
||||
assert list(profile.view_tags["side"].values()) == [
|
||||
4, 5, 6, 15, 17
|
||||
]
|
||||
assert list(profile.view_tags["top"].values()) == [8, 9]
|
||||
assert profile.preflight_view_roles == {
|
||||
"front": ("front_base",),
|
||||
"side": ("side_base",),
|
||||
"top": ("top_base",),
|
||||
}
|
||||
assert [spec.task_name for spec in profile.sweep_specs[:4]] == [
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
"thumb_mcp_ip_front",
|
||||
"thumb_cmc_yaw_top",
|
||||
]
|
||||
assert [
|
||||
spec.task_name for spec in profile.sweep_specs[4:7]
|
||||
] == [
|
||||
"pinky_roll_multiview",
|
||||
"pinky_pitch_side",
|
||||
"pinky_pip_side",
|
||||
]
|
||||
assert profile.sweep_specs[4].joints == (
|
||||
"pinky_mcp_roll",
|
||||
"pinky_mcp_roll_side",
|
||||
)
|
||||
assert {
|
||||
name: spec.source_joint
|
||||
for name, spec in profile.joint_specs.items()
|
||||
if spec.source_joint is not None
|
||||
} == {
|
||||
"index_dip": "index_pip",
|
||||
"middle_dip": "middle_pip",
|
||||
"ring_dip": "ring_pip",
|
||||
"pinky_dip": "pinky_pip",
|
||||
}
|
||||
|
||||
|
||||
def test_right_15_derives_four_dip_curves_from_source_urdf_mimic() -> None:
|
||||
profile = RIGHT_19_HAND_PROFILE
|
||||
values = tuple(0.002 * (255 - command) for command in range(256))
|
||||
fit = JointCurveFit(values, values, values, {}, 0.0, 0.01, {})
|
||||
measured = {name: fit for name in profile.measured_joints}
|
||||
source_urdf = (
|
||||
Path(__file__).resolve().parents[3]
|
||||
/ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
|
||||
"linker_hand/g20_right/linkerhand_g20_right.urdf"
|
||||
)
|
||||
|
||||
runtime = derive_mimic_passive_fits(
|
||||
source_urdf, measured, profile=profile
|
||||
)
|
||||
|
||||
assert set(runtime) == set(profile.joint_specs)
|
||||
for finger in ("index", "middle", "ring", "pinky"):
|
||||
pip = runtime[f"{finger}_pip"]
|
||||
dip = runtime[f"{finger}_dip"]
|
||||
assert dip.angle_rad == pytest.approx(
|
||||
tuple(0.89 * value for value in pip.angle_rad)
|
||||
)
|
||||
assert dip.maximum_hysteresis_rad == pytest.approx(0.0089)
|
||||
assert dip.circle["curve_source"] == f"{finger}_pip"
|
||||
assert dip.circle["visual_measurement"] is False
|
||||
|
||||
|
||||
def _changed_motors(
|
||||
previous: tuple[int, ...] | list[int], waypoint: tuple[int, ...]
|
||||
) -> list[int]:
|
||||
return [
|
||||
index
|
||||
for index, (left, right) in enumerate(zip(previous, waypoint))
|
||||
if left != right
|
||||
]
|
||||
|
||||
|
||||
def _phase_changes(
|
||||
previous: tuple[int, ...] | list[int],
|
||||
waypoints: tuple[tuple[int, ...], ...],
|
||||
) -> list[list[int]]:
|
||||
phases: list[list[int]] = []
|
||||
for waypoint in waypoints:
|
||||
phases.append(_changed_motors(previous, waypoint))
|
||||
previous = waypoint
|
||||
return phases
|
||||
|
||||
|
||||
def test_right_19_side_clearance_waypoints_group_joint_classes() -> None:
|
||||
profile = RIGHT_19_HAND_PROFILE
|
||||
spec = next(
|
||||
item for item in profile.sweep_specs
|
||||
if item.task_name == "index_roll_multiview"
|
||||
)
|
||||
baseline = [255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
waypoints = build_calibration_preparation_waypoints(
|
||||
spec,
|
||||
255,
|
||||
current_command=baseline,
|
||||
profile=profile,
|
||||
)
|
||||
# Side clearance keeps all neighbouring roll axes neutral, flexes the
|
||||
# neighbour PIPs together, then their pitches, and moves the index target
|
||||
# last through the cleared swept volume.
|
||||
assert _phase_changes(baseline, waypoints) == [
|
||||
[17, 18, 19], [2, 3, 4], [6],
|
||||
]
|
||||
assert waypoints[-1][1] == 255 and waypoints[-1][16] == 255
|
||||
|
||||
returns = build_calibration_return_waypoints(
|
||||
baseline,
|
||||
current_command=waypoints[-1],
|
||||
profile=profile,
|
||||
)
|
||||
assert _phase_changes(waypoints[-1], returns) == [
|
||||
[6], [2, 3, 4], [17, 18, 19],
|
||||
]
|
||||
assert list(returns[-1]) == baseline
|
||||
|
||||
|
||||
def test_right_19_side_clearance_waypoints_move_one_motor_at_a_time() -> None:
|
||||
profile = RIGHT_19_HAND_PROFILE
|
||||
spec = next(
|
||||
item for item in profile.sweep_specs
|
||||
if item.task_name == "index_roll_multiview"
|
||||
)
|
||||
waypoints = build_calibration_preparation_waypoints(
|
||||
spec,
|
||||
255,
|
||||
current_command=[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
profile=profile,
|
||||
parallel=False,
|
||||
)
|
||||
previous = tuple([255] * 6 + [127] * 4 + [255] * 10)
|
||||
changed_motors = []
|
||||
for waypoint in waypoints:
|
||||
changed = _changed_motors(previous, waypoint)
|
||||
assert len(changed) == 1
|
||||
changed_motors.append(changed[0])
|
||||
previous = waypoint
|
||||
# Side clearance keeps all neighbouring roll axes neutral, flexes them out
|
||||
# of the swept volume, and moves the index target last.
|
||||
assert changed_motors[:7] == [19, 4, 18, 3, 17, 2, 6]
|
||||
assert previous[1] == 255 and previous[16] == 255
|
||||
|
||||
returns = build_calibration_return_waypoints(
|
||||
[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
current_command=previous,
|
||||
profile=profile,
|
||||
parallel=False,
|
||||
)
|
||||
for waypoint in returns:
|
||||
changed = sum(left != right for left, right in zip(previous, waypoint))
|
||||
assert changed == 1
|
||||
previous = waypoint
|
||||
|
||||
|
||||
def test_right_15_ring_multiview_keeps_pinky_roll_neutral_and_flexes_it() -> None:
|
||||
profile = RIGHT_19_HAND_PROFILE
|
||||
spec = next(
|
||||
item for item in profile.sweep_specs
|
||||
if item.task_name == "ring_roll_multiview"
|
||||
)
|
||||
current = tuple(THREE_CAMERA_BASELINE_COMMAND)
|
||||
|
||||
waypoints = build_calibration_preparation_waypoints(
|
||||
spec,
|
||||
255,
|
||||
current_command=current,
|
||||
profile=profile,
|
||||
)
|
||||
|
||||
assert _phase_changes(current, waypoints) == [[6, 7], [19], [4], [8]]
|
||||
assert all(waypoint[9] == 127 for waypoint in waypoints)
|
||||
assert waypoints[-1][8] == 255
|
||||
assert waypoints[-1][19] == 0
|
||||
assert waypoints[-1][4] == 0
|
||||
|
||||
|
||||
def test_middle_multiview_recovers_neighbour_from_observed_58_not_zero() -> None:
|
||||
profile = RIGHT_19_HAND_PROFILE
|
||||
spec = next(
|
||||
item for item in profile.sweep_specs
|
||||
if item.task_name == "middle_roll_multiview"
|
||||
)
|
||||
# Session 20260819_160238 stopped here: the legacy front-only avoidance
|
||||
# asked neighbouring ring roll (motor 8) for 0 and it saturated at 58.
|
||||
current = [254] * 20
|
||||
current[6:10] = [254, 127, 58, 127]
|
||||
current[10] = 252
|
||||
current[11:15] = [0, 0, 0, 0]
|
||||
|
||||
waypoints = build_calibration_preparation_waypoints(
|
||||
spec,
|
||||
255,
|
||||
current_command=current,
|
||||
profile=profile,
|
||||
)
|
||||
|
||||
# Both differing roll axes (parked motor 6 and the stuck motor 8) move in
|
||||
# the single roll phase, still never commanded toward 0.
|
||||
phases = _phase_changes(current, waypoints)
|
||||
assert phases[0] == [6, 8]
|
||||
assert waypoints[0][8] == 127
|
||||
assert all(
|
||||
waypoint[motor] != 0
|
||||
for waypoint in waypoints
|
||||
for motor in range(6, 10)
|
||||
)
|
||||
assert waypoints[-1][6:10] == (255, 255, 127, 127)
|
||||
|
||||
|
||||
def test_right_19_ring_pitch_keeps_target_neutral_during_clearance() -> None:
|
||||
profile = RIGHT_19_HAND_PROFILE
|
||||
spec = next(
|
||||
item for item in profile.sweep_specs
|
||||
if item.task_name == "ring_pitch_side"
|
||||
)
|
||||
current = tuple(THREE_CAMERA_BASELINE_COMMAND)
|
||||
|
||||
waypoints = build_calibration_preparation_waypoints(
|
||||
spec,
|
||||
255,
|
||||
current_command=current,
|
||||
profile=profile,
|
||||
)
|
||||
|
||||
assert _phase_changes(current, waypoints) == [[6, 7], [19], [4]]
|
||||
assert all(waypoint[8] == 127 for waypoint in waypoints)
|
||||
assert all(waypoint[9] == 127 for waypoint in waypoints)
|
||||
assert waypoints[-1][9] == 127
|
||||
assert waypoints[-1][19] == 0
|
||||
assert waypoints[-1][4] == 0
|
||||
|
||||
|
||||
def test_right_19_waypoints_never_wait_for_reserved_feedback_channels() -> None:
|
||||
profile = RIGHT_19_HAND_PROFILE
|
||||
spec = profile.sweep_specs[0]
|
||||
# This is the real G20 feedback shape: reserved channels 11..14 report zero
|
||||
# while active endpoint channels commonly settle one or two counts low.
|
||||
current = [
|
||||
255, 254, 254, 254, 254, 254, 127, 127, 127, 127,
|
||||
253, 0, 0, 0, 0, 255, 253, 253, 253, 253,
|
||||
]
|
||||
waypoints = build_calibration_preparation_waypoints(
|
||||
spec,
|
||||
255,
|
||||
current_command=current,
|
||||
profile=profile,
|
||||
)
|
||||
assert waypoints
|
||||
assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in waypoints)
|
||||
controlled = {joint.motor_index for joint in profile.joint_specs.values()}
|
||||
for previous, waypoint in zip((tuple(current), *waypoints[:-1]), waypoints):
|
||||
assert set(_changed_motors(previous, waypoint)) <= controlled
|
||||
|
||||
returns = build_calibration_return_waypoints(
|
||||
list(THREE_CAMERA_BASELINE_COMMAND),
|
||||
current_command=waypoints[-1],
|
||||
profile=profile,
|
||||
)
|
||||
assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in returns)
|
||||
|
||||
|
||||
def test_right_19_parallel_return_moves_rolls_together_then_fingers() -> None:
|
||||
# Full four-finger avoidance pose: rolls parked at 255 and every clearance
|
||||
# finger flexed to 0.
|
||||
current = list(THREE_CAMERA_BASELINE_COMMAND)
|
||||
current[6:10] = [255, 255, 255, 255]
|
||||
for motor in (1, 2, 3, 4, 16, 17, 18, 19):
|
||||
current[motor] = 0
|
||||
returns = build_calibration_return_waypoints(
|
||||
list(THREE_CAMERA_BASELINE_COMMAND),
|
||||
current_command=current,
|
||||
profile=RIGHT_19_HAND_PROFILE,
|
||||
anchor_roll_motor=9,
|
||||
)
|
||||
assert _phase_changes(current, returns) == [
|
||||
[6, 7, 8, 9], [1, 2, 3, 4], [16, 17, 18, 19],
|
||||
]
|
||||
assert list(returns[-1]) == list(THREE_CAMERA_BASELINE_COMMAND)
|
||||
|
||||
|
||||
def test_right_19_pinky_exit_returns_target_roll_before_other_fingers() -> None:
|
||||
current = list(THREE_CAMERA_BASELINE_COMMAND)
|
||||
current[6:10] = [255, 255, 255, 255]
|
||||
returns = build_calibration_return_waypoints(
|
||||
list(THREE_CAMERA_BASELINE_COMMAND),
|
||||
current_command=current,
|
||||
profile=RIGHT_19_HAND_PROFILE,
|
||||
anchor_roll_motor=9,
|
||||
parallel=False,
|
||||
)
|
||||
phases = _phase_changes(current, returns)
|
||||
assert all(len(phase) == 1 for phase in phases)
|
||||
assert [motor for phase in phases for motor in phase][:4] == [9, 8, 7, 6]
|
||||
|
||||
|
||||
def test_right_19_ring_exit_returns_ring_before_neighbours() -> None:
|
||||
current = list(THREE_CAMERA_BASELINE_COMMAND)
|
||||
current[6:10] = [255, 255, 255, 0]
|
||||
returns = build_calibration_return_waypoints(
|
||||
list(THREE_CAMERA_BASELINE_COMMAND),
|
||||
current_command=current,
|
||||
profile=RIGHT_19_HAND_PROFILE,
|
||||
anchor_roll_motor=8,
|
||||
parallel=False,
|
||||
)
|
||||
|
||||
first = returns[0]
|
||||
assert first[8] == 127
|
||||
assert first[6:8] == (255, 255)
|
||||
assert first[9] == 0
|
||||
|
||||
|
||||
def test_cross_view_roll_curve_must_agree_before_fusion() -> None:
|
||||
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
|
||||
fit = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
|
||||
assert compare_cross_view_roll_curves(fit, fit)[
|
||||
"angle_rad_rms_difference_rad"
|
||||
] == 0.0
|
||||
bad_values = tuple(value + 0.03 * (command != 127) for command, value in enumerate(values))
|
||||
bad = replace(fit, angle_rad=bad_values, decreasing_rad=bad_values,
|
||||
increasing_rad=bad_values)
|
||||
with pytest.raises(ValueError, match="cross_view_roll_curve"):
|
||||
compare_cross_view_roll_curves(fit, bad)
|
||||
|
||||
reverse = replace(
|
||||
fit,
|
||||
increasing_rad=tuple(value + math.radians(0.5) for value in values),
|
||||
)
|
||||
with pytest.raises(ValueError, match="branch_gap_difference"):
|
||||
compare_cross_view_roll_curves(fit, reverse)
|
||||
|
||||
|
||||
def _records() -> list[dict[str, object]]:
|
||||
commands = list(range(0, 256, 16))
|
||||
if commands[-1] != 255:
|
||||
@@ -498,3 +876,88 @@ def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
|
||||
assert joint["angle_rad"] == source["angle_rad"]
|
||||
assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0}
|
||||
assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01}
|
||||
|
||||
|
||||
def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() -> None:
|
||||
from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
get_zero_calibration_profile,
|
||||
)
|
||||
|
||||
profile = RIGHT_19_HAND_PROFILE
|
||||
zero = get_zero_calibration_profile("right", "g20_right_15")
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127] * 4
|
||||
fits = {}
|
||||
diagnostics = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
zero_command = baseline[spec.motor_index]
|
||||
values = tuple(
|
||||
0.002 * (zero_command - command) for command in range(256)
|
||||
)
|
||||
fits[name] = JointCurveFit(
|
||||
values,
|
||||
values,
|
||||
values,
|
||||
{},
|
||||
0.0,
|
||||
0.0,
|
||||
{"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
|
||||
)
|
||||
diagnostics[name] = {
|
||||
"cycle_travel_rad": [0.5, 0.5, 0.5, 0.5],
|
||||
"cycle_travel_range_rad": 0.0,
|
||||
"baseline_hysteresis_by_cycle_rad": [0.0, 0.0, 0.0, 0.0],
|
||||
"holdout_cycle": 3,
|
||||
"holdout_cycle_mae_rad": 0.0,
|
||||
"holdout_cycle_max_rad": 0.0,
|
||||
}
|
||||
offsets = {name: 0.0 for name in profile.active_joints}
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_RIGHT_019",
|
||||
measured_fits=fits,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[0.0],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
side="right",
|
||||
layout_id="g20_right_15",
|
||||
zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
|
||||
zero_cycle_offsets_rad={
|
||||
name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
|
||||
},
|
||||
zero_observers=zero.offset_observer_joint,
|
||||
artifact_hashes={
|
||||
"source_urdf_sha256": "0" * 64,
|
||||
"camera_extrinsics_sha256": "1" * 64,
|
||||
"corrected_urdf_sha256": "2" * 64,
|
||||
},
|
||||
cross_view_roll_metrics={
|
||||
f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
},
|
||||
joint_dynamic_diagnostics=diagnostics,
|
||||
zero_geometry_diagnostics={
|
||||
"training_cycles": (0, 1, 2),
|
||||
"validation_cycle": 3,
|
||||
"axis_line_rms_m": 0.0,
|
||||
"validation_line_error_by_joint_m": {},
|
||||
},
|
||||
)
|
||||
|
||||
validate_compact_payload(payload)
|
||||
assert payload["schema_version"] == 4
|
||||
assert set(payload) == {
|
||||
"schema_version", "model", "side", "serial_number", "angle_unit",
|
||||
"command_range", "baseline_command_u8", "joints", "quality",
|
||||
}
|
||||
assert len(zero.direct_zero_joints) == 12
|
||||
assert payload["joints"]["thumb_mcp"]["zero_angles"] == {
|
||||
"urdf_zero_offset_rad": 0.0
|
||||
}
|
||||
assert payload["joints"]["thumb_ip"]["passive"] is True
|
||||
assert set(payload["joints"]["thumb_ip"]) == {
|
||||
"motor_index", "angle_rad", "passive"
|
||||
}
|
||||
assert all(
|
||||
"source_joint" not in joint for joint in payload["joints"].values()
|
||||
)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -234,6 +234,70 @@ def _pose(
|
||||
)
|
||||
|
||||
|
||||
def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("base", "mcp", "ip"),
|
||||
adjacent_pairs=(("base", "mcp"), ("mcp", "ip")),
|
||||
maximum_pose_jump_rad=np.deg2rad(360.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(1000.0),
|
||||
relative_translation_scale_m=1.0,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=1.0,
|
||||
reset_after_seconds=5.0,
|
||||
coupled_rotation_pairs=(
|
||||
("base", "mcp", "mcp", "ip", 1.03),
|
||||
),
|
||||
coupled_rotation_scale_rad=np.deg2rad(3.0),
|
||||
maximum_coupled_rotation_residual_rad=np.deg2rad(7.5),
|
||||
)
|
||||
baseline = {
|
||||
"base": (_pose(0.0, 0.00, 0.05),),
|
||||
"mcp": (_pose(0.0, 0.03, 0.05),),
|
||||
"ip": (_pose(0.0, 0.06, 0.05),),
|
||||
}
|
||||
selected, reason = tracker.select(
|
||||
baseline,
|
||||
stamp_ns=1_000_000_000,
|
||||
trajectory_command_u8=255,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert selected is not None
|
||||
assert reason == ""
|
||||
|
||||
driver = _pose(10.0, 0.03, 0.05)
|
||||
measured_ip = _pose(20.3, 0.06, 0.20)
|
||||
lower_reprojection_mirror = _pose(30.0, 0.06, 0.05)
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"base": (_pose(0.0, 0.00, 0.05),),
|
||||
"mcp": (driver,),
|
||||
"ip": (lower_reprojection_mirror, measured_ip),
|
||||
},
|
||||
stamp_ns=1_033_000_000,
|
||||
trajectory_command_u8=128,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected is not None
|
||||
assert selected["ip"] == measured_ip
|
||||
|
||||
fallback, fallback_reason = tracker.select(
|
||||
{
|
||||
"base": (_pose(0.0, 0.00, 0.05),),
|
||||
"mcp": (driver,),
|
||||
"ip": (lower_reprojection_mirror,),
|
||||
},
|
||||
stamp_ns=1_066_000_000,
|
||||
trajectory_command_u8=128,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert fallback is not None
|
||||
assert fallback_reason == ""
|
||||
assert fallback["ip"] == lower_reprojection_mirror
|
||||
|
||||
|
||||
def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("t0", "t3", "t4", "t5"),
|
||||
@@ -324,6 +388,58 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
|
||||
assert selected == {"t4": return_t4, "t5": return_t5}
|
||||
|
||||
|
||||
def test_group_tracker_uses_outbound_pose_at_same_command_on_return() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("parent", "child"),
|
||||
adjacent_pairs=(("parent", "child"),),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
)
|
||||
parent = _pose(0.0, 0.00, 0.05)
|
||||
for stamp, command, angle in (
|
||||
(1_000_000_000, 255, 20.0),
|
||||
(1_033_000_000, 64, 45.0),
|
||||
(1_066_000_000, 0, 50.0),
|
||||
):
|
||||
selected, reason = tracker.select(
|
||||
{"parent": (parent,), "child": (_pose(angle, 0.03, 0.05),)},
|
||||
stamp_ns=stamp,
|
||||
trajectory_command_u8=command,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert reason == ""
|
||||
assert selected is not None
|
||||
|
||||
# The true return contains 3 deg of real hysteresis along the learned
|
||||
# y-axis. The lower-error mirror candidate is temporally smoother but
|
||||
# adds a 2 deg tilt outside that physical motion axis.
|
||||
true_return = _pose(42.0, 0.03, 0.20)
|
||||
smoother_mirror = SquareTagPose(
|
||||
quaternion_xyzw=tuple(
|
||||
Rotation.from_euler("xy", [2.0, 49.0], degrees=True).as_quat()
|
||||
),
|
||||
translation_xyz_m=(0.03, 0.0, 0.25),
|
||||
reprojection_error_px=0.01,
|
||||
)
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (smoother_mirror, true_return),
|
||||
},
|
||||
stamp_ns=1_099_000_000,
|
||||
trajectory_command_u8=64,
|
||||
trajectory_direction="increasing",
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": true_return}
|
||||
|
||||
|
||||
def test_group_tracker_initializes_from_multiple_static_frames() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("parent", "child"),
|
||||
@@ -372,6 +488,63 @@ def test_group_tracker_initializes_from_multiple_static_frames() -> None:
|
||||
] < np.deg2rad(1.0)
|
||||
|
||||
|
||||
def test_group_tracker_preserves_task_branch_anchor_across_cycles() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("parent", "child"),
|
||||
adjacent_pairs=(("parent", "child"),),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
initialization_frames=8,
|
||||
)
|
||||
parent = _pose(0.0, 0.00, 0.05)
|
||||
anchored_child = _pose(20.0, 0.03, 0.20)
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{"parent": (parent,), "child": (anchored_child,)},
|
||||
stamp_ns=1_000_000_000 + index * 33_000_000,
|
||||
trajectory_command_u8=255,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": anchored_child}
|
||||
|
||||
tracker.reset(preserve_task_reference=True)
|
||||
lower_error_mirror = _pose(5.0, 0.03, 0.01)
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (lower_error_mirror, anchored_child),
|
||||
},
|
||||
stamp_ns=2_000_000_000 + index * 33_000_000,
|
||||
trajectory_command_u8=255,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": anchored_child}
|
||||
assert tracker.last_initialization_quality["task_reference_used"] == "true"
|
||||
|
||||
tracker.reset()
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (lower_error_mirror, anchored_child),
|
||||
},
|
||||
stamp_ns=3_000_000_000 + index * 33_000_000,
|
||||
trajectory_command_u8=255,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": lower_error_mirror}
|
||||
|
||||
|
||||
def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("mcp", "pip", "dip"),
|
||||
|
||||
@@ -74,6 +74,36 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
|
||||
assert "/g20_calibration/resume" in text
|
||||
|
||||
|
||||
def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED", "combination_pose_prediction_failed", {}
|
||||
)
|
||||
|
||||
assert "多关节组合姿态" in explanation
|
||||
assert "未分类原因码" not in explanation
|
||||
assert "combination_validation_failure" in action
|
||||
assert "不要重新采集16个单关节任务" in action
|
||||
|
||||
|
||||
def test_multiview_failure_names_the_camera_specific_joint() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"sweep_bin_gap_too_large:pinky_mcp_roll_side",
|
||||
{
|
||||
"sample": {
|
||||
"maximum_bin_gap": 20,
|
||||
"maximum_bin_gap_start_u8": 100,
|
||||
"maximum_bin_gap_end_u8": 120,
|
||||
"allowed_maximum_bin_gap": 16,
|
||||
}
|
||||
},
|
||||
)
|
||||
|
||||
assert "小指MCP侧摆(侧面校验)" in explanation
|
||||
assert "100→120" in explanation
|
||||
assert "resume" in action
|
||||
|
||||
|
||||
def test_preflight_lists_missing_tags_in_chinese() -> None:
|
||||
payload = {
|
||||
"state": "PREFLIGHT",
|
||||
@@ -137,6 +167,82 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
|
||||
assert "运动采样:" not in text
|
||||
|
||||
|
||||
def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "joint_fit_check_failed",
|
||||
"progress": 0.15,
|
||||
"completed_sweeps": 24,
|
||||
"total_sweeps": 160,
|
||||
"active": {
|
||||
"kind": "fit_failure",
|
||||
"view": "front",
|
||||
"motor_index": 15,
|
||||
"joints": ["thumb_ip"],
|
||||
"attempt": 3,
|
||||
"directions_to_rescan": 6,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "thumb_ip",
|
||||
"metric": "baseline_hysteresis_deg",
|
||||
"actual": 1.34,
|
||||
"limit": 0.5,
|
||||
"comparison": "maximum",
|
||||
"cycle_values_deg": [1.34, 0.04, 0.14],
|
||||
}
|
||||
],
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "baseline正反程关节角差为1.34°" in text
|
||||
assert "要求不超过0.50°" in text
|
||||
assert "各轮=1.34°/0.04°/0.14°" in text
|
||||
assert "未知原因" not in text
|
||||
|
||||
|
||||
def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "sweep_bin_gap_too_large",
|
||||
"progress": 0.1,
|
||||
"completed_sweeps": 1,
|
||||
"total_sweeps": 10,
|
||||
"active": {
|
||||
"kind": "sweep",
|
||||
"view": "top",
|
||||
"motor_index": 10,
|
||||
"joints": ["thumb_cmc_yaw"],
|
||||
"cycle": 1,
|
||||
"repetitions": 3,
|
||||
"start_u8": 255,
|
||||
"target_u8": 0,
|
||||
"actual_u8": 3.0,
|
||||
"valid_frames": 610,
|
||||
"detection_frames": 644,
|
||||
"detection_valid_frames": 610,
|
||||
"detection_rate": 610 / 644,
|
||||
"sample": {
|
||||
"minimum_u8": 3.0,
|
||||
"maximum_u8": 252.0,
|
||||
"maximum_bin_gap": 17,
|
||||
"maximum_bin_gap_start_u8": 99,
|
||||
"maximum_bin_gap_end_u8": 116,
|
||||
"allowed_maximum_bin_gap": 16,
|
||||
},
|
||||
},
|
||||
"views": {},
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "最大空缺为17,(99→116)" in text
|
||||
assert "本方向Tag检出:94.7%(610/644帧)" in text
|
||||
|
||||
|
||||
def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
@@ -210,6 +316,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
|
||||
"reason": (
|
||||
"motor_state_stalled:return_baseline:motor_index=10:"
|
||||
"target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:"
|
||||
"timeout_seconds=2.000:"
|
||||
"error_u8=5.000"
|
||||
),
|
||||
"progress": 0.0,
|
||||
@@ -224,6 +331,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
|
||||
"actual_u8": 250.0,
|
||||
"error_u8": 5.0,
|
||||
"tolerance_u8": 4.0,
|
||||
"timeout_seconds": 2.0,
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
@@ -231,7 +339,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "电机10反馈连续8秒" in text
|
||||
assert "电机10反馈连续2秒" in text
|
||||
assert "目标255.0、实际250.0、误差5.000 u8" in text
|
||||
assert "允许容差±4.0 u8" in text
|
||||
assert "当前任务:电机10运动停滞,目标255.0、实际250.0" in text
|
||||
@@ -371,3 +479,15 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None:
|
||||
assert "阶段速度:五指目标[15, 5, 15, 15, 15]" in text
|
||||
assert "SDK报告[15, 5, 15, 15, 15]" in text
|
||||
assert "自动重试:当前方向已自动重扫1/2次" in text
|
||||
|
||||
|
||||
def test_same_finger_transition_explains_that_clearance_stays_parked() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"RETURN_BASELINE",
|
||||
"holding_same_finger_clearance_before_next_task",
|
||||
{},
|
||||
)
|
||||
|
||||
assert "继续保持当前避让姿态" in explanation
|
||||
assert "只调整被测关节" in explanation
|
||||
assert "不要手动展开" in action
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -19,6 +19,7 @@ from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
UrdfKinematicModel,
|
||||
_angles_from_state,
|
||||
_zero_sensitive_axis_error_rad,
|
||||
baseline_hysteresis_by_cycle_rad,
|
||||
fit_joint_axis_measurement,
|
||||
fit_rotation_joint_curve,
|
||||
solve_urdf_zero_offsets,
|
||||
@@ -157,6 +158,78 @@ def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
|
||||
assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
|
||||
|
||||
|
||||
def test_baseline_hysteresis_uses_only_revolute_axis_component() -> None:
|
||||
off_axis_noise = Rotation.from_rotvec(
|
||||
np.radians([1.2, 0.0, 0.2])
|
||||
).as_quat().tolist()
|
||||
records = []
|
||||
for cycle in range(3):
|
||||
records.extend(
|
||||
(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": "decreasing",
|
||||
"command_u8": 255,
|
||||
"relative_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
|
||||
},
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": "increasing",
|
||||
"command_u8": 255,
|
||||
"relative_quaternion_xyzw": off_axis_noise,
|
||||
},
|
||||
)
|
||||
)
|
||||
|
||||
full_pose = baseline_hysteresis_by_cycle_rad(
|
||||
records, zero_command_u8=255
|
||||
)
|
||||
joint_angle = baseline_hysteresis_by_cycle_rad(
|
||||
records, zero_command_u8=255, axis_xyz=[0.0, 0.0, 1.0]
|
||||
)
|
||||
|
||||
assert math.degrees(full_pose[0]) == pytest.approx(
|
||||
math.hypot(1.2, 0.2), abs=1.0e-9
|
||||
)
|
||||
assert math.degrees(joint_angle[0]) == pytest.approx(0.2, abs=1.0e-9)
|
||||
|
||||
|
||||
def test_canonical_zero_preserves_opposite_direction_baseline_offset() -> None:
|
||||
records = []
|
||||
commands = sorted({0, 127, 255, *range(0, 256, 16)})
|
||||
branch_offset = math.radians(1.0)
|
||||
for cycle in range(3):
|
||||
for direction in ("decreasing", "increasing"):
|
||||
offset = branch_offset if direction == "increasing" else 0.0
|
||||
for command in commands:
|
||||
angle = math.radians(50.0) * (127.0 - command) / 255.0
|
||||
angle += offset
|
||||
rotation = Rotation.from_rotvec([0.0, 0.0, angle])
|
||||
records.append(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_quaternion_xyzw": rotation.as_quat().tolist(),
|
||||
}
|
||||
)
|
||||
|
||||
curve = fit_rotation_joint_curve(
|
||||
records,
|
||||
zero_command_u8=127,
|
||||
canonical_zero_direction="decreasing",
|
||||
)
|
||||
|
||||
assert curve.angle_rad == curve.decreasing_rad
|
||||
assert curve.decreasing_rad[127] == pytest.approx(0.0, abs=1.0e-9)
|
||||
assert curve.increasing_rad[127] == pytest.approx(
|
||||
branch_offset, abs=1.0e-6
|
||||
)
|
||||
assert curve.maximum_hysteresis_rad == pytest.approx(
|
||||
branch_offset, abs=1.0e-6
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"])
|
||||
def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth(
|
||||
joint: str,
|
||||
@@ -234,6 +307,35 @@ def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
|
||||
) < 1.0e-6
|
||||
|
||||
|
||||
def test_side_roll_axis_point_ignores_real_screw_translation() -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
|
||||
axis_parent = common_from_parent.inv().apply(expected_axis)
|
||||
screw_records = []
|
||||
for record in records:
|
||||
changed = dict(record)
|
||||
fraction = (255.0 - float(record["command_u8"])) / 255.0
|
||||
changed["relative_translation_xyz_m"] = (
|
||||
np.asarray(record["relative_translation_xyz_m"], dtype=float)
|
||||
+ 0.008 * fraction * axis_parent
|
||||
).tolist()
|
||||
screw_records.append(changed)
|
||||
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"middle_mcp_roll_side",
|
||||
screw_records,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
constrained_circle_joints=frozenset({"middle_mcp_roll_side"}),
|
||||
)
|
||||
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert measurement.pose_axis_line_rms_m < 1.0e-6
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 1.0e-6
|
||||
|
||||
|
||||
def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None:
|
||||
records, expected_axis, _ = _arbitrary_tag_records()
|
||||
assert not any(record["command_u8"] == 127 for record in records)
|
||||
@@ -479,15 +581,17 @@ def _solve_synthetic_offsets(
|
||||
side: str,
|
||||
offset_degrees: list[float],
|
||||
*,
|
||||
layout_id: str = "legacy_11",
|
||||
inject_oblique_optical_depth_bias: bool = False,
|
||||
inject_secondary_root_axis_bias_degrees: float = 0.0,
|
||||
inject_secondary_root_point_bias_m: float = 0.0,
|
||||
inject_observer_cone_bias_degrees: float = 0.0,
|
||||
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
|
||||
joint_maximum_offset_degrees: dict[str, float] | None = None,
|
||||
validation_offset_bias_degrees: dict[str, float] | None = None,
|
||||
):
|
||||
hand = get_hand_calibration_profile(side)
|
||||
zero = get_zero_calibration_profile(side)
|
||||
hand = get_hand_calibration_profile(side, layout_id)
|
||||
zero = get_zero_calibration_profile(side, layout_id)
|
||||
source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
|
||||
baseline = [255.0] * 20
|
||||
baseline[6:10] = [127.0] * 4
|
||||
@@ -517,7 +621,9 @@ def _solve_synthetic_offsets(
|
||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
||||
measurements: list[JointAxisMeasurement] = []
|
||||
for cycle in range(3):
|
||||
validation_cycle = 3 if layout_id == "g20_right_15" else 2
|
||||
training_cycles = tuple(range(validation_cycle))
|
||||
for cycle in range(validation_cycle + 1):
|
||||
for joint in zero.axis_joints:
|
||||
state = list(baseline)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
@@ -528,8 +634,18 @@ def _solve_synthetic_offsets(
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero.inherited_zero_joints,
|
||||
)
|
||||
cycle_offsets = dict(offsets)
|
||||
if cycle == validation_cycle:
|
||||
cycle_offsets.update(
|
||||
{
|
||||
name: cycle_offsets[name] + math.radians(value)
|
||||
for name, value in (
|
||||
validation_offset_bias_degrees or {}
|
||||
).items()
|
||||
}
|
||||
)
|
||||
axis, point = model.axis_line(
|
||||
joint, zero_offsets=offsets, joint_angles=angles
|
||||
joint, zero_offsets=cycle_offsets, joint_angles=angles
|
||||
)
|
||||
point_common = base_rotation.apply(point) + base_translation
|
||||
if (
|
||||
@@ -622,14 +738,229 @@ def _solve_synthetic_offsets(
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
hand_type=side,
|
||||
tag_layout=layout_id,
|
||||
joint_maximum_offset_rad={
|
||||
name: math.radians(value)
|
||||
for name, value in (joint_maximum_offset_degrees or {}).items()
|
||||
},
|
||||
training_cycles=training_cycles,
|
||||
validation_cycle=validation_cycle,
|
||||
)
|
||||
return zero, result
|
||||
|
||||
|
||||
def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> None:
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
1.0, -1.0,
|
||||
0.8, -0.7,
|
||||
-0.5, 0.6,
|
||||
1.1, -1.0,
|
||||
]
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right", injected, layout_id="g20_right_15"
|
||||
)
|
||||
|
||||
assert len(zero.direct_zero_joints) == 12
|
||||
assert len(zero.axis_joints) == 17
|
||||
assert result.passed is True
|
||||
finger_rolls = tuple(
|
||||
name
|
||||
for name in zero.direct_zero_joints
|
||||
if name.endswith("_mcp_roll") and not name.startswith("thumb_")
|
||||
)
|
||||
roll_common = float(
|
||||
np.median(
|
||||
[
|
||||
injected[zero.direct_zero_joints.index(name)]
|
||||
for name in finger_rolls
|
||||
]
|
||||
)
|
||||
)
|
||||
for name, expected in zip(zero.direct_zero_joints, injected):
|
||||
if name == "thumb_mcp":
|
||||
expected = 0.0
|
||||
elif name in finger_rolls:
|
||||
expected -= roll_common
|
||||
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
|
||||
expected, abs=0.05
|
||||
)
|
||||
assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx(
|
||||
0.0, abs=0.05
|
||||
)
|
||||
assert result.training_cycles == (0, 1, 2)
|
||||
assert result.validation_cycle == 3
|
||||
assert all(
|
||||
value == pytest.approx(0.0, abs=1.0e-12)
|
||||
for value in result.offset_confidence_half_width_rad.values()
|
||||
)
|
||||
assert all(
|
||||
result.all_active_offsets_rad[f"{finger}_pip"] == 0.0
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
)
|
||||
assert result.observability_parameter_count == 18
|
||||
assert result.observability_rank == 18
|
||||
assert math.isfinite(result.observability_condition_number)
|
||||
assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints)
|
||||
|
||||
|
||||
def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
|
||||
# A shared electrical centre belongs to the four-motor common datum; the
|
||||
# strict 3 deg assembly guard applies to finger-to-finger deviations.
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
6.0, -1.0,
|
||||
6.4, -0.7,
|
||||
5.7, 0.6,
|
||||
6.2, -1.0,
|
||||
]
|
||||
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right", injected, layout_id="g20_right_15"
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
finger_rolls = tuple(
|
||||
name
|
||||
for name in zero.direct_zero_joints
|
||||
if name.endswith("_mcp_roll") and not name.startswith("thumb_")
|
||||
)
|
||||
roll_common = float(
|
||||
np.median(
|
||||
[
|
||||
injected[zero.direct_zero_joints.index(name)]
|
||||
for name in finger_rolls
|
||||
]
|
||||
)
|
||||
)
|
||||
for name, expected in zip(zero.direct_zero_joints, injected):
|
||||
if name == "thumb_mcp":
|
||||
expected = 0.0
|
||||
elif name in finger_rolls:
|
||||
expected -= roll_common
|
||||
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
|
||||
expected, abs=0.05
|
||||
)
|
||||
assert np.median(
|
||||
[result.direct_offsets_rad[name] for name in finger_rolls]
|
||||
) == pytest.approx(0.0, abs=1.0e-10)
|
||||
|
||||
|
||||
def test_right_19_holdout_never_changes_frozen_training_offsets() -> None:
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
1.0, -1.0,
|
||||
0.8, -0.7,
|
||||
-0.5, 0.6,
|
||||
1.1, -1.0,
|
||||
]
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_15",
|
||||
validation_offset_bias_degrees={"thumb_cmc_roll": 2.0},
|
||||
)
|
||||
|
||||
assert math.degrees(
|
||||
result.direct_offsets_rad["thumb_cmc_roll"]
|
||||
) == pytest.approx(injected[0], abs=0.05)
|
||||
assert result.validation_cycle not in result.training_cycles
|
||||
|
||||
|
||||
def test_right_15_palm_pose_and_12_zero_observation_jacobian_is_full_rank() -> None:
|
||||
"""Guard the reviewed 6-palm-DOF plus 12-static-zero observability."""
|
||||
zero = get_zero_calibration_profile("right", "g20_right_15")
|
||||
model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
|
||||
parameter_count = 6 + len(zero.direct_zero_joints)
|
||||
|
||||
def line_observations(parameters: np.ndarray) -> np.ndarray:
|
||||
palm_rotation = Rotation.from_rotvec(parameters[:3])
|
||||
palm_translation = parameters[3:6]
|
||||
offsets = {
|
||||
name: float(value)
|
||||
for name, value in zip(
|
||||
zero.direct_zero_joints, parameters[6:]
|
||||
)
|
||||
}
|
||||
result: list[float] = []
|
||||
for joint in zero.axis_joints:
|
||||
axis, point = model.axis_line(
|
||||
joint, zero_offsets=offsets, joint_angles={}
|
||||
)
|
||||
axis = palm_rotation.apply(axis)
|
||||
point = palm_rotation.apply(point) + palm_translation
|
||||
# An oriented 3-D line is represented by its direction and
|
||||
# Pluecker moment. The moment is invariant to choosing a different
|
||||
# point along the same axis, so no unobservable along-axis Tag
|
||||
# placement is accidentally counted as information.
|
||||
result.extend(float(value) for value in axis)
|
||||
result.extend(float(value) for value in np.cross(point, axis))
|
||||
return np.asarray(result, dtype=float)
|
||||
|
||||
origin = np.zeros(parameter_count, dtype=float)
|
||||
step = 1.0e-6
|
||||
jacobian = np.column_stack(
|
||||
[
|
||||
(
|
||||
line_observations(
|
||||
origin + np.eye(parameter_count, dtype=float)[index] * step
|
||||
)
|
||||
- line_observations(
|
||||
origin - np.eye(parameter_count, dtype=float)[index] * step
|
||||
)
|
||||
)
|
||||
/ (2.0 * step)
|
||||
for index in range(parameter_count)
|
||||
]
|
||||
)
|
||||
|
||||
assert parameter_count == 18
|
||||
assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count
|
||||
|
||||
|
||||
def test_right_15_urdf_writer_changes_only_the_12_static_targets(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
zero = get_zero_calibration_profile("right", "g20_right_15")
|
||||
offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints}
|
||||
destination = write_zero_corrected_urdf(
|
||||
source_urdf=RIGHT_SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_RIGHT_019",
|
||||
offsets_rad=offsets,
|
||||
timestamp="20260818_120000",
|
||||
)
|
||||
|
||||
original = {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(RIGHT_SOURCE_URDF).getroot().findall("joint")
|
||||
}
|
||||
corrected = {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(destination).getroot().findall("joint")
|
||||
}
|
||||
for mesh in ET.parse(destination).getroot().findall(".//mesh"):
|
||||
relative = Path(mesh.get("filename"))
|
||||
copied = destination.parent / relative
|
||||
source = RIGHT_SOURCE_URDF.parent / relative
|
||||
assert copied.is_file()
|
||||
assert copied.stat().st_size == source.stat().st_size
|
||||
changed = set()
|
||||
for name in original:
|
||||
original_origin = original[name].find("origin")
|
||||
corrected_origin = corrected[name].find("origin")
|
||||
if original_origin is None or corrected_origin is None:
|
||||
continue
|
||||
if original_origin.get("rpy") != corrected_origin.get("rpy"):
|
||||
changed.add(name)
|
||||
assert original_origin.get("xyz") == corrected_origin.get("xyz")
|
||||
assert changed == set(zero.direct_zero_joints)
|
||||
assert "thumb_mcp" in changed
|
||||
assert not set(get_hand_calibration_profile(
|
||||
"right", "g20_right_15"
|
||||
).passive_joints) & changed
|
||||
|
||||
|
||||
def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None:
|
||||
zero, result = _solve_synthetic_offsets("right", [0.1] * 7)
|
||||
assert result.passed is True
|
||||
@@ -712,7 +1043,7 @@ def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None
|
||||
|
||||
assert result.passed is True
|
||||
assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
|
||||
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0))
|
||||
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0))
|
||||
assert "thumb_ip" not in result.validation_error_by_joint_rad
|
||||
for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
|
||||
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
|
||||
|
||||
Reference in New Issue
Block a user