基于 wbc_mpc 中的 shuttle_nav 任务包与 g2wbc-runtime / g2wbc-sdk-integration。第一章是对 code/docs/DESIGN.md 的完整中文翻译与整理;第二、三章分别覆盖真机部署清单与后续优化方向。
本章内容对应仓库内工程记录 code/docs/DESIGN.md(英文原稿的中文译本)。演示与验证产物在 media/(头抓闭环 run、head11)。
本方案建立在 aCodeDog/AgibotG2 已验证的 shuttle 任务之上,不是从零重写:
bend / descend / close / lift / return / hold。| 模式 | head_grasp | 站位距离 | 手臂 | 说明 |
|---|---|---|---|---|
| 参考侧抓 | false | 0.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 做在线限速。
DWA = Dynamic Window Approach(动态窗口法)。经典差分底盘在线速度 \(v\) 与角速度 \(\omega\) 的可行窗口里采样;本仓库的 G2 全向底盘实现(code/task/dwa.py)改为采样航向 × 速度,短视界前滚轨迹,再按「靠近目标 / 离障 / 偏快 / 少换向」打分,选出本 tick 的 \((v_x, v_y)\)。
参考 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)。
头模式只用头的 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。
参考控制器并不直接下发 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
q_ff:从阶段起点到求解帧的五次多项式关节空间混合(head6 通过的模式);dq_fb:每 tick 用带约束的差分 IK QP,把实际 TCP(实际关节、实际底盘)相对规划世界路径(前馈在 plan['base'] 下算出的 TCP,终点即 IK 目标)求修正,并限制在 0.006 rad/tick。同样施加耦合/碰撞/关节限位;QP 区间钳制,避免关节越界把求解器打崩;base_reposition 的 near 分支)。否则 0.55 rad 的到达航向误差会在弯腰起点变成 0.37 m 的世界系 TCP 误差(head10);对齐后闭环跟踪误差最大 2.7 cm(head11)。head_closed_loop: false——纯前馈关节混合(head6 A/B)。
指标写在 task_metrics:tcp_feedback_max_m(相对规划路径的最大实测 TCP 误差)、trim_step_max_rad。
竖直抓取还需要这些修正:
plan_targets);球在中线时毫米级抖动会翻符号,左臂竖直折叠会把夹爪相机撞到底盘(CONTACT:collision_base_link_1 vs collision_gripper_l_camera_link_0),侧抓碰撞集未建模。头模式用 hand='right' 重规划;左臂头抓未验证;BASE_DRIFT(run54);_nav_standoff_m / _side_offset 由头规划导出,弯腰起点不再额外挪底盘;refresh_grasp:IK 帧已按头目标拟合;ShuttleScene._valid + head_grasp):竖直接近、TCP 中心距头 ≤ 2 cm、开度闭合到 26.8 mm 头直径。阶段起点: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=True,stop_reason=operator_stop,无仿真故障标志,模型检查通过;tcp_feedback_max_m = 0.0267 m,trim_step_max_rad = 0.006(在折叠最快段触达每 tick 上限);grasp_active=1,一次闭合,抬升 0.1532 m,最大滑移 0.118 mm / 0.202°,hold 在 38.17 s 完成(2.15 s 门限);task_scene_extras:tcp_left/right_{x,y,z}、com_{x,y,z}、shuttle_head_{x,y,z}。task_metrics:tcp_global(抓取手)、com_global、com_forward_m、standoff_m(头模式 0.60 / 侧抓 0.65)。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。
task/controller.py 把手臂目标规划挪到单独 spawn 的规划进程(10 Hz),从不进 100 Hz 控制 tick:
planning/ready/base_reposition(DWA 阶段)把跟踪球位姿 + 实测 yaw 写入小共享数组;(pre, R, grasp, R, lift, R);_online_stop.set()),使 bend/descend/close/lift/return/hold 的节拍与参考控制器一致;spawn:fork 会继承 MuJoCo/BLAS 状态并卡死父进程的规划 QP;头模式下,规划进程在 reset 时就停;头目标由头 xyz 求解,用前馈混合 + 闭环 trim 执行。DWA 底盘仍每个 tick 按跟踪球重规划。
fault_holds: false 会关掉 runner 的 HOLD/STOP 路径(状态检查、目标过滤、漏发、跟踪/观测拒绝、后端闩锁停止都变透传),只有操作员 PAUSE/ESTOP 能停。头抓验证在此配置下完成。真机 real.yaml 默认 50 Hz,并保留参考的 hold/stop;除非刻意重新投产,不要改 fault_holds。
控制路径只需要头 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,垫片沿球轴闭合,一侧靠近裙边(真羽毛球裙边是柔顺的)。上机前务必在实物球上复核闭合方向与裙边间隙。
./install_into_agibotg2.sh /path/to/AgibotG2
会安装任务与任务配置、应用整文件补丁,并把 demo 部署配置装成 deploy/config/deploy.shuttle_demo.yaml(不覆盖硬件 deploy.yaml)。
主要补丁:
tasks/geometry.py:PrivateModel.robot_com()(排除球的机器人 COM)tasks/shuttle/kinematics.py:com_at/robot_com、退化碰撞法向保护、track_trim()tasks/shuttle/planning.py:可选 hand 覆盖(头模式强制右臂)tasks/shuttle/scene.py:extras 中的 TCP/COM/头、头抓焊死模拟、任务参数sim/graphics.py:跟随相机,便于看头抓与球sim/runner.py、safety.py、config.py:demo 的 fault_holds 透传tasks/shuttle_nav/scene.py:障碍、16×16 地面、底盘关节/伺服范围扩到 x[−1,7]、y[−3,3]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 关键帧)。
仿真验证基于 aCodeDog/AgibotG2 @ 8d0e7b6 的 shuttle_nav。上机时必须改/审的项如下(综合 DEPLOYMENT.md 与 g2wbc-sdk-integration)。
头模式只要羽毛球头 xyz,不要姿态。仿真读场景 extras;真机 state.sim is None,若不接感知,_head_mode 保持 False,退回侧抓。
task/controller.py 两处必须由感知喂数:
reset()——构造目标;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 权限。
R=diag(1,-1,-1);底盘 head.x−0.60、head.y±0.28;强制右臂。
| 项 | 仿真 demo | 真机建议 |
|---|---|---|
head_grasp / head_closed_loop | true / true | 按阶段打开;先侧抓再头抓 |
disable_robot_effort_limits | true | 禁止;用 SDK 力矩限 |
| tracking delay/noise/max_age | 0 / 0 / 0.10 | 按真实感知管线重设 |
fault_holds | false(透传) | 默认 true,保留 hold/stop |
| runner rate | 100 Hz | 投产 50 Hz,复测阶段时长 |
| channels | joints / base_velocity / gripper | 后端必须三通道都 advertise |
ShuttleScene._valid 焊球到 TCP——真机用闭合夹爪,不要移植焊死逻辑。shuttle_head_* 等)是仿真遥测;真机用控制器 FK 的 tcp_global。head_grasp: false,先跑通参考侧抓 + DWA 站位(run55 基线)。tcp_feedback_max_m 小(仿真约 0.027 m)、trim 不长期饱和。close/lift/return/hold。g2wbc-sdk-integration/ 是真机工程层:
launch/pickup.py + run_pickup.sh:H6 mode-3 拾取入口,默认只读,--execute 才下发;mocap/:动捕收数、标定、航向交叉验证;offline_validation/:stall / retry / rolling 等修复的离线证据;/home/agi/g2wbc-runtime,环境 source /home/agi/app/env.sh。下列方向来自 DESIGN/DEPLOYMENT 的已知限制、失败 A/B,以及真机集成中已暴露的工程债,按优先级分组。
| 方向 | 动机 | 思路 |
|---|---|---|
| 物体大位移时重解帧 | 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 路线与头抓闭环统一到同一状态机 |
shuttle_head tracked pose 正式接入 controller 两处读点,并定义 max_age_s / 丢帧策略。mocap/ 已有多次 recalibration 产物)。fault_holds;为漏发、跟踪超时、底盘漂移分别设可解释停机原因。tcp_feedback_max_m、漂移、相位时间线,对齐 head11 基线。pickup.py 大量 *.before_* 备份迁出主目录,保留一条可发布入口。