Anger / Robot / 推理部署 / π₀.₅ / G2 移动弯腰捡羽毛球
AgiBot G2 · shuttle_nav · WBC / 任务空间 QP

移动弯腰捡羽毛球:方案设计、真机部署与优化方向

基于 wbc_mpc 中的 shuttle_nav 任务包与 g2wbc-runtime / g2wbc-sdk-integration。第一章是对 code/docs/DESIGN.md 的完整中文翻译与整理;第二、三章分别覆盖真机部署清单与后续优化方向。

1方案设计

本章内容对应仓库内工程记录 code/docs/DESIGN.md(英文原稿的中文译本)。演示与验证产物在 media/(头抓闭环 run、head11)。

Phase 1DWA 底盘趋近
每 tick 重算站位
对准航向弯腰前对齐 plan yaw
Phase 2bend → descend → close
→ lift → return → hold
全身腰 + 右臂关节
前馈轨迹 + 闭环 trim

1.1 这是什么

本方案建立在 aCodeDog/AgibotG2 已验证的 shuttle 任务之上,不是从零重写:

模式head_grasp站位距离手臂说明
参考侧抓false0.65 m(seed 标定)右run55;参考 _track QP
羽毛球头抓(交付默认)true头后方 0.60 m,侧向 0.28 m右head11;前馈 + 闭环 IK trim

底盘线速度与偏航角速度受模型漂移限制约束(base_translation_stop_m=0.05、base_yaw_stop_rad=0.05、base.max_w=1.0):上限约 0.30 m/s 与 0.20 rad/s,并对 STATE.base_drift 做在线限速。

1.2 DWA 底盘原理(动画)

DWA = Dynamic Window Approach(动态窗口法)。经典差分底盘在线速度 \(v\) 与角速度 \(\omega\) 的可行窗口里采样;本仓库的 G2 全向底盘实现(code/task/dwa.py)改为采样航向 × 速度,短视界前滚轨迹,再按「靠近目标 / 离障 / 偏快 / 少换向」打分,选出本 tick 的 \((v_x, v_y)\)。

一个控制周期里 DWA 在做什么

准备开始…
① 感知与目标读底盘位姿、激光障碍点;用球头 xyz 重算抓取站位(头后 0.60 m / 侧向 0.28 m)。
② 动态窗口采样在航向环 × 若干速度档上生成候选(代码默认 48 向 × 5 速)。
③ 短视界 rollout每条候选按匀速前滚约 0.8 s,画出底盘圆心扫过的轨迹。
④ 代价打分奖励靠近目标、惩罚侵入 body_radius、偏好更快与少换向;过近则硬扣分。
⑤ 选最优并执行取最高分速度向量发出;下一 tick 球动了站位也会变,再重规划。
机器人 羽毛球 / 站位目标 障碍 危险轨迹 优选轨迹 发出的速度

1.3 为什么侧抓不能用 0.5 m

参考 shuttle 的 seed 按 0.65 m 站位标定。强行改成 0.5 m 测过:抓取仍能成功(grasp_active=1,抬升约 0.15 m),但最终 hold 过不了参考自身门限(torso_deg≤10、height_m≤0.1):

单靠手臂够不到地面(肩高约 1.33 m,臂长约 1.0 m,球在约 0.02 m),因此必须弯腰(腰折)。在可达姿态网格搜索中,归档的 C-fold 是质心最优解(COM 在底盘前方 0.136 m);更平的 1.8/−0.9 躯干虽能够到,但 COM 推到 0.179 m,底盘偏斜超过 0.05 m 的 BASE_DRIFT 限(实测 0.0503 m)。

1.4 羽毛球头抓取:前馈 + 闭环 IK

头模式只用头的 xyz(不要姿态):夹爪固定竖直向下 R = [[1,0,0],[0,-1,0],[0,0,-1]],目标点:

pre   = head + [0, 0, 0.055 + 0.10]
grasp = head + [0, 0, 0.055]
lift  = grasp + [0, 0, 0.15]
hold  = head + [0, 0, 0.055 + 0.50]

底盘站在 head.x − 0.60、head.y ± 0.28(落在伸出手臂的一侧),航向用复位时的初始 yaw。

为什么参考 QP 不够用

参考控制器并不直接下发 IK 关节帧。规划用 ShuttleMath.fit_iteration 求出四个帧;执行时每 tick 调用 ShuttleMath.track——这是一个任务空间 QP,在碰撞、耦合、关节速度、以及 target_lead 约束下,把关节引导与 TCP 位姿项(12*pose_error + feed)混合。TCP 项是软代价,会和引导项、约束互相折中。_at_target 检查的是实测 TCP(实际关节的 FK);当底盘站偏时,两项互相打架,弯腰阶段超时(runs 50–54)。帧本身是可达的:fit_iteration 可在一秒内收敛。

执行模式

head_closed_loop: true(交付默认)——实际指令为:

q_cmd = q_ff + dq_fb * dt

head_closed_loop: false——纯前馈关节混合(head6 A/B)。

指标写在 task_metrics:tcp_feedback_max_m(相对规划路径的最大实测 TCP 误差)、trim_step_max_rad。

竖直抓取还需要这些修正:

1.5 头抓验证结果(head11)

阶段起点:planning 0.00 / ready 0.75 / base_reposition 1.96 / bend 20.43 / descend 26.18 / close 27.16 / lift 27.67 / return 28.65 / hold 36.01;hold 在 38.17 s 完成。

结果passed
底盘漂移0.038 m / 0.020 rad
TCP 跟踪误差2.7 cm
抬升 / 滑移0.153 m / 0.12 mm

1.6 输出与坐标系约定(不报底盘全局坐标)

run55 侧抓结果(在线重规划、hold 关闭):passed=True,无故障标志,零跟踪/观测 hold;记录了 2 次漏发并忽略(fault_holds: false,继续跑);最大底盘漂移 0.0379 m / 0.0198 rad,grasp_active=1,抬升 0.149 m,球 z=1.284 m,hold 在 19.4 s 成熟,最终 TCP (5.849, 0.012, 1.320) m。

1.7 在线重规划(独立规划进程)

task/controller.py 把手臂目标规划挪到单独 spawn 的规划进程(10 Hz),从不进 100 Hz 控制 tick:

头模式下,规划进程在 reset 时就停;头目标由头 xyz 求解,用前馈混合 + 闭环 trim 执行。DWA 底盘仍每个 tick 按跟踪球重规划。

运行时说明:仿真 demo 的 fault_holds: false 会关掉 runner 的 HOLD/STOP 路径(状态检查、目标过滤、漏发、跟踪/观测拒绝、后端闩锁停止都变透传),只有操作员 PAUSE/ESTOP 能停。头抓验证在此配置下完成。真机 real.yaml 默认 50 Hz,并保留参考的 hold/stop;除非刻意重新投产,不要改 fault_holds。

1.8 头模式的硬件接口

控制路径只需要头 xyz。仿真读 state.sim.task['shuttle_head_{x,y,z}'](场景 extras),位置在 reset()(构目标)和 descend 预闭合门。真机上把这两处换成感知提供的头 xyz(tracked pose 名 shuttle_head 最自然)。其余——IK 帧、前馈混合、闭环 trim、底盘 DWA——不变。trim 权限故意很小(0.006 rad/tick);若抓取过程中物体移动更大,应重解帧(同 fit_iteration)或刻意提高 trim 权限。场景焊死是仿真仿真;真机靠闭合夹爪抓取。

固定竖直抓的 caveat:没有姿态时,闭合轴是世界 x。本夹具上球大致沿 x,垫片沿球轴闭合,一侧靠近裙边(真羽毛球裙边是柔顺的)。上机前务必在实物球上复核闭合方向与裙边间隙。

1.9 安装到 AgibotG2 与仿真运行

./install_into_agibotg2.sh /path/to/AgibotG2

会安装任务与任务配置、应用整文件补丁,并把 demo 部署配置装成 deploy/config/deploy.shuttle_demo.yaml(不覆盖硬件 deploy.yaml)。

主要补丁:

PYTHONPATH=deploy/src python3 -m g2_deploy.cli sim --network=lo --task shuttle_nav \
  --config deploy/config/deploy.shuttle_demo.yaml \
  --record /tmp/g2_shuttle_nav.mp4 --report /tmp/nav_report.json
PYTHONPATH=deploy/src python3 -m g2_deploy.cli run --network=lo --task shuttle_nav \
  --config deploy/config/deploy.shuttle_demo.yaml \
  --rate 100 --script deploy/config/tasks/shuttle_nav_smoke.yaml

停仿真用 SIGTERM / Ctrl-C,不要 SIGKILL:录像器在关机时写 mp4 索引。切回参考侧抓:在 config/shuttle_nav.yaml 设 head_grasp: false(DWA 站位回到 0.65 m,沿用归档 seed 关键帧)。

2真机部署

仿真验证基于 aCodeDog/AgibotG2 @ 8d0e7b6 的 shuttle_nav。上机时必须改/审的项如下(综合 DEPLOYMENT.md 与 g2wbc-sdk-integration)。

2.1 头位置输入(头抓必接)

头模式只要羽毛球头 xyz,不要姿态。仿真读场景 extras;真机 state.sim is None,若不接感知,_head_mode 保持 False,退回侧抓。

task/controller.py 两处必须由感知喂数:

  1. reset()——构造目标;
  2. descend 预闭合门——再读 live 头,确认 TCP 仍跨在头上。
def _head_xyz(self, state):
    pose = (state.tracked_poses or {}).get('shuttle_head')
    if pose is not None:
        return np.asarray(pose.p, dtype=float)
    task = state.sim.task if state.sim is not None else None
    if task and 'shuttle_head_x' in task:
        return np.array([task['shuttle_head_x'], task['shuttle_head_y'],
                         task['shuttle_head_z']], dtype=float)
    return None

默认假设头从 reset 到 hold 静止。若感知可持续更新,应在弯腰起点刷新目标;闭环 trim 只有 0.006 rad/tick 权限。

几何校核量:头半径 0.0134 m;TCP = head+[0,0,0.055];pre=+0.155;lift=grasp+0.15;hold=head+0.555; 竖直 R=diag(1,-1,-1);底盘 head.x−0.60、head.y±0.28;强制右臂。

2.2 配置审查

项仿真 demo真机建议
head_grasp / head_closed_looptrue / true按阶段打开;先侧抓再头抓
disable_robot_effort_limitstrue禁止;用 SDK 力矩限
tracking delay/noise/max_age0 / 0 / 0.10按真实感知管线重设
fault_holdsfalse(透传)默认 true,保留 hold/stop
runner rate100 Hz投产 50 Hz,复测阶段时长
channelsjoints / base_velocity / gripper后端必须三通道都 advertise

2.3 仿真专用逻辑勿上机

2.4 建议上机顺序

  1. head_grasp: false,先跑通参考侧抓 + DWA 站位(run55 基线)。
  2. 机器人静止,喂入测绘的静态头 xyz,检查规划 TCP:pre 在头上 0.155 m、grasp 0.055 m、夹爪竖直;核对站位 0.60 m / 侧偏 0.28 m。
  3. 轮子锁死或低速,只跑 bend/descend,看首次接触:垫片跨在头上、不刮裙边。
  4. 打开闭环,确认 tcp_feedback_max_m 小(仿真约 0.027 m)、trim 不长期饱和。
  5. 再跑完整 close/lift/return/hold。

2.5 SDK 集成包现状

g2wbc-sdk-integration/ 是真机工程层:

已知限制(部署侧同样成立):左臂头抓未验证;trim 只能跟小扰动;头默认静态;100 Hz 调的 timing 需在 50 Hz 复测。

3优化方向

下列方向来自 DESIGN/DEPLOYMENT 的已知限制、失败 A/B,以及真机集成中已暴露的工程债,按优先级分组。

3.1 控制与规划

方向动机思路
物体大位移时重解帧trim 仅 0.006 rad/tick跟踪误差超阈触发 fit_iteration;或分阶段提高 trim 权限
50 Hz 复标定阶段时长验证在 100 Hz按 rate 缩放相位时长与门限;对比 hold 成熟时间
左臂头抓相机碰撞未建模补碰撞几何 + 新 C-fold;或限制仅右臂产品化
闭合轴相对裙边无姿态时闭合轴=世界 x引入粗姿态 / 强制相对球轴的闭合方向
真正全身 MPC现为任务空间 QP + 差分 IK若要预测底盘-腰-臂耦合,可在 QP 外加短视界 MPC;先做仿真 A/B
动态球 / 滚动拾取集成包已有 rolling 实验把 rolling 路线与头抓闭环统一到同一状态机

3.2 感知与标定

3.3 安全与工程化

3.4 性能与产品化