Skip to content

feat(force-position): 夹持意图 set_target(..., grip=True),目标落在物体表面也锁存到预算 - #33

Merged
Vertax42 merged 4 commits into
mainfrom
feat/fp-grip-intent
Nov 19, 2026
Merged

Vertax42 merged 4 commits into
mainfrom
feat/fp-grip-intent

Conversation

@Vertax42

Copy link
Copy Markdown
Collaborator

背景

XTac-UMI 的数据是主爪夹着物体录的,策略输出的夹爪目标正好等于物体宽度。部署到从爪时,爪子停在物体表面、离目标不到 arrival_eps(10 mrad),原来"停在目标前方才升力"的规则不会触发,只剩位置弹簧 kp*error ≤ 0.2 N·m:夹得住表面,但夹不紧。

同一个位置目标,在遥操作里(主爪空捏,目标远越过物体)和策略部署里意图不同,只能由调用方显式说明。

改动

d1aea87 夹持意图 set_target(..., grip=True)

  • set_target 新增 grip 参数(C++ 三参重载,Python 关键字 grip=False)。grip=True 时,爪子在目标处或越过目标 grip_overdrive_rad(默认 0.05 rad)以内静止,就按原速率升力并锁存到预算。
  • 松开:夹持期间目标抖动或往合拢方向移动不解锁;目标相对夹持位置张开超过 grip_release_rad(默认 0.02 rad)才松开,松开后再合拢超过同一阈值才重新武装。
  • 空爪:越过目标超过 grip_overdrive_rad 就判定没有物体,放弃夹持、回到目标,只探一次。
  • ForcePositionSnapshot.grip、ForcePositionController.arrival_eps_rad。
  • grip=False 的路径与之前完全一致。

6d7d1c5 柔性物体 + 目标抖动也能锁存;压过目标不再报 OPENING

  • 0016s 真机上,目标设为物体宽度、30 Hz 流式 ±5 mrad 抖动时,物体被压缩、目标又不静止,6 秒只升到 2.9/3.6 N·m,始终没锁存。grip 生效时只保留速度门限。
  • 物体压过目标时状态由 OPENING 改为 CLOSING。

验证

  • 新增 ForcePositionPolicyGrip 15 项闭环测试(摩擦 + 弹性物体模型),并做了变异检验。提交记录中 ctest 590/590 通过。
  • 真机 0016s(RS00,固件 1.2.17,预算 3.6 N·m,物体表面开度 0.5174):
情况 结果
grip=False,目标 = 物体宽度 0.16 N·m(原问题)
grip=True 锁存,2.86 N·m
grip=True,±5 mrad 抖动 2.83 N·m
目标张开 10 mrad 保持夹紧(2.64 N·m)
目标张开 30 mrad 松开回到目标,不重新夹
空爪 冲过 50 mrad 放弃夹持,约 0.2 s 回到目标,无振荡,最大 0.31 N·m

接触到实测 > 2 N·m 约 0.9 s,到锁存约 1.45 s;物体压缩 36–41 mrad(< 50 mrad overdrive)。

本 PR 合进 main-0.4.6-fp-brake-imu-high-rate,与鱼眼 PR #32 互不依赖。

🤖 Generated with Claude Code

Vertax42 and others added 3 commits November 19, 2026 10:03
XTac-UMI 数据是主爪自己夹着物体录的,策略输出的夹爪目标正好是物体宽度。部署到从爪时,爪子
停在物体上、离目标不到 arrival_eps(10 mrad),原有"停在目标前方才升力"的规则不会触发,
只剩位置弹簧 kp*error ≤ 0.2 N·m,夹得住表面但夹不紧。同一个位置目标在遥操作(主爪空捏、
目标远越过物体)和策略部署中意图不同,只能由调用方显式说明。

- set_target 新增 grip 参数(C++ 三参重载、Python 关键字 grip=False)。grip=True 时,爪子在
  目标处或越过目标 grip_overdrive_rad(默认 0.05 rad,覆盖垫片/物体压缩)以内静止,就按原有
  速率升力并锁存到预算;已锁存的夹持不管斜坡与目标相距多远都钉在预算上。
- 松开语义:夹持期间目标抖动或往合拢方向移动不解锁;只有目标相对最合拢的夹持目标张开超过
  grip_release_rad(默认 0.02 rad)才松开。松开后需目标再合拢超过同一阈值才重新武装。
- 空爪:越过目标超过 grip_overdrive_rad 即判定没有物体,放弃夹持、回到目标正常到位,只探一次。
- 夹持生效时丢弃反方向的补力/锁存(grip 标志闪断一帧时,原规则会在压缩过头的爪子上建立
  张开方向的补力,导致夹持无法恢复,测试复现后修复)。
- ForcePositionSnapshot.grip、ForcePositionController.arrival_eps_rad(静态只读)。
- grip=False 的路径与之前完全一致。

测试:新增 ForcePositionPolicyGrip 13 项闭环测试(摩擦+弹性物体模型):目标在物体表面
(±6 mrad)grip=True 锁存到预算、grip=False 仍只有弹簧;柔性物体压过目标仍保持;夹持中
±8 mrad 逐帧抖动不解锁、抖动接近也能锁存;张开不足阈值保持、超过阈值松开且不重新夹、再合拢
重新武装;空爪探出不超过 overdrive 后回到目标到位、无振荡;闭合端点受预算约束;grip 闪断后
重新夹紧;参数校验。变异检验:去掉 grip 升力、overdrive 放弃、eps 解锁豁免、锁存钉住、
反向丢弃、engaged 豁免、松开、重新武装,各自都有测试失败(另一处 abs 变异为等价变异)。
ctest 588/588 通过。

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
0016s(RS00,固件 1.2.17)真机:目标设为物体宽度、30 Hz 流式 ±5 mrad 抖动时,物体被压缩、爪子
一直在缓慢前进,目标又从不静止,升力的"静止"判据(爪子停住或目标稳定)大部分帧不成立,6 秒只
升到 2.9/3.6 N·m、始终没锁存。仿真物体太硬没测出来。grip 生效时调用方已经声明这里有物体,不需要
再用这两个条件区分摩擦停滞,只保留速度门限;空爪仍由 grip_overdrive_rad 兜底。

另:grip 把物体压过目标时状态按"在目标哪一侧"判成 OPENING,改为 CLOSING。

新增两项闭环测试(柔性物体+30 Hz 抖动锁存;压过目标不报 OPENING),修复前均失败。ctest 590/590。

真机(0016s RS00,预算 3.6 N·m,物体表面开度 0.5174):
- grip=False 目标=宽度:实测 0.16 N·m(原问题)
- grip=True:锁存,实测 2.86 N·m;±5 mrad 抖动 2.83 N·m;目标松 5 mrad 2.76 N·m
- 物体压缩 36-41 mrad(< 50 mrad overdrive);接触到实测 >2 N·m 约 0.9 s,到锁存约 1.45 s
- 目标张开 10 mrad 保持夹紧(2.64 N·m),张开 30 mrad 松开回到目标、不重新夹
- 空爪(修复前后一致):冲过 50 mrad 放弃夹持,约 0.2 s 回到目标,无振荡,最大 0.31 N·m

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
d1aea87 新增的两个 ForcePositionConfig 字段没有加进 test_control_config_surface 的字段清单。
补上字段及默认值(0.05 / 0.02 rad)。刹车/起步加速度两个字段在基线分支另行补上。

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
…e' into feat/fp-grip-intent

# Conflicts:
#	python/tests/test_control_config_surface.py
Base automatically changed from main-0.4.6-fp-brake-imu-high-rate to main November 19, 2026 09:23
@Vertax42
Vertax42 merged commit 53b550a into main Nov 19, 2026
3 checks passed
@Vertax42
Vertax42 deleted the feat/fp-grip-intent branch November 19, 2026 09:26
Vertax42 added a commit that referenced this pull request Nov 19, 2026
…ion-solo

CHANGELOG [Unreleased] 两边都保留:鱼眼标定分辨率在前,夹持意图与高频 IMU 在后。

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant