feat: add head control and joint data decoder

README.md

@@ -47,6 +47,9 @@ TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务
   稳定松开，再重新长按 3 秒。pending 和活动会话都不会自动重试或自动恢复。
 - 遥操开启且双摇杆先回中过一次后，左摇杆上下直接控制 HBWALK 前进/后退。
 - 遥操开启且双摇杆先回中过一次后，右摇杆左右直接控制 HBWALK 原地转向。
+- 遥操开启且右摇杆纵轴先回中过一次后，按住左 Z + 右摇杆上下调整
+  头部相机视角：上推抬头、下推低头；无需 R3。按住 Z 时会屏蔽右摇杆水平
+  转向，避免调整视角时误转。软件工作区为 `-10°～+50°`，最大 `0.2 rad/s`。
 - 右 B 连续 1 秒：左右手同时进入厂商“单食指”姿态；松开至少 0.5 秒后再次连续
   1 秒退出。手指动作继续受 `400 units/s` 限速。
 - 按住右 A：右手限速摆到目标位置 `[428,735,500,77,77,72]`；松开 A 后恢复
@@ -57,9 +60,9 @@ TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务
 ## 数采保存
 
 Nvidia 独立服务录制明确白名单内的机器人实测状态、双臂/双手命令与反馈、行走、
-IMU、电源状态、xTELE 应用帧以及头/腰 Orbbec 的 RGB-D。RGB 使用标准 JPEG 压缩流，
-16 位深度使用无损 compressedDepth PNG，并同时保存相机内参与静态 TF。头部和腰部
-相机都是可选流：服务运行就自动采集，未运行不阻塞核心 episode；manifest 会分别标记
+IMU、电源状态、xTELE 应用帧以及头部 Orbbec 的 RGB-D。RGB 使用标准 JPEG 压缩流，
+16 位深度使用无损 compressedDepth PNG，并同时保存相机内参与静态 TF。头部相机
+是可选流：服务运行就自动采集，未运行不阻塞核心 episode；manifest 会标记
 `absent/healthy/partial/low_rate`。它不接管或停止厂家
 `/record_bag_node`，也不录点云。正常结束后先校验 MCAP、生成 SHA-256
 `manifest.json` 和 `READY`，再由

docs/天工3.0同构臂遥操与数采跨设备迁移手册.md

@@ -488,8 +488,9 @@ joint_goal_rad = [<新机器人实测的14个弧度值>]
 - `[hands].closed_normalized` 和单食指姿态；
 - `[control]` 关节限位是否仍对应当前型号和 SDK；
 - `[locomotion]` 的 Topic、方向和厂家速度上限；
+- `[head]` 的 `/head/cmd`、`/robot_state`、俯仰方向、工作区、速度/加速度和外部命令租约；
 - `base_directory` 是否仍是 `/home/nvidia/tg3_data_collection`；
-- 相机 Topic 是否仍为 `/ob_camera_head/...` 和 `/ob_camera_waist/...`。
+- 头部相机 Topic 是否仍为 `/ob_camera_head/...`。
 
 ### 6.5 新机器人必须重新采集 Home
 
@@ -568,7 +569,8 @@ restart 本机同步服务。
 | 换 TS1P | EAI 原厂标定、EAI Peer、机器人 `expected_iarm_id`，必要时方向和按键复核 |
 | 换灵巧手型号 | 必须改代码和消息映射；不能只换 Topic 字符串 |
 | 相机未安装或未启动 | 不阻塞数采；manifest 标记 `absent` |
-| 头/腰相机启动 | 下一条数采自动包含对应压缩 RGB-D，并记录质量状态 |
+| 头部相机启动 | 下一条数采自动包含头部压缩 RGB-D，并记录质量状态 |
+| 腰部相机启动 | 不录制；当前白名单不含 `/ob_camera_waist/...` |
 
 ## 9. 统一启动顺序与现场验收
 
@@ -618,7 +620,7 @@ python3 -m json.tool Data_Get/sync_status.json
 - EAI 本地 5003/5001 数据持续更新；
 - 待机时 EAI 不发送业务帧；
 - PS 同步无错误且 pending 为 0；
-- 头/腰相机未启动不影响 recorder 服务 active。
+- 头部相机未启动不影响 recorder 服务 active；腰部相机不在数采白名单。
 
 ### 9.3 逐级运动验收
 
@@ -631,8 +633,10 @@ python3 -m json.tool Data_Get/sync_status.json
 5. 长按右 B 1 秒验证左右手同时单食指，再松开 0.5 秒、长按 1 秒退出；
 6. 确认双摇杆回中后，直接小幅推动左摇杆验证前后，回中应立即零速；
 7. 直接小幅横推右摇杆验证原地转向，回中应立即零速；
-8. L3 长按 1 秒开始短数采，再次长按 1 秒结束；
-9. 长按 Z+C 3 秒结束遥操，确认限速回新机器人的 Home。
+8. 右摇杆纵轴回中后，按住左 Z 再小幅上推/下推，确认头部相机视角
+   抬高/降低；同时小幅横向偏移不应让机器人转向，松开 Z 或回中后停止更新；
+9. L3 长按 1 秒开始短数采，再次长按 1 秒结束；
+10. 长按 Z+C 3 秒结束遥操，确认限速回新机器人的 Home。
 
 不要用本项目发布 `gotoHBWALK`，不要向厂家 `/proc_manager/config/notify` 发送伪进程启停
 JSON，也不要为了迁移删除厂家限位、电流、碰撞或急停保护。

docs/天工3.0本地同构臂遥操迁移部署指南.md

@@ -926,12 +926,15 @@ systemctl --user enable --now tg3-omnisocket-sender.service
 11. 仅在 `[locomotion].enabled=true` 且相应链路断流保护已经验证后：先确认双摇杆回中，
     再把左摇杆小幅向前推出死区，确认下一个 50 Hz 周期立即响应；回中后确认立即零速停止；
 12. 在同一行走前提下，直接把右摇杆小幅横推，确认机器人原地转向；回中应立即清零角速度；
-13. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home；
-14. 仅在 OmniSocket 模式、机器人已静止且急停可用时测试 Hub 断线：EAI sender 的 KCP
+13. 确认右摇杆纵轴回中后，按住左 Z 再小幅上推/下推，验证头部相机抬头/低头；
+    按 Z 期间右摇杆水平轴不应让机器人转向。检查角度始终位于 `-10°～+50°`，且
+    外部 `/head/cmd` 活跃时本项目会暂停头部输出；
+14. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home；
+15. 仅在 OmniSocket 模式、机器人已静止且急停可用时测试 Hub 断线：EAI sender 的 KCP
     反馈超时约为 500 ms，触发后会作废 session；机器人若持续约 2 秒收不到活动帧，会另行
     重建我们的接收桥。不应期待断线后透明续控；恢复后先确认未武装，稳定松开 Z+C 后再重新长按
     启动。纯 ZMQ 模式在完成本地断流门控前禁止做运动中的断链测试；
-15. 记录 SSH 地址、Home、行走限速、手部端点和单食指姿态到设备档案；OmniSocket 模式
+16. 记录 SSH 地址、Home、行走限速、手部端点和单食指姿态到设备档案；OmniSocket 模式
     另记 Peer ID/Hub，纯 ZMQ 模式改记 EAI 固定局域网 IP、TCP 5003 防火墙范围和断流门控参数。
 
 迁移时确认目标机提供 `/hric/robot/cmd_vel`（`geometry_msgs/msg/TwistStamped`），并保持
@@ -1026,6 +1029,7 @@ ZMQ 专用断流门控前不得启用运动发布。
 
 ```text
 tg3_local_teleop.py
+head_control.py
 data_collection.py
 data_recorder_protocol.py
 data_recorder_node.py
@@ -1052,38 +1056,37 @@ systemctl --user restart tg3-local-teleop.service
 ```
 
 录制脚本必须依次加载 `/opt/ros/jazzy`、`/home/nvidia/xos` 和
-`/opt/robot_tele_server/install`，但**不能**再加载本项目 `ros2_py/install`。后者是桥接
-Python 进程专用的最小 `ArmStatus` 绑定，会遮蔽厂家完整 `ros2_bridge_msgs` C++ 库，导致
+`/opt/robot_tele_server/install`，但**不能**再加载本项目 `ros2_py/install`。后者只是遥操桥
+Python 进程专用的消息绑定（含 `ArmStatus`/`RobotState`/`HeadCtrl`），并不是
+厂家完整 `ros2_bridge_msgs` 包；它会遮蔽厂家 C++ 库，导致
 rosbag 加载 `RobotState/ArmCtrl` 时出现 undefined symbol。`run.sh` 仍需项目 overlay；
 `run_data_recorder.sh` 只用厂家完整消息 overlay。
 还需 Python 3 的 PyYAML（Ubuntu 包 `python3-yaml`）解析 rosbag metadata。
 迁移时在 `config.toml [data_collection]` 核对 `base_directory`、100 GiB 余量、30 分钟
 上限、明确 topic 白名单和 required topics。不要改成 `ros2 bag record -a`，点云需另行
-估算带宽和磁盘后再加入。当前白名单已经包含头/腰 Orbbec 的 JPEG RGB、无损 PNG 深度、
-CameraInfo、Metadata 和 `/tf_static`。头、腰相机都是 optional：服务已启动时 rosbag 自动
+估算带宽和磁盘后再加入。当前白名单只包含头部 Orbbec 的 JPEG RGB、无损 PNG 深度、
+CameraInfo、Metadata 和 `/tf_static`。头部相机是 optional：服务已启动时 rosbag 自动
 发现并采集；未启动时不影响核心 episode 开始或进入 READY。收尾后 manifest 会分别把
-两台相机标成 `absent`、`healthy`、`partial` 或 `low_rate`，后两种附带质量警告但不阻塞
+头部相机标成 `absent`、`healthy`、`partial` 或 `low_rate`，后两种附带质量警告但不阻塞
 核心数据保存。
 
-需要采集某台相机的完整 episode 时，先确认相机服务和发布者；不需要相机时可保持服务
+需要采集头部相机的完整 episode 时，先确认相机服务和发布者；不需要相机时可保持服务
 停止，L3 数采仍可正常启动：
 
 ```bash
-systemctl is-active orbbec_waist.service orbbec_head.service
-sudo systemctl enable --now orbbec_waist.service
-# 同时需要头部 RGB-D 时：
+systemctl is-active orbbec_head.service
 sudo systemctl enable --now orbbec_head.service
 
-ros2 topic info /ob_camera_waist/color/image_raw/compressed
-ros2 topic info /ob_camera_waist/depth/image_raw/compressedDepth
+ros2 topic info /ob_camera_head/color/image_raw/compressed
+ros2 topic info /ob_camera_head/depth/image_raw/compressedDepth
 ```
 
-希望采集腰部图像时，两条腰部话题应显示 `Publisher count: 1`；头部同理。原始
+希望釆头部图像时，两条话题都应显示 `Publisher count: 1`。原始
 1280x720 RGB+深度约
 `8.3 GB/min/相机`，所以默认不重复录 raw，而是录标准 image_transport 压缩流；现场
-单相机约 `1.3–1.5 GB/min`，实际随画面变化。头部和腰部都运行时应按约两倍预留空间与
-同步时间。`minimum_free_gib=100` 是停止下限，不是整条 episode 的容量预算；双相机
-长录制前至少预留“100 GiB + 预计数据量”，并在短录包中先确认实际带宽。
+单相机约 `1.3–1.5 GB/min`，实际随画面变化。`minimum_free_gib=100` 是停止下限，
+不是整条 episode 的容量预算；长录制前至少预留“100 GiB + 预计数据量”，并在短录包中
+先确认实际带宽。
 
 ### 11.2 PS 本机
 

tg3_data_collection/README.md

@@ -116,6 +116,32 @@ NOREPLACE 语义原子发布、fsync 父目录，最后写入并 fsync `VERIFIED
 刚创建的失败 payload，因此机器人不会长期保留失败 MCAP。若以后改回 `true` 以保留故障
 诊断，必须另建独立验真与回收协议，不能整目录清空。
 
+## 解码双臂关节数据
+
+MCAP 内的自定义 TG3 消息使用 CDR 编码。解码器读取项目保存的消息定义，并把实际反馈、
+遥操目标、厂家桥后命令和 xTELE 原始目标分别导出为长表 CSV。首次使用在独立虚拟环境
+安装依赖：
+
+```bash
+python3 -m venv /tmp/tg3-mcap-venv
+/tmp/tg3-mcap-venv/bin/pip install mcap==1.4.0 mcap-ros2-support==0.5.7
+
+/tmp/tg3-mcap-venv/bin/python tg3_data_collection/decode_joint_data.py \
+  Data_Get/<episode_id>
+```
+
+输出位于 `Data_Get/<episode_id>/decoded_joints/`：
+
+- `robot_arm_feedback.csv`：`/robot_state`、`/freq_change/arm_status` 和
+  `/data_logger/arm_status` 的实测位置、速度、电流、温度及错误码；
+- `teleop_joint_target.csv`：`/encoder_identical_joint` 的 14 轴遥操目标；
+- `vendor_arm_command.csv`：`/arm/cmd` 的厂家桥后控制参数；
+- `iarm_source_joint.csv`：`/tg3/data_collection/iarm_frame` 中的 TS1P 原始目标；
+- `summary.json`：关节顺序、Motor ID、单位和行数。
+
+关节顺序为左臂 7 轴后右臂 7 轴，Motor ID 分别为 `11..17` 和 `21..27`；位置单位
+为 rad，速度单位为 rad/s。输出目录必须不存在，避免误覆盖已经分析或标注过的数据。
+
 迁移后需要同步修改 service 中的本机项目路径和机器人 SSH 地址。如果 Nvidia 的
 `192.168.41.2` 改变，只改 unit 的 `--remote` 并重新部署 helper；它不在机器人
 `config.toml` 或 OmniSocket Peer 设置中。启用自动删除时远端路径必须保持固定的

tg3_data_collection/decode_joint_data.py

@@ -0,0 +1,377 @@
+#!/usr/bin/env python3
+"""Decode TG3 joint streams from one locally stored MCAP episode into CSV."""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+from pathlib import Path
+from typing import Any, Callable
+
+
+ARM_MOTOR_IDS = [*range(11, 18), *range(21, 28)]
+JOINT_NAMES = [
+    *(f"left_joints_{index}" for index in range(7)),
+    *(f"right_joints_{index}" for index in range(7)),
+]
+CUSTOM_TOPICS = {
+    "/robot_state": "ros2_bridge_msgs/msg/RobotState",
+    "/freq_change/arm_status": "ros2_bridge_msgs/msg/ArmStatus",
+    "/data_logger/arm_status": "ros2_bridge_msgs/msg/ArmStatus",
+    "/arm/cmd": "ros2_bridge_msgs/msg/ArmCtrl",
+}
+TOPICS = [
+    "/robot_state",
+    "/encoder_identical_joint",
+    "/freq_change/arm_status",
+    "/data_logger/arm_status",
+    "/arm/cmd",
+    "/tg3/data_collection/iarm_frame",
+]
+
+
+def _message_directory() -> Path:
+    return (
+        Path(__file__).resolve().parents[1]
+        / "tg3_local_teleop"
+        / "ros2_py"
+        / "src"
+        / "ros2_bridge_msgs"
+        / "msg"
+    )
+
+
+def _read_definition(name: str) -> str:
+    path = _message_directory() / f"{name}.msg"
+    if not path.is_file():
+        raise RuntimeError(f"required message definition is missing: {path}")
+    return path.read_text(encoding="utf-8")
+
+
+def _definition_bundle(root_name: str, dependencies: list[tuple[str, str]]) -> str:
+    if root_name == "ArmCtrl":
+        text = (
+            "std_msgs/Header header\n"
+            "uint8 mode\n"
+            "uint8 label\n"
+            "uint8 reserved\n"
+            "ros2_bridge_msgs/MotorCtrl[] ctrl\n"
+        )
+    else:
+        text = _read_definition(root_name)
+    for full_name, definition in dependencies:
+        text += f"\n===\nMSG: {full_name}\n{definition}"
+    return text
+
+
+def _custom_decoders() -> dict[str, Callable[[bytes], Any]]:
+    try:
+        from mcap_ros2._dynamic import generate_dynamic
+    except ImportError as exc:
+        raise RuntimeError(
+            "missing MCAP decoder; install with: pip install mcap mcap-ros2-support"
+        ) from exc
+
+    header = "builtin_interfaces/Time stamp\nstring frame_id\n"
+    motor_status = _read_definition("MotorStatus")
+    common = [
+        ("std_msgs/msg/Header", header),
+        ("ros2_bridge_msgs/msg/MotorStatus", motor_status),
+    ]
+    robot_dependencies = common + [
+        (f"ros2_bridge_msgs/msg/{name}", _read_definition(name))
+        for name in (
+            "HeadStatus",
+            "WaistStatus",
+            "LegStatus",
+            "ArmStatus",
+            "ImuStatus",
+        )
+    ]
+    schemas = {
+        "ros2_bridge_msgs/msg/RobotState": _definition_bundle(
+            "RobotState", robot_dependencies
+        ),
+        "ros2_bridge_msgs/msg/ArmStatus": _definition_bundle(
+            "ArmStatus", common
+        ),
+        "ros2_bridge_msgs/msg/ArmCtrl": _definition_bundle(
+            "ArmCtrl",
+            [
+                ("std_msgs/msg/Header", header),
+                ("ros2_bridge_msgs/msg/MotorCtrl", _read_definition("MotorCtrl")),
+            ],
+        ),
+    }
+    return {
+        type_name: generate_dynamic(type_name, text)[type_name]
+        for type_name, text in schemas.items()
+    }
+
+
+def _stamp_ns(header: Any) -> int:
+    return int(header.stamp.sec) * 1_000_000_000 + int(header.stamp.nanosec)
+
+
+def _side_and_index(motor_id: int) -> tuple[str, int]:
+    if 11 <= motor_id <= 17:
+        return "left", motor_id - 11
+    if 21 <= motor_id <= 27:
+        return "right", motor_id - 21
+    return "unknown", -1
+
+
+def _open_csv(path: Path, columns: list[str]) -> tuple[Any, csv.DictWriter]:
+    stream = path.open("x", encoding="utf-8", newline="")
+    writer = csv.DictWriter(stream, fieldnames=columns)
+    writer.writeheader()
+    return stream, writer
+
+
+def decode_episode(episode: Path, output: Path) -> dict[str, int]:
+    try:
+        from mcap.reader import make_reader
+        from mcap_ros2.decoder import DecoderFactory
+    except ImportError as exc:
+        raise RuntimeError(
+            "missing MCAP decoder; install with: pip install mcap mcap-ros2-support"
+        ) from exc
+
+    bag_files = sorted((episode / "bag").glob("*.mcap"))
+    if not bag_files:
+        raise RuntimeError(f"no MCAP files found under {episode / 'bag'}")
+    output.mkdir(parents=True, exist_ok=False)
+
+    feedback_columns = [
+        "bag_time_ns",
+        "ros_time_ns",
+        "topic",
+        "side",
+        "joint_index",
+        "motor_id",
+        "position_rad",
+        "speed_rad_s",
+        "current_a",
+        "temperature_c",
+        "mos_temperature_c",
+        "error",
+    ]
+    target_columns = [
+        "bag_time_ns",
+        "ros_time_ns",
+        "joint_index",
+        "joint_name",
+        "motor_id",
+        "side",
+        "position_rad",
+        "velocity_rad_s",
+        "effort",
+    ]
+    command_columns = [
+        "bag_time_ns",
+        "ros_time_ns",
+        "mode",
+        "label",
+        "motor_id",
+        "side",
+        "joint_index",
+        "kp",
+        "kd",
+        "position_rad",
+        "speed_rad_s",
+        "torque_feedforward",
+        "current_limit_a",
+    ]
+    source_columns = [
+        "bag_time_ns",
+        "source_time_ms",
+        "joint_index",
+        "joint_name",
+        "motor_id",
+        "side",
+        "position_rad",
+    ]
+    streams: list[Any] = []
+    feedback_stream, feedback_writer = _open_csv(
+        output / "robot_arm_feedback.csv", feedback_columns
+    )
+    target_stream, target_writer = _open_csv(
+        output / "teleop_joint_target.csv", target_columns
+    )
+    command_stream, command_writer = _open_csv(
+        output / "vendor_arm_command.csv", command_columns
+    )
+    source_stream, source_writer = _open_csv(
+        output / "iarm_source_joint.csv", source_columns
+    )
+    streams.extend((feedback_stream, target_stream, command_stream, source_stream))
+
+    counts = {
+        "robot_arm_feedback_rows": 0,
+        "teleop_joint_target_rows": 0,
+        "vendor_arm_command_rows": 0,
+        "iarm_source_joint_rows": 0,
+    }
+    custom_decoders = _custom_decoders()
+    standard_factory = DecoderFactory()
+    try:
+        for bag_file in bag_files:
+            with bag_file.open("rb") as bag_stream:
+                reader = make_reader(bag_stream)
+                for schema, channel, message in reader.iter_messages(topics=TOPICS):
+                    topic = channel.topic
+                    if topic in CUSTOM_TOPICS:
+                        decoded = custom_decoders[CUSTOM_TOPICS[topic]](message.data)
+                    else:
+                        decoder = standard_factory.decoder_for(
+                            channel.message_encoding, schema
+                        )
+                        if decoder is None:
+                            raise RuntimeError(
+                                f"no decoder for {topic} ({schema.name if schema else 'no schema'})"
+                            )
+                        decoded = decoder(message.data)
+
+                    if topic in (
+                        "/robot_state",
+                        "/freq_change/arm_status",
+                        "/data_logger/arm_status",
+                    ):
+                        status_message = decoded.arm if topic == "/robot_state" else decoded
+                        for motor in status_message.status:
+                            motor_id = int(motor.name)
+                            side, joint_index = _side_and_index(motor_id)
+                            feedback_writer.writerow(
+                                {
+                                    "bag_time_ns": message.log_time,
+                                    "ros_time_ns": _stamp_ns(decoded.header),
+                                    "topic": topic,
+                                    "side": side,
+                                    "joint_index": joint_index,
+                                    "motor_id": motor_id,
+                                    "position_rad": motor.pos,
+                                    "speed_rad_s": motor.speed,
+                                    "current_a": motor.current,
+                                    "temperature_c": motor.temperature,
+                                    "mos_temperature_c": motor.mos_temperature,
+                                    "error": motor.error,
+                                }
+                            )
+                            counts["robot_arm_feedback_rows"] += 1
+                    elif topic == "/encoder_identical_joint":
+                        for index, position in enumerate(decoded.position):
+                            motor_id = ARM_MOTOR_IDS[index] if index < 14 else -1
+                            side, _ = _side_and_index(motor_id)
+                            target_writer.writerow(
+                                {
+                                    "bag_time_ns": message.log_time,
+                                    "ros_time_ns": _stamp_ns(decoded.header),
+                                    "joint_index": index,
+                                    "joint_name": decoded.name[index]
+                                    if index < len(decoded.name)
+                                    else "",
+                                    "motor_id": motor_id,
+                                    "side": side,
+                                    "position_rad": position,
+                                    "velocity_rad_s": decoded.velocity[index]
+                                    if index < len(decoded.velocity)
+                                    else "",
+                                    "effort": decoded.effort[index]
+                                    if index < len(decoded.effort)
+                                    else "",
+                                }
+                            )
+                            counts["teleop_joint_target_rows"] += 1
+                    elif topic == "/arm/cmd":
+                        for motor in decoded.ctrl:
+                            motor_id = int(motor.name)
+                            side, joint_index = _side_and_index(motor_id)
+                            command_writer.writerow(
+                                {
+                                    "bag_time_ns": message.log_time,
+                                    "ros_time_ns": _stamp_ns(decoded.header),
+                                    "mode": decoded.mode,
+                                    "label": decoded.label,
+                                    "motor_id": motor_id,
+                                    "side": side,
+                                    "joint_index": joint_index,
+                                    "kp": motor.kp,
+                                    "kd": motor.kd,
+                                    "position_rad": motor.pos,
+                                    "speed_rad_s": motor.spd,
+                                    "torque_feedforward": motor.tor,
+                                    "current_limit_a": motor.cur,
+                                }
+                            )
+                            counts["vendor_arm_command_rows"] += 1
+                    elif topic == "/tg3/data_collection/iarm_frame":
+                        payload = json.loads(decoded.data)
+                        positions = payload.get("arm", {}).get("position", {})
+                        values = [
+                            *positions.get("left", []),
+                            *positions.get("right", []),
+                        ]
+                        if len(values) != 14:
+                            continue
+                        source_time = payload.get("timestamp", "")
+                        for index, position in enumerate(values):
+                            motor_id = ARM_MOTOR_IDS[index]
+                            side, _ = _side_and_index(motor_id)
+                            source_writer.writerow(
+                                {
+                                    "bag_time_ns": message.log_time,
+                                    "source_time_ms": source_time,
+                                    "joint_index": index,
+                                    "joint_name": JOINT_NAMES[index],
+                                    "motor_id": motor_id,
+                                    "side": side,
+                                    "position_rad": position,
+                                }
+                            )
+                            counts["iarm_source_joint_rows"] += 1
+    finally:
+        for stream in streams:
+            stream.close()
+
+    (output / "summary.json").write_text(
+        json.dumps(
+            {
+                "episode": episode.name,
+                "joint_order": JOINT_NAMES,
+                "motor_ids": ARM_MOTOR_IDS,
+                "units": {
+                    "position": "rad",
+                    "speed": "rad/s",
+                    "current": "A",
+                    "temperature": "degC",
+                },
+                "rows": counts,
+            },
+            ensure_ascii=False,
+            indent=2,
+        )
+        + "\n",
+        encoding="utf-8",
+    )
+    return counts
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("episode", type=Path, help="Data_Get episode directory")
+    parser.add_argument(
+        "--output",
+        type=Path,
+        help="new output directory (default: <episode>/decoded_joints)",
+    )
+    args = parser.parse_args()
+    episode = args.episode.resolve()
+    output = (args.output or episode / "decoded_joints").resolve()
+    counts = decode_episode(episode, output)
+    print(json.dumps({"output": str(output), "rows": counts}, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tg3_local_teleop/README.md

@@ -15,6 +15,8 @@ TS1P 同构臂
   -> 天工 3.0 BrainCo Revo2 双灵巧手
   -> /hric/robot/cmd_vel（左摇杆前后 / 右摇杆转向；50 Hz TwistStamped）
   -> 天工 3.0 HBWALK 行走
+  -> /head/cmd（左 Z + 右摇杆上下；Head Pitch 位置模式）
+  -> 天工 3.0 头部相机上下视角
 ```
 
 ## 自动运行与操作
@@ -38,6 +40,13 @@ TS1P 同构臂
   `0.8 rad/s`；死区 `0.2`，输出频率 `50 Hz`。
 - xTELE 0.1.2 的摇杆数组顺序是“纵轴、横轴”；左纵轴映射到 `linear.x`，右横轴
   映射到 `angular.z`。`TwistStamped.header.frame_id` 按二次开放文档设置为 `pelvis`。
+- 头部视角：遥操已启动且右摇杆纵轴先回中过一次后，按住左 Z，右摇杆上推使
+  Head Pitch 向负值移动（抬头），下推使其向正值移动（低头）；不需要 R3。按 Z
+  期间右摇杆水平转向会被屏蔽，避免调整视角时误转。控制只使用 `mode=0` 绝对
+  位置、`cur=1.0 A`，从 `/robot_state` 的真实 `name=2` 反馈起步，限制在 `-10°～+50°`、
+  最大 `0.2 rad/s`、加速度 `0.5 rad/s²`。摇杆回中、输入/反馈失联或头电机报错时停止
+  更新目标。检测到 `control_forward`、`patrol_agent`、`joystick_bridge` 等外部
+  `/head/cmd` 实际消息后，本项目暂停头部输出，静默 1 秒并再次观察摇杆回中后才恢复。
 - 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能
   在不清楚厂商语义的情况下把它擅自绑定为状态切换。
 - 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的
@@ -73,6 +82,10 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node`
 或右摇杆横向越过死区时发布。摇杆回中后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
 回 Home 或机器人退出 HBWALK 也走同一停止逻辑。
 
+头部控制只在同一次遥操武装期间启用，不改变 Head Yaw，也不改变厂家头部机械限位、
+电流、高温或碰撞保护。服务重启或新会话不会把软件累计角度当真实位置，必须重新读取
+`/robot_state` 的 Head Pitch 反馈。
+
 灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
 位置单位平滑跟随。BrainCo 状态 `0`（空闲）、`1`（运动）、`2`（接触/堵转或到限位）
 和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态
@@ -166,13 +179,12 @@ OmniSocket 进程；在此之前桥会保持最后一帧目标。未 START 或
 项目自有 `tg3-data-recorder.service` 在 Nvidia 上运行独立 `ros2 bag record`，不停止、
 重配或接管 Ubuntu 厂家 `/record_bag_node`。仅录 `config.toml` 中的明确白名单，包含
 `/robot_state`、双臂/BrainCo 双手命令与反馈、HBWALK、IMU、电源状态、完整 xTELE
-应用帧，以及头/腰 Orbbec 的 RGB、深度、内参和元数据；仍禁止改成 `-a` 或录点云。
+应用帧，以及头部 Orbbec 的 RGB、深度、内参和元数据；仍禁止改成 `-a` 或录点云。
 
 为避免 1280x720 原始 RGB+深度每台相机约 `8.3 GB/min` 的持续写盘负载，默认录制
 `color/image_raw/compressed`（JPEG）和 `depth/image_raw/compressedDepth`（16UC1
-无损 PNG）。现场单台腰部相机实测合计约 `22–25 MB/s`（约 `1.3–1.5 GB/min`，随
-场景变化）。头、腰两组相机都不是 required topics：服务在运行就由 rosbag 自动发现并
-采集，未运行不阻塞 L3 或核心 READY。manifest 对每组记录 `absent`、`healthy`、
+无损 PNG）。头部相机不是 required topic：服务在运行就由 rosbag 自动发现并
+采集，未运行不阻塞 L3 或核心 READY。manifest 记录 `absent`、`healthy`、
 `partial` 或 `low_rate`，并保存各话题消息数和图像平均频率；相机异常会产生明确质量
 警告，但不会丢弃其余机器人控制数据。静态 TF 也尽力采集，不作为启动门槛。
 
@@ -187,7 +199,7 @@ OmniSocket 进程；在此之前桥会保持最后一帧目标。未 START 或
 ```
 
 MCAP 使用 `zstd_fast`、64 MiB cache、每 300 秒分片；每条 episode 最长 30 分钟，
-加入双 RGB-D 后启动/运行最低保留 100 GiB。正常停止以 SIGINT 让 rosbag 写完
+启动/运行最低保留 100 GiB。正常停止以 SIGINT 让 rosbag 写完
 `metadata.yaml`，随后执行
 `ros2 bag info`，并确认所有 required topics 都有非零消息数；然后计算 SHA-256、写
 `manifest.json` 和 `READY`，最后才原子进入 `ready/`。Nvidia 需已安装 `python3-yaml`
@@ -209,7 +221,8 @@ sudo systemctl enable --now orbbec_head.service
 ```
 
 录制服务只加载厂家完整 `/opt/robot_tele_server/install` 消息 overlay，不加载本项目
-`ros2_py/install`；项目 overlay 仅供桥的 Python `ArmStatus` 绑定使用。两者混在同一个
+`ros2_py/install`；项目 overlay 仅供遥操桥的 Python 消息绑定使用
+（包括 `ArmStatus`/`RobotState`/`HeadCtrl`，但不是厂家完整包）。两者混在同一个
 rosbag 环境会让部分 `ros2_bridge_msgs` C++ 类型支持出现动态库符号冲突。
 
 部署两个机器人侧服务：

tg3_local_teleop/config.toml

@@ -84,8 +84,8 @@ max_duration_seconds = 1800.0
 
 # Explicit data whitelist; never replace this with `-a` on the live robot.
 # RGB is recorded through image_transport JPEG and 16UC1 depth through its
-# lossless compressedDepth PNG transport. Both RGB-D cameras are optional:
-# rosbag discovers and records either one whenever its service is running.
+# lossless compressedDepth PNG transport. Only the head RGB-D camera is in the
+# whitelist. It is optional and is discovered whenever its service is running.
 # /robot_state is the authoritative measured robot state.
 topics = [
   "/robot_state",
@@ -102,6 +102,7 @@ topics = [
   "/hric/robot/cmd_vel",
   "/hric/robot/cmd_vel_status",
   "/hric/robot/rl_state",
+  "/head/cmd",
   "/imu_data",
   "/power/board/key_status",
   "/power/board/status",
@@ -112,12 +113,6 @@ topics = [
   "/ob_camera_head/depth/image_raw/compressedDepth",
   "/ob_camera_head/depth/camera_info",
   "/ob_camera_head/depth/metadata",
-  "/ob_camera_waist/color/image_raw/compressed",
-  "/ob_camera_waist/color/camera_info",
-  "/ob_camera_waist/color/metadata",
-  "/ob_camera_waist/depth/image_raw/compressedDepth",
-  "/ob_camera_waist/depth/camera_info",
-  "/ob_camera_waist/depth/metadata",
   "/tf_static",
   "/tg3/data_collection/control",
   "/tg3/data_collection/status",
@@ -157,20 +152,6 @@ topics = [
 "/ob_camera_head/color/image_raw/compressed" = 20.0
 "/ob_camera_head/depth/image_raw/compressedDepth" = 20.0
 
-[data_collection.optional_topic_groups.waist_rgbd]
-topics = [
-  "/ob_camera_waist/color/image_raw/compressed",
-  "/ob_camera_waist/color/camera_info",
-  "/ob_camera_waist/color/metadata",
-  "/ob_camera_waist/depth/image_raw/compressedDepth",
-  "/ob_camera_waist/depth/camera_info",
-  "/ob_camera_waist/depth/metadata",
-]
-
-[data_collection.optional_topic_groups.waist_rgbd.minimum_topic_rates_hz]
-"/ob_camera_waist/color/image_raw/compressed" = 20.0
-"/ob_camera_waist/depth/image_raw/compressedDepth" = 20.0
-
 [locomotion]
 # Once Z+C has armed teleoperation and both sticks have been observed neutral,
 # left-stick vertical directly controls translation and right-stick horizontal
@@ -196,6 +177,39 @@ forward_axis_sign = 1.0
 yaw_axis_sign = -1.0
 zero_burst_frames = 10
 
+[head]
+# Camera-pitch mode: after Z+C arms teleoperation and the right-stick vertical
+# axis has been observed neutral once, hold left Z while pushing the right stick
+# up/down. While Z is held, right-stick yaw is suppressed so camera adjustment
+# cannot accidentally turn the robot. No R3 is used.
+enabled = true
+command_topic = "/head/cmd"
+state_topic = "/robot_state"
+pitch_motor_id = 2
+frame_id = "head"
+mode = 0
+label = 151
+max_current_a = 1.0
+command_speed_rad_s = 0.2
+
+# Documented mechanical range is -15..+58 deg. Keep the commissioned visual
+# working range at -10..+50 deg and always start from fresh measured feedback.
+min_pitch_rad = -0.1745329252
+max_pitch_rad = 0.872664626
+max_pitch_speed_rad_s = 0.2
+max_pitch_accel_rad_s2 = 0.5
+pitch_axis_sign = -1.0
+joystick_deadzone = 0.1
+max_integration_dt_s = 0.1
+input_timeout_s = 0.3
+feedback_timeout_s = 0.25
+
+# Existing vendor nodes advertise /head/cmd publishers. They are allowed when
+# idle, but any actual foreign message leases head control for this interval;
+# our bridge resumes only after quiet plus another neutral-stick observation.
+foreign_command_quiet_s = 1.0
+self_command_window_s = 0.25
+
 [hands]
 # TianGong 3.0 on this robot uses BrainCo Revo2 hands. Commands are published
 # only while the same physical 3-second teleoperation latch is armed.

tg3_local_teleop/head_control.py

@@ -0,0 +1,226 @@
+#!/usr/bin/env python3
+"""Pure safety state for direct TS1P right-stick head-pitch control."""
+
+from __future__ import annotations
+
+import math
+from typing import Any, Mapping
+
+
+def right_stick_vertical(data: Mapping[str, Any]) -> float | None:
+    """Return the TS1P right-stick vertical axis or fail closed."""
+
+    try:
+        joysticks = data["joystick"]
+        if not isinstance(joysticks, Mapping):
+            return None
+        right = joysticks["right"]
+        if not isinstance(right, (list, tuple)) or len(right) != 2:
+            return None
+        value = float(right[0])
+        if not math.isfinite(value) or abs(value) > 1.2:
+            return None
+        return max(-1.0, min(1.0, value))
+    except (KeyError, TypeError, ValueError):
+        return None
+
+
+def left_z_pressed(data: Mapping[str, Any]) -> bool | None:
+    """Return the physical left Z state, accepting only explicit binary data."""
+
+    try:
+        buttons = data["button"]
+        if not isinstance(buttons, Mapping):
+            return None
+        left = buttons["left"]
+        if not isinstance(left, (list, tuple)) or len(left) < 3:
+            return None
+        raw = left[2]
+        if isinstance(raw, bool):
+            return raw
+        if isinstance(raw, int) and not isinstance(raw, bool) and raw in (0, 1):
+            return bool(raw)
+        return None
+    except (KeyError, TypeError):
+        return None
+
+
+def head_pitch_command_axis(data: Mapping[str, Any]) -> float | None:
+    """Gate right-stick pitch behind left Z; malformed input fails closed."""
+
+    pressed = left_z_pressed(data)
+    axis = right_stick_vertical(data)
+    if pressed is None or axis is None:
+        return None
+    return axis if pressed else 0.0
+
+
+class HeadPitchController:
+    """Integrate a joystick velocity request into a bounded pitch target."""
+
+    def __init__(
+        self,
+        *,
+        deadzone: float,
+        max_speed_rad_s: float,
+        max_accel_rad_s2: float,
+        min_pitch_rad: float,
+        max_pitch_rad: float,
+        axis_sign: float,
+        max_dt_s: float = 0.1,
+    ) -> None:
+        values = (
+            deadzone,
+            max_speed_rad_s,
+            max_accel_rad_s2,
+            min_pitch_rad,
+            max_pitch_rad,
+            axis_sign,
+            max_dt_s,
+        )
+        if not all(math.isfinite(value) for value in values):
+            raise ValueError("head-pitch settings must be finite")
+        if not 0.0 <= deadzone < 1.0:
+            raise ValueError("head-pitch deadzone must be in [0, 1)")
+        if max_speed_rad_s <= 0.0 or max_accel_rad_s2 <= 0.0:
+            raise ValueError("head-pitch speed and acceleration must be positive")
+        if min_pitch_rad >= max_pitch_rad:
+            raise ValueError("head-pitch range is invalid")
+        if axis_sign == 0.0 or max_dt_s <= 0.0:
+            raise ValueError("head-pitch sign and maximum dt must be non-zero")
+        self.deadzone = float(deadzone)
+        self.max_speed_rad_s = float(max_speed_rad_s)
+        self.max_accel_rad_s2 = float(max_accel_rad_s2)
+        self.min_pitch_rad = float(min_pitch_rad)
+        self.max_pitch_rad = float(max_pitch_rad)
+        self.axis_sign = math.copysign(1.0, axis_sign)
+        self.max_dt_s = float(max_dt_s)
+        self.armed = False
+        self.require_neutral = True
+        self.active = False
+        self.target_rad: float | None = None
+        self.velocity_rad_s = 0.0
+        self.last_update_at: float | None = None
+        self.last_transition = "initialized"
+
+    def new_session(self) -> None:
+        self.armed = True
+        self.require_neutral = True
+        self.active = False
+        self.target_rad = None
+        self.velocity_rad_s = 0.0
+        self.last_update_at = None
+        self.last_transition = "new_session"
+
+    def disarm(self, reason: str = "disarmed") -> None:
+        self.armed = False
+        self.require_neutral = True
+        self.active = False
+        self.target_rad = None
+        self.velocity_rad_s = 0.0
+        self.last_update_at = None
+        self.last_transition = reason
+
+    def _shaped_axis(self, axis: float) -> float:
+        magnitude = abs(axis)
+        if magnitude <= self.deadzone:
+            return 0.0
+        normalized = (magnitude - self.deadzone) / (1.0 - self.deadzone)
+        return math.copysign(normalized, axis)
+
+    def update(
+        self,
+        now: float,
+        *,
+        armed: bool,
+        input_healthy: bool,
+        feedback_healthy: bool,
+        external_busy: bool,
+        axis: float | None,
+        feedback_position_rad: float | None,
+    ) -> float | None:
+        """Return one absolute pitch target, or ``None`` when output is gated."""
+
+        if not math.isfinite(now):
+            raise ValueError("head-pitch clock must be finite")
+        if not armed:
+            if self.armed:
+                self.disarm()
+            return None
+        if not self.armed:
+            self.new_session()
+
+        feedback_valid = (
+            feedback_position_rad is not None
+            and math.isfinite(float(feedback_position_rad))
+        )
+        if (
+            not input_healthy
+            or not feedback_healthy
+            or external_busy
+            or axis is None
+            or not feedback_valid
+        ):
+            self.require_neutral = True
+            self.active = False
+            self.target_rad = None
+            self.velocity_rad_s = 0.0
+            self.last_update_at = now
+            if external_busy:
+                self.last_transition = "external_command_busy"
+            elif not feedback_healthy or not feedback_valid:
+                self.last_transition = "feedback_unhealthy"
+            else:
+                self.last_transition = "input_unhealthy"
+            return None
+
+        feedback = float(feedback_position_rad)
+        shaped = self._shaped_axis(float(axis))
+        if self.require_neutral:
+            self.active = False
+            self.target_rad = feedback
+            self.velocity_rad_s = 0.0
+            self.last_update_at = now
+            if shaped == 0.0:
+                self.require_neutral = False
+                self.last_transition = "neutral_ready"
+            return None
+
+        if shaped == 0.0:
+            self.active = False
+            self.target_rad = feedback
+            self.velocity_rad_s = 0.0
+            self.last_update_at = now
+            self.last_transition = "neutral"
+            return None
+
+        if self.target_rad is None:
+            self.target_rad = feedback
+        dt = 0.0
+        if self.last_update_at is not None:
+            dt = min(self.max_dt_s, max(0.0, now - self.last_update_at))
+        self.last_update_at = now
+        desired_velocity = shaped * self.axis_sign * self.max_speed_rad_s
+        max_velocity_step = self.max_accel_rad_s2 * dt
+        self.velocity_rad_s += max(
+            -max_velocity_step,
+            min(max_velocity_step, desired_velocity - self.velocity_rad_s),
+        )
+        self.target_rad = max(
+            self.min_pitch_rad,
+            min(
+                self.max_pitch_rad,
+                self.target_rad + self.velocity_rad_s * dt,
+            ),
+        )
+        self.active = True
+        self.last_transition = "active"
+        return self.target_rad
+
+
+__all__ = [
+    "HeadPitchController",
+    "head_pitch_command_axis",
+    "left_z_pressed",
+    "right_stick_vertical",
+]

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt

@@ -7,7 +7,14 @@ find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/MotorStatus.msg"
+  "msg/MotorCtrl.msg"
   "msg/ArmStatus.msg"
+  "msg/HeadCtrl.msg"
+  "msg/HeadStatus.msg"
+  "msg/WaistStatus.msg"
+  "msg/LegStatus.msg"
+  "msg/ImuStatus.msg"
+  "msg/RobotState.msg"
   DEPENDENCIES std_msgs
 )
 

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadCtrl.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+uint8 mode
+uint8 label
+uint8 reserved
+ros2_bridge_msgs/MotorCtrl[] ctrl

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadStatus.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+uint8 label
+uint8 reserved
+ros2_bridge_msgs/MotorStatus[] status

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ImuStatus.msg

@@ -0,0 +1,20 @@
+std_msgs/Header header
+uint8 label
+uint8 reserved
+float64 qx
+float64 qy
+float64 qz
+float64 qw
+float64 roll
+float64 pitch
+float64 yaw
+float64 wx
+float64 wy
+float64 wz
+float64 ax
+float64 ay
+float64 az
+float64[9] orientation_covariance
+float64[9] angular_velocity_covariance
+float64[9] linear_acceleration_covariance
+uint32 error

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/LegStatus.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+uint8 label
+uint8 reserved
+ros2_bridge_msgs/MotorStatus[] status

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorCtrl.msg

@@ -0,0 +1,8 @@
+uint16 name
+float64 kp
+float64 kd
+float64 pos
+float64 spd
+float64 tor
+float64 cur
+string joint_ids

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/RobotState.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+ros2_bridge_msgs/HeadStatus head
+ros2_bridge_msgs/WaistStatus waist
+ros2_bridge_msgs/LegStatus leg
+ros2_bridge_msgs/ArmStatus arm
+ros2_bridge_msgs/ImuStatus imu

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/WaistStatus.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+uint8 label
+uint8 reserved
+ros2_bridge_msgs/MotorStatus[] status

tg3_local_teleop/test_head_control.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import unittest
+
+from head_control import (
+    HeadPitchController,
+    head_pitch_command_axis,
+    left_z_pressed,
+    right_stick_vertical,
+)
+
+
+def controller() -> HeadPitchController:
+    return HeadPitchController(
+        deadzone=0.1,
+        max_speed_rad_s=0.2,
+        max_accel_rad_s2=0.5,
+        min_pitch_rad=-0.1745329252,
+        max_pitch_rad=0.872664626,
+        axis_sign=-1.0,
+        max_dt_s=0.1,
+    )
+
+
+class RightStickParserTest(unittest.TestCase):
+    def test_accepts_only_finite_two_axis_right_stick(self) -> None:
+        self.assertEqual(
+            right_stick_vertical({"joystick": {"right": [0.5, -0.2]}}),
+            0.5,
+        )
+        self.assertIsNone(right_stick_vertical({"joystick": {"right": [0.5]}}))
+        self.assertIsNone(
+            right_stick_vertical({"joystick": {"right": [float("nan"), 0.0]}})
+        )
+        self.assertIsNone(right_stick_vertical({"joystick": {"right": [2.0, 0.0]}}))
+
+    def test_left_z_strictly_gates_head_pitch_axis(self) -> None:
+        released = {
+            "button": {"left": [0, 0, 0]},
+            "joystick": {"right": [0.8, 0.1]},
+        }
+        pressed = {
+            "button": {"left": [0, 0, 1]},
+            "joystick": {"right": [0.8, 0.1]},
+        }
+        self.assertFalse(left_z_pressed(released))
+        self.assertEqual(head_pitch_command_axis(released), 0.0)
+        self.assertTrue(left_z_pressed(pressed))
+        self.assertEqual(head_pitch_command_axis(pressed), 0.8)
+        self.assertIsNone(
+            head_pitch_command_axis(
+                {
+                    "button": {"left": [0, 0, "pressed"]},
+                    "joystick": {"right": [0.8, 0.1]},
+                }
+            )
+        )
+
+
+class HeadPitchControllerTest(unittest.TestCase):
+    def test_new_session_requires_neutral_before_direct_motion(self) -> None:
+        head = controller()
+        head.new_session()
+        self.assertIsNone(
+            head.update(
+                0.0,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=1.0,
+                feedback_position_rad=0.1,
+            )
+        )
+        self.assertTrue(head.require_neutral)
+        head.update(
+            0.1,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=0.1,
+        )
+        self.assertFalse(head.require_neutral)
+        target = head.update(
+            0.2,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=1.0,
+            feedback_position_rad=0.1,
+        )
+        self.assertIsNotNone(target)
+        self.assertLess(target, 0.1)  # stick up -> negative pitch -> view up
+
+    def test_center_stops_and_resynchronizes_to_real_feedback(self) -> None:
+        head = controller()
+        head.new_session()
+        head.update(
+            0.0,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=0.2,
+        )
+        head.update(
+            0.1,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=-1.0,
+            feedback_position_rad=0.2,
+        )
+        self.assertTrue(head.active)
+        self.assertIsNone(
+            head.update(
+                0.2,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=0.0,
+                feedback_position_rad=0.25,
+            )
+        )
+        self.assertFalse(head.active)
+        self.assertEqual(head.target_rad, 0.25)
+        self.assertEqual(head.velocity_rad_s, 0.0)
+
+    def test_feedback_gap_or_external_command_fails_closed(self) -> None:
+        for feedback_healthy, external_busy in ((False, False), (True, True)):
+            head = controller()
+            head.new_session()
+            head.update(
+                0.0,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=0.0,
+                feedback_position_rad=0.0,
+            )
+            self.assertIsNone(
+                head.update(
+                    0.1,
+                    armed=True,
+                    input_healthy=True,
+                    feedback_healthy=feedback_healthy,
+                    external_busy=external_busy,
+                    axis=1.0,
+                    feedback_position_rad=0.0,
+                )
+            )
+            self.assertTrue(head.require_neutral)
+            self.assertFalse(head.active)
+
+    def test_target_is_clamped_to_software_working_range(self) -> None:
+        head = controller()
+        head.new_session()
+        head.update(
+            0.0,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=-0.17,
+        )
+        target = None
+        for index in range(1, 200):
+            target = head.update(
+                index * 0.1,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=1.0,
+                feedback_position_rad=-0.17,
+            )
+        self.assertEqual(target, head.min_pitch_rad)
+
+
+if __name__ == "__main__":
+    unittest.main()

tg3_local_teleop/test_idle_session_refresh.py

@@ -31,6 +31,9 @@ for package, message_name in (
     ("diagnostic_msgs.msg", "DiagnosticStatus"),
     ("geometry_msgs.msg", "TwistStamped"),
     ("ros2_bridge_msgs.msg", "ArmStatus"),
+    ("ros2_bridge_msgs.msg", "HeadCtrl"),
+    ("ros2_bridge_msgs.msg", "MotorCtrl"),
+    ("ros2_bridge_msgs.msg", "RobotState"),
     ("sensor_msgs.msg", "JointState"),
     ("std_srvs.srv", "Trigger"),
 ):

tg3_local_teleop/test_session_gate.py

@@ -28,7 +28,7 @@ for package, names in {
     "brainco_hand_msgs.msg": ("MotorStatus", "SetMotorMulti"),
     "diagnostic_msgs.msg": ("DiagnosticStatus",),
     "geometry_msgs.msg": ("TwistStamped",),
-    "ros2_bridge_msgs.msg": ("ArmStatus",),
+    "ros2_bridge_msgs.msg": ("ArmStatus", "HeadCtrl", "MotorCtrl", "RobotState"),
     "sensor_msgs.msg": ("JointState",),
     "std_msgs.msg": ("String",),
     "std_srvs.srv": ("Trigger",),
@@ -83,6 +83,7 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge.hands_enabled = False
         bridge.right_point_gesture_enabled = False
         bridge.right_a_pose_enabled = False
+        bridge.head_enabled = False
         bridge.robot_arm_positions = [0.0] * 14
         bridge.last_command = None
         bridge.last_publish_at = 0.0
@@ -307,6 +308,22 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge._tick_locomotion(1.3, turning)
         self.assertEqual(published[-1], (0.0, -0.8))
 
+        head_mode_with_sideways_error = ArmSnapshot(
+            {
+                "button": {
+                    "left": [False, False, True],
+                    "right": [False, False, False],
+                },
+                "joystick": {
+                    "left": [0.0, 0.0],
+                    "right": [0.8, 1.0],
+                },
+            },
+            received_at=1.35,
+        )
+        bridge._tick_locomotion(1.35, head_mode_with_sideways_error)
+        self.assertEqual(published[-1], (0.0, 0.0))
+
         old_wrong_binding = ArmSnapshot(
             {
                 "button": {
@@ -351,11 +368,17 @@ class RobotSessionGateTest(unittest.TestCase):
 
         bridge._publish_walk = MethodType(publish, bridge)
         deflected = ArmSnapshot(
-            {"joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]}},
+            {
+                "button": {"left": [0, 0, 0]},
+                "joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]},
+            },
             received_at=2.0,
         )
         neutral = ArmSnapshot(
-            {"joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]}},
+            {
+                "button": {"left": [0, 0, 0]},
+                "joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]},
+            },
             received_at=2.1,
         )
         bridge._tick_locomotion(2.0, deflected)

tg3_local_teleop/tg3_local_teleop.py

@@ -27,7 +27,7 @@ from brainco_hand_msgs.msg import MotorStatus, SetMotorMulti
 from diagnostic_msgs.msg import DiagnosticStatus
 from geometry_msgs.msg import TwistStamped
 from rclpy.node import Node
-from ros2_bridge_msgs.msg import ArmStatus
+from ros2_bridge_msgs.msg import ArmStatus, HeadCtrl, MotorCtrl, RobotState
 from sensor_msgs.msg import JointState
 from std_msgs.msg import String
 from std_srvs.srv import Trigger
@@ -41,6 +41,7 @@ from gesture_toggle import (
     right_b_pressed,
     select_hand_target,
 )
+from head_control import HeadPitchController, head_pitch_command_axis, left_z_pressed
 
 
 JOINT_NAMES = [
@@ -453,6 +454,25 @@ class LocalTeleopBridge(Node):
         self.locomotion_enabled = bool(
             self.locomotion_cfg.get("enabled", False)
         )
+        self.head_cfg = config.get("head", {})
+        self.head_enabled = bool(self.head_cfg.get("enabled", False))
+        self.head_pitch_controller = HeadPitchController(
+            deadzone=float(self.head_cfg.get("joystick_deadzone", 0.1)),
+            max_speed_rad_s=float(
+                self.head_cfg.get("max_pitch_speed_rad_s", 0.2)
+            ),
+            max_accel_rad_s2=float(
+                self.head_cfg.get("max_pitch_accel_rad_s2", 0.5)
+            ),
+            min_pitch_rad=float(
+                self.head_cfg.get("min_pitch_rad", -0.1745329252)
+            ),
+            max_pitch_rad=float(
+                self.head_cfg.get("max_pitch_rad", 0.872664626)
+            ),
+            axis_sign=float(self.head_cfg.get("pitch_axis_sign", -1.0)),
+            max_dt_s=float(self.head_cfg.get("max_integration_dt_s", 0.1)),
+        )
         self.data_collection_cfg = config.get("data_collection", {})
         self.data_collection_enabled = bool(
             self.data_collection_cfg.get("enabled", False)
@@ -504,6 +524,17 @@ class LocalTeleopBridge(Node):
         self.walk_zero_frames_remaining = 0
         self.walk_require_neutral = True
         self.last_walk_publish_at = 0.0
+        self.head_publisher = None
+        self.robot_head_positions: dict[int, float] = {}
+        self.robot_head_errors: dict[int, int] = {}
+        self.robot_head_at = 0.0
+        self.expected_head_messages: list[tuple[tuple[Any, ...], float]] = []
+        self.foreign_head_command_at = 0.0
+        self.foreign_head_command_count = 0
+        self.head_publish_count = 0
+        self.last_head_publish_at = 0.0
+        self.head_output_reasons: list[str] = []
+        self.head_z_pressed: bool | None = None
 
         self.publisher = self.create_publisher(JointState, ros_cfg["command_topic"], 10)
         self.walk_publisher = None
@@ -511,6 +542,22 @@ class LocalTeleopBridge(Node):
             self.walk_publisher = self.create_publisher(
                 TwistStamped, self.locomotion_cfg["command_topic"], 10
             )
+        if self.head_enabled:
+            self.head_publisher = self.create_publisher(
+                HeadCtrl, self.head_cfg["command_topic"], 10
+            )
+            self.create_subscription(
+                RobotState,
+                self.head_cfg["state_topic"],
+                self._on_robot_state,
+                10,
+            )
+            self.create_subscription(
+                HeadCtrl,
+                self.head_cfg["command_topic"],
+                self._on_head_command,
+                10,
+            )
         self.create_subscription(
             DiagnosticStatus, ros_cfg["rl_state_topic"], self._on_rl_state, 10
         )
@@ -597,6 +644,7 @@ class LocalTeleopBridge(Node):
             f"local bridge started in {mode}; source={self.source.description}; "
             f"target={ros_cfg['command_topic']}; brainco_hands={self.hands_enabled}; "
             f"locomotion={self.locomotion_enabled}; "
+            f"head_pitch={self.head_enabled}; "
             f"data_collection={self.data_collection_enabled}"
         )
 
@@ -620,6 +668,20 @@ class LocalTeleopBridge(Node):
         self.robot_arm_errors = [int(by_id[motor_id].error) for motor_id in MOTOR_IDS]
         self.robot_arm_at = time.monotonic()
 
+    def _on_robot_state(self, msg: RobotState) -> None:
+        by_id = {int(motor.name): motor for motor in msg.head.status}
+        pitch_id = int(self.head_cfg.get("pitch_motor_id", 2))
+        if pitch_id not in by_id:
+            return
+        positions = {motor_id: float(motor.pos) for motor_id, motor in by_id.items()}
+        if not all(math.isfinite(value) for value in positions.values()):
+            return
+        self.robot_head_positions = positions
+        self.robot_head_errors = {
+            motor_id: int(motor.error) for motor_id, motor in by_id.items()
+        }
+        self.robot_head_at = time.monotonic()
+
     def _on_hand_status(self, side: str, msg: MotorStatus) -> None:
         positions = [int(value) for value in msg.positions]
         states = [int(value) for value in msg.states]
@@ -899,6 +961,45 @@ class LocalTeleopBridge(Node):
                 "foreign BrainCo hand command source detected", success=False
             )
 
+    @staticmethod
+    def _head_message_signature(msg: HeadCtrl) -> tuple[Any, ...]:
+        return (
+            int(msg.mode),
+            int(msg.label),
+            int(msg.reserved),
+            tuple(
+                (
+                    int(motor.name),
+                    float(motor.kp),
+                    float(motor.kd),
+                    float(motor.pos),
+                    float(motor.spd),
+                    float(motor.tor),
+                    float(motor.cur),
+                    str(motor.joint_ids),
+                )
+                for motor in msg.ctrl
+            ),
+        )
+
+    def _on_head_command(self, msg: HeadCtrl) -> None:
+        now = time.monotonic()
+        window = float(self.head_cfg.get("self_command_window_s", 0.25))
+        self.expected_head_messages = [
+            expected
+            for expected in self.expected_head_messages
+            if now - expected[1] <= window
+        ]
+        signature = self._head_message_signature(msg)
+        if any(signature == expected[0] for expected in self.expected_head_messages):
+            return
+        self.foreign_head_command_at = now
+        self.foreign_head_command_count += 1
+        if self.head_pitch_controller.active:
+            self.get_logger().warning(
+                "external /head/cmd detected; local head-pitch output paused"
+            )
+
     def _on_command_topic(self, msg: JointState) -> None:
         if msg.header.frame_id != self.FRAME_ID and len(msg.position) in (14, 16):
             if not self.foreign_source_seen:
@@ -1001,6 +1102,8 @@ class LocalTeleopBridge(Node):
         # decision, so a matching STOP or safety teardown always wins over an
         # L3 press observed in the same control tick.
         self._tick_data_collection(now, sample)
+        if self.head_enabled:
+            self._tick_head(now, sample)
 
         if self.right_point_gesture_enabled:
             gesture_toggled = self.right_point_gesture.update(
@@ -1288,6 +1391,8 @@ class LocalTeleopBridge(Node):
             self.right_point_gesture.new_session()
         if self.right_a_pose_enabled:
             self.right_a_pose.new_session()
+        if self.head_enabled:
+            self.head_pitch_controller.new_session()
         if getattr(self, "data_collection_enabled", False):
             self.data_capture_session_id = session_id
             self.data_collection_gate.new_session()
@@ -1360,6 +1465,8 @@ class LocalTeleopBridge(Node):
             self.right_point_gesture.new_session()
         if self.right_a_pose_enabled:
             self.right_a_pose.new_session()
+        if self.head_enabled:
+            self.head_pitch_controller.new_session()
         if getattr(self, "data_collection_enabled", False):
             self.data_capture_session_id = "direct_" + uuid.uuid4().hex
             self.data_collection_gate.new_session()
@@ -1398,6 +1505,8 @@ class LocalTeleopBridge(Node):
             self.right_point_gesture.disarm(reason)
         if self.right_a_pose_enabled:
             self.right_a_pose.disarm(reason)
+        if self.head_enabled:
+            self.head_pitch_controller.disarm(reason)
         self.armed = False
         self.hand_output_ready = False
         self.runtime_hand_output_reasons = []
@@ -1438,6 +1547,9 @@ class LocalTeleopBridge(Node):
             # convention requested for this installation.
             raw_forward = float(left_joystick[0])
             raw_yaw = float(right_joystick[1])
+            head_mode = left_z_pressed(sample.data)
+            if head_mode is None:
+                raise ValueError("left Z state is malformed")
             deadzone = float(cfg["joystick_deadzone"])
             forward_expo = float(cfg["joystick_expo"])
             yaw_expo = float(cfg.get("yaw_joystick_expo", forward_expo))
@@ -1447,6 +1559,11 @@ class LocalTeleopBridge(Node):
             shaped_yaw = self._shape_joystick_axis(
                 raw_yaw, deadzone, yaw_expo
             )
+            # Left Z reserves the entire right stick for head-pitch mode.
+            # Suppress yaw so a small sideways component while looking up or
+            # down cannot turn the robot.
+            if head_mode:
+                shaped_yaw = 0.0
             signed_forward = shaped_forward * float(
                 cfg.get("forward_axis_sign", 1.0)
             )
@@ -1533,6 +1650,96 @@ class LocalTeleopBridge(Node):
         self.last_walk_publish_at = now
         self.walk_publish_count += 1
 
+    def _tick_head(self, now: float, sample: ArmSnapshot | None) -> None:
+        pitch_id = int(self.head_cfg.get("pitch_motor_id", 2))
+        pitch_position = self.robot_head_positions.get(pitch_id)
+        feedback_timeout = float(self.head_cfg.get("feedback_timeout_s", 0.25))
+        feedback_age = (
+            math.inf if self.robot_head_at == 0.0 else now - self.robot_head_at
+        )
+        feedback_healthy = (
+            pitch_position is not None
+            and math.isfinite(float(pitch_position))
+            and feedback_age <= feedback_timeout
+            and int(self.robot_head_errors.get(pitch_id, -1)) == 0
+        )
+        input_timeout = float(self.head_cfg.get("input_timeout_s", 0.3))
+        input_healthy = (
+            self.armed
+            and sample is not None
+            and now - sample.received_at <= input_timeout
+        )
+        self.head_z_pressed = (
+            None if sample is None else left_z_pressed(sample.data)
+        )
+        axis = None if sample is None else head_pitch_command_axis(sample.data)
+        quiet_s = float(self.head_cfg.get("foreign_command_quiet_s", 1.0))
+        foreign_age = (
+            math.inf
+            if self.foreign_head_command_at == 0.0
+            else now - self.foreign_head_command_at
+        )
+        external_busy = foreign_age <= quiet_s
+
+        reasons: list[str] = []
+        if not self.armed:
+            reasons.append("teleoperation is not armed")
+        if not input_healthy:
+            reasons.append("right-stick input is unavailable/stale")
+        if axis is None:
+            reasons.append("left Z or right-stick vertical input is malformed")
+        elif not self.head_z_pressed:
+            reasons.append("left Z is not held; head-pitch control is idle")
+        if not feedback_healthy:
+            reasons.append("head pitch feedback is unavailable/stale or in error")
+        if external_busy:
+            reasons.append("external /head/cmd publisher has an active lease")
+        self.head_output_reasons = reasons
+
+        target = self.head_pitch_controller.update(
+            now,
+            armed=self.armed,
+            input_healthy=input_healthy,
+            feedback_healthy=feedback_healthy,
+            external_busy=external_busy,
+            axis=axis,
+            feedback_position_rad=pitch_position,
+        )
+        if target is not None:
+            self._publish_head_pitch(target, now)
+
+    def _publish_head_pitch(self, target_rad: float, now: float) -> None:
+        if not self.allow_publish or self.head_publisher is None:
+            return
+        motor = MotorCtrl()
+        motor.name = int(self.head_cfg.get("pitch_motor_id", 2))
+        motor.kp = 0.0
+        motor.kd = 0.0
+        motor.pos = float(target_rad)
+        motor.spd = float(self.head_cfg.get("command_speed_rad_s", 0.2))
+        motor.tor = 0.0
+        motor.cur = float(self.head_cfg.get("max_current_a", 1.0))
+        motor.joint_ids = ""
+
+        msg = HeadCtrl()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = str(self.head_cfg.get("frame_id", "head"))
+        msg.mode = int(self.head_cfg.get("mode", 0))
+        msg.label = int(self.head_cfg.get("label", 151))
+        msg.reserved = 0
+        msg.ctrl = [motor]
+        signature = self._head_message_signature(msg)
+        window = float(self.head_cfg.get("self_command_window_s", 0.25))
+        self.expected_head_messages = [
+            expected
+            for expected in self.expected_head_messages
+            if now - expected[1] <= window
+        ][-64:]
+        self.expected_head_messages.append((signature, now))
+        self.head_publisher.publish(msg)
+        self.head_publish_count += 1
+        self.last_head_publish_at = now
+
     def _home_start_reasons(
         self,
         now: float,
@@ -2111,6 +2318,42 @@ class LocalTeleopBridge(Node):
             "last_locomotion_publish_age_s": None
             if self.last_walk_publish_at == 0.0
             else round(now - self.last_walk_publish_at, 4),
+            "head_pitch_enabled": self.head_enabled,
+            "head_pitch_binding": "left_Z + right_stick_vertical after neutral",
+            "head_pitch_left_z_pressed": self.head_z_pressed,
+            "head_pitch_active": self.head_pitch_controller.active,
+            "head_pitch_state": self.head_pitch_controller.last_transition,
+            "head_pitch_requires_neutral": (
+                self.head_pitch_controller.require_neutral
+            ),
+            "head_pitch_target_rad": self.head_pitch_controller.target_rad,
+            "head_pitch_velocity_rad_s": (
+                self.head_pitch_controller.velocity_rad_s
+            ),
+            "head_pitch_feedback_rad": self.robot_head_positions.get(
+                int(self.head_cfg.get("pitch_motor_id", 2))
+            ),
+            "head_pitch_feedback_error": self.robot_head_errors.get(
+                int(self.head_cfg.get("pitch_motor_id", 2))
+            ),
+            "head_pitch_feedback_age_s": None
+            if self.robot_head_at == 0.0
+            else round(now - self.robot_head_at, 4),
+            "head_pitch_output_reasons": self.head_output_reasons,
+            "head_pitch_limits_rad": {
+                "up": self.head_pitch_controller.min_pitch_rad,
+                "down": self.head_pitch_controller.max_pitch_rad,
+                "max_speed": self.head_pitch_controller.max_speed_rad_s,
+                "max_accel": self.head_pitch_controller.max_accel_rad_s2,
+            },
+            "head_pitch_foreign_command_age_s": None
+            if self.foreign_head_command_at == 0.0
+            else round(now - self.foreign_head_command_at, 4),
+            "head_pitch_foreign_command_count": self.foreign_head_command_count,
+            "head_pitch_publish_count": self.head_publish_count,
+            "last_head_pitch_publish_age_s": None
+            if self.last_head_publish_at == 0.0
+            else round(now - self.last_head_publish_at, 4),
             "unsupported_binding": (
                 "right_C + right_A (C+A) is not registered by xTELE 0.1.2"
             ),

verify.sh

@@ -14,7 +14,8 @@ for executable in \
   "$repo_dir/tg3_local_teleop/wait_ros_ready.sh" \
   "$repo_dir/tg3_local_teleop/home.sh" \
   "$repo_dir/tg3_local_teleop/status.sh" \
-  "$repo_dir/tg3_data_collection/data_get_sync.py"; do
+  "$repo_dir/tg3_data_collection/data_get_sync.py" \
+  "$repo_dir/tg3_data_collection/decode_joint_data.py"; do
   if [[ ! -x "$executable" ]]; then
     echo "Required executable bit is missing: $executable" >&2
     exit 1
@@ -25,14 +26,17 @@ python3 -m py_compile \
   "$repo_dir/tg3_omnisocket_transport/omnisocket_xtele_sender.py" \
   "$repo_dir/tg3_local_teleop/tg3_local_teleop.py" \
   "$repo_dir/tg3_local_teleop/gesture_toggle.py" \
+  "$repo_dir/tg3_local_teleop/head_control.py" \
   "$repo_dir/tg3_local_teleop/data_collection.py" \
   "$repo_dir/tg3_local_teleop/data_recorder_protocol.py" \
   "$repo_dir/tg3_local_teleop/data_recorder_node.py" \
   "$repo_dir/tg3_local_teleop/delete_ready_episode.py" \
-  "$repo_dir/tg3_data_collection/data_get_sync.py"
+  "$repo_dir/tg3_data_collection/data_get_sync.py" \
+  "$repo_dir/tg3_data_collection/decode_joint_data.py"
 python3 "$repo_dir/tg3_omnisocket_transport/test_session_gate.py"
 python3 "$repo_dir/tg3_local_teleop/test_session_gate.py"
 python3 "$repo_dir/tg3_local_teleop/test_gesture_toggle.py"
+python3 "$repo_dir/tg3_local_teleop/test_head_control.py"
 python3 "$repo_dir/tg3_local_teleop/test_idle_session_refresh.py"
 python3 "$repo_dir/tg3_local_teleop/test_data_collection.py"
 python3 "$repo_dir/tg3_local_teleop/test_data_recorder_protocol.py"
@@ -55,34 +59,34 @@ head_rgbd = {
     "/ob_camera_head/depth/camera_info",
     "/ob_camera_head/depth/metadata",
 }
-waist_rgbd = {
-    "/ob_camera_waist/color/image_raw/compressed",
-    "/ob_camera_waist/color/camera_info",
-    "/ob_camera_waist/color/metadata",
-    "/ob_camera_waist/depth/image_raw/compressedDepth",
-    "/ob_camera_waist/depth/camera_info",
-    "/ob_camera_waist/depth/metadata",
-}
 assert head_rgbd <= topics, "head RGB-D topics must stay in the recording whitelist"
-assert waist_rgbd <= topics, "waist RGB-D topics must stay in the recording whitelist"
 assert head_rgbd.isdisjoint(required), "head RGB-D must not block recording"
-assert waist_rgbd.isdisjoint(required), "waist RGB-D must not block recording"
+assert not any(topic.startswith("/ob_camera_waist/") for topic in topics), (
+    "waist camera topics must stay out of the recording whitelist"
+)
 assert "/tf_static" in topics and "/tf_static" not in required
 assert section.get("minimum_topic_rates_hz", {}) == {}
 groups = section["optional_topic_groups"]
-assert set(groups) == {"head_rgbd", "waist_rgbd"}
+assert set(groups) == {"head_rgbd"}
 assert set(groups["head_rgbd"]["topics"]) == head_rgbd
-assert set(groups["waist_rgbd"]["topics"]) == waist_rgbd
 assert groups["head_rgbd"]["minimum_topic_rates_hz"] == {
     "/ob_camera_head/color/image_raw/compressed": 20.0,
     "/ob_camera_head/depth/image_raw/compressedDepth": 20.0,
 }
-assert groups["waist_rgbd"]["minimum_topic_rates_hz"] == {
-    "/ob_camera_waist/color/image_raw/compressed": 20.0,
-    "/ob_camera_waist/depth/image_raw/compressedDepth": 20.0,
-}
 assert section["minimum_free_gib"] >= 100.0
 assert section["retain_failed_episodes"] is False
+head = document["head"]
+assert head["enabled"] is True
+assert head["command_topic"] == "/head/cmd"
+assert head["state_topic"] == "/robot_state"
+assert head["pitch_motor_id"] == 2
+assert head["mode"] == 0
+assert 0.0 < head["max_current_a"] <= 1.0
+assert -0.2617994 <= head["min_pitch_rad"] < head["max_pitch_rad"] <= 1.012291
+assert 0.0 < head["max_pitch_speed_rad_s"] <= 0.2
+assert 0.0 < head["max_pitch_accel_rad_s2"] <= 0.5
+assert 0.0 <= head["joystick_deadzone"] < 1.0
+assert "/head/cmd" in topics and "/head/cmd" not in required
 print("config.toml parse passed")
 PY