状态机与话题¶

本技术栈的有限状态机在 arms_ros2_control。共享原语在 libraries/arms_controller_common/（StateHome、StateHold、StateMoveJ、FSMCommandPublisher）。笛卡尔末端话题由 arms_target_manager（PoseBasedReferenceManager）持有。

权威来源

/fsm_command 是 std_msgs/Int32，不是 String，也不是 stand / walk / arm_teleop 这类轮式臂字符串。

/fsm_state 同样是 std_msgs/Int32（锁存）——见 ros2_robot_interface API_REFERENCE 与 panthera-ht README。

WBC 栈上的 /mode_command 是 std_msgs/String（ros2_robot_interface 的 send_mode_command：BODY_TRACKING、ARMS_COUPLED、BASE_LOCK 等）。它不是 FSM 字符串，也不能代替 /fsm_command。

笛卡尔末端目标是 /left_target、/left_target/stamped、/left_target/twist、/left_target/relative（以及右侧 / 双臂对应话题），不是全栈统一的 /target_pose。各控制器额外话题见对应页面与软件包 README。Python 对照：ros2_robot_interface。

`/fsm_command`（`std_msgs/Int32`）¶

FSMCommandPublisher 向 /fsm_command 发布 std_msgs/Int32。约定整数：

取值	常量 / 标签	作用
`1`	`FSM_HOME` / HOME	运动到预设 HOME 构型
`2`	`FSM_HOLD` / HOLD	保持；安全停止。从 OCS2 / MOVEJ 回来的典型路径
`3`	`FSM_OCS2` / OCS2	手臂 / WBC MPC（笛卡尔目标）。在独立运行的 `basic_joint_controller` 上，`3` 是 MOVEJ 的遗留别名
`4`	`FSM_MOVEJ` / MOVEJ	直接关节位置模式（混合 OCS2/WBC 栈上的规范 MOVEJ）
`5`	`FSM_COMPLIANCE`	ros2_robot_interface 中的柔顺 / FT 力控模式（`enter_compliance()`）。必须经 HOLD 转入
`100`	`switch_command_base` (default)	已在 HOME 时：循环到下一组 `home_*` 构型（basic_joint_controller）
`101`, `102`, …	`switch_command_base + 1 + index`	在 HOME 中：选择构型索引 `0`、`1`、…

具体转换取决于正在运行的控制器：

控制器	状态	命令
Basic Joint Controller	HOME / HOLD / MOVEJ	README：`1` HOME、`2` HOLD、`4` MOVEJ（规范值）；本控制器独立运行时 `3` 是遗留 MOVEJ 别名，在混合 OCS2/WBC 栈上表示 OCS2
OCS2 Arm Controller	HOME / OCS2 / HOLD	README 整数：`1` HOME、`2` HOLD、`3` OCS2。操作员和 `ros2_robot_interface` 发到 `/fsm_command`。从 HOLD 起步；OCS2 只能回到 HOLD。共用的 `FSMCommandPublisher` 还定义了 `4` MOVEJ
OCS2 WBC Controller	全身栈	私有软件包；额外状态见开通后的该 README。公开 Python 仍使用同一套 `/fsm_command` 整数以及 `/mode_command` 字符串

混合栈上，头部 / 分体腰部的 basic_joint_controller 把 3 和 4 都当作 MOVEJ；手臂 / WBC 控制器则把 3 当作 OCS2、4 当作 MOVEJ。

ocs2_arm_controller README 仍把 1/2/3 这些整数写在 /control_input 上。那不是全栈 FSM 话题。arms_target_manager 上的手柄 control_input（arms_ros2_control_msgs/Inputs）是另一种消息：按比例映射到 /left_target/twist / /right_target/twist。

ros2 topic pub --once /fsm_command std_msgs/msg/Int32 "data: 2"   # HOLD

分体 vs 全身 launch（以及 Lift2S quick_start 的分体 / 全身菜单）见分体控制 vs 全身控制。遥操作是另一套接口，不是额外 FSM 状态——同构遥操作。

状态与指令话题¶

下列名称来自公开的 ros2_robot_interface 配置与 API_REFERENCE 的订阅 / 发布，以及公开控制器 README。当分体与 WBC 关节前缀同时存在时，connect() 会改写前缀（WBC 优先）。

状态¶

话题	类型	典型用途
`/joint_states`	`sensor_msgs/JointState`	全部关节（手臂、头部、腰部、夹爪）
`/left_current_pose`, `/right_current_pose`	`geometry_msgs/PoseStamped`	当前末端位姿（`frame_id` 是 base，或 WBC 轮式 world——不是末端 link）
`/left_current_target`, `/right_current_target`	`geometry_msgs/PoseStamped`	当前笛卡尔指令回写；到位检查（`check_arrival()`）
`/body_current_pose`	`geometry_msgs/PoseStamped`	WBC 当前身体位姿
`/body_current_target`	`geometry_msgs/PoseStamped`	WBC 身体指令回写（`body_target_enabled`）
`/head_current_target`	`geometry_msgs/PoseStamped`	WBC 头部 6D 最终目标回写；`arms_target_manager` 只订阅
`/ocs2_wbc_controller/current_state`	`arms_ros2_control_msgs/WbcCurrentState`	WBC 约束快照；用来确认 `/mode_command`

ros2 topic echo /joint_states
ros2 topic echo /left_current_pose
ros2 topic echo /left_current_target

笛卡尔手臂目标（`arms_target_manager`）¶

左臂；右臂对称（/right_target…）。摘自 arms_target_manager README 的 PoseBasedReferenceManager 表。

不带 stamp 的 /left_target 用一个位姿替换当前末端目标（VR / 高频率遥操作）。带 stamp 的 /left_target/stamped 会经 TF 变换到控制器坐标系并插值成 MoveL 序列（视觉抓取 / RViz 绝对）。未 stamp 的远距离跳变可能产生冲击。

话题	类型	典型用途
`/left_target`, `/right_target`	`geometry_msgs/Pose`	`base_frame` 下立即采用的绝对位姿
`/left_target/stamped`, `/right_target/stamped`	`geometry_msgs/PoseStamped`	绝对 MoveL；非 base 的 `frame_id` 会经 TF 变到 base
`/left_target/twist`, `/right_target/twist`	`geometry_msgs/Twist`	速度流（m/s、rad/s），在 base 下；锁存并以 `dt` 积分。手柄 `control_input` 映射到这里
`/left_target/relative`, `/right_target/relative`	`geometry_msgs/TwistStamped`	一次性相对增量（m、rad）再走 MoveL；`header.frame_id` 是增量坐标系（base / 末端 / 其它 TF）
`/dual_target/stamped`	`nav_msgs/Path`	双臂；Path 位姿为 `[left, right]`（WBC 上可选第三项 body）。MOVEJ 双臂发送若有三个 pose，会忽略 body
`/target_path`	`nav_msgs/Path`	双臂路点路径（Python API 标明该话题已弃用，改用 `execute_path` 服务）

relative 使用 TwistStamped：twist 是位移增量，不是速度。非 base 的 frame_id 会先把 linear / angular 旋到 base 再合成。twist 话题仍是裸 Twist（速度语义，仅基座）。angular.{x,y,z} 对应 roll / pitch / yaw（合成：R' = RΔ(yaw)·RΔ(pitch)·RΔ(roll)·R）。

身体目标（全身 OCS2）¶

全身控制（WBC）可用性

/body_target*、/head_target*、/mode_command 以及 WbcCurrentState（/ocs2_wbc_controller/current_state）需要 ocs2_wbc_controller 以及该机型的全身启动/配置（控制器类型为 ocs2_wbc_controller/Ocs2WbcController 时用 full_body.launch.py）。默认 mock 演示（demo.launch.py）和分体（split_body.launch.py）并不表示这些能力可用。

该控制器是私有子模块；额外 FSM 状态开通后见该仓库 README。哪些约束真正生效取决于机型（msgs README 中的 WbcCapability：移动底盘、身体相对、头部 6D、中点注视等）。公开的 Taku mock 使用 split_body.launch.py robot:=taku（臂 MPC + 身体/头的 basic_joint_controller + adaptive_gripper_controller）— 不是 WBC。full_body.launch.py robot:=taku 需要私有 ocs2_wbc_controller 子模块；公开描述里有 config/ocs2/fixed_base_tcp.info 并不够。当该配置和私有模块都在时，Taku 全身默认 headMode 是 HEAD_GAZE（注视 head_camera_mid_optical_frame）；target_manager.yaml 里 enable_head_control: false，因此头跟随 OCS2，而不是关节空间 marker。分体仍通过 head_joint_controller 遥操头部。Taku 身体相对静止位大约在 x≈−0.21，不是 Bot2 的 [0, 0.25]。头部 6D 跟踪（HEAD_TRACKING）只有控制器报出 head_tracking_ee_enabled 时才有。

这些身体笛卡尔话题走全身 / WBC 路径（full_body.launch.py，body_target_enabled）。分体（split_body.launch.py）用 basic_joint_controller 的关节话题驱动腰部（/body_joint_controller/target_joint_position 等），不是这组话题。

话题	类型	典型用途
`/body_target`	`geometry_msgs/Pose`	立即绝对身体位姿
`/body_target/stamped`	`geometry_msgs/PoseStamped`	绝对身体 MoveL（RViz TRACKING）
`/body_target/relative`	`geometry_msgs/TwistStamped`	身体一次性相对增量再走 MoveL（`frame_id` = base / `body_frame`）
`/body_current_target`	`geometry_msgs/PoseStamped`	当前身体指令回显

Python 的 send_body_target* / send_body_relative 还会发布 /mode_command 的 BODY_TRACKING（已在该模式则跳过）。双臂 /dual_target/stamped 仅在 WBC 上可带第三个身体 pose；MOVEJ 双臂下发若有三个 pose 会忽略身体那个。

头部六维目标（全身 OCS2）¶

WBC 头部 XYZ+RPY。FSM 为 OCS2 且 WbcCurrentState.head_state 为 HEAD_TRACKING 时，arms_target_manager 才插入六维 marker。离开该模式会移除 marker。HEAD_GAZE（中点注视）走同一门控并隐藏 marker。split_body 不会打开这个 marker。分体头部关节仍走 /head_joint_controller/target_joint_position。

话题	类型	典型用途
`/head_target`	`geometry_msgs/Pose`	WBC 头部 6D 最终目标，连续更新
`/head_target/stamped`	`geometry_msgs/PoseStamped`	WBC 头部 6D 插值目标，单次发送
`/head_current_target`	`geometry_msgs/PoseStamped`	最终目标回显；仅 `arms_target_manager` 订阅

API_REFERENCE 映射的是头部关节（send_head_joint_positions），不是 /head_target。head_state 常量见 msgs README（HEAD_DISABLED / HEAD_TRACKING / HEAD_GAZE / HEAD_FORWARD）。

MOVEJ + stamped → IK MoveL¶

PoseBasedReferenceManager 是 left / right / dual_target/stamped 与 TF buffer 的唯一持有者。FSM 为 OCS2 时，stamped 写入笛卡尔参考缓冲；非 OCS2 时转给 StateMoveJ.startLinearTrajectory（lina MoveL + 逐点 IK），语义同 execute_linear。vel/acc/jerk 留空则回退控制器 cartesian_defaults（README 默认：max_linear_velocity=0.25，ik_type=AUTO，time_mode=false）。没有 lina_planning 时，MOVEJ 侧为空操作。

这条 IK MoveL 路径在 ocs2_arm_controller。ocs2_wbc_controller 的 MOVEJ 只有关节（无 IK MoveL）。full_body.launch.py 在 WBC 下设 enable_movej_cartesian_markers:=false，MOVEJ 手臂 marker 保持隐藏。Python send_target_stamped 发同一条 stamped 话题；要走 IK 路径须保持 FSM=MOVEJ（关闭位姿类自动切 OCS2）。RViz Joint 面板的 MOVEJ 仍只发关节数组。

MoveJ¶

话题	类型	典型用途
`/ocs2_wbc_controller/target_joint_position/left` (and `/right`, `/body`, `/head`)	`std_msgs/Float64MultiArray`	全身 / `full_body.launch.py`
`/ocs2_arm_controller/target_joint_position/left` (and `/right`)	`std_msgs/Float64MultiArray`	分体手臂 / `split_body.launch.py`
`/ocs2_arm_controller/target_joint_position`	`std_msgs/Float64MultiArray`	单臂 OCS2
`/body_joint_controller/target_joint_position`	`std_msgs/Float64MultiArray`	分体腰部
`/head_joint_controller/target_joint_position`	`std_msgs/Float64MultiArray`	头部（`basic_joint_controller`）
`/{controller}/target_joint_trajectory`	`trajectory_msgs/JointTrajectory`	多路点 MoveJ。分体示例：`/body_joint_controller/…`、`/head_joint_controller/…`。WBC：`/ocs2_wbc_controller/target_joint_trajectory`

夹爪（`adaptive_gripper_controller`）¶

三条独立通道，每条命令立即生效。摘自 adaptive_gripper_controller README。示例名称：控制器 left_gripper_controller，关节 left_gripper_joint。

话题	类型	典型用途
`/left_gripper_joint/position_command`	`std_msgs/Float64`	直接行程（弧度或米）。无力反馈。截断到 URDF 限位
`/left_gripper_controller/target_command`	`std_msgs/Int32` (`0`/`1`)	`0` 关闭并启用力反馈；`1` 打开、无力反馈
`/left_gripper_controller/target_percent`	`std_msgs/Float64` (`0.0`–`1.0`)	在关闭↔打开之间线性混合。向关闭方向启用力反馈；向打开方向不启用

力反馈只在开关 / 比例通道上生效，且需 use_effort_interface 为 true、运动方向为关闭、以及 |effort| > force_threshold。随后剩余行程按 force_feedback_ratio 缩放（0.0 停在当前位置，1.0 继续到原始目标）。直接 position_command 从不走这条路径。

手（`basic_joint_controller`）¶

需要 target_command_enabled 且处于 MOVEJ。开/合位姿来自 Home 构型。

话题	类型	典型用途
`/{hand_controller}/target_command`	`std_msgs/Int32` (`0`/`1`)	`0` 闭合构型，`1` 打开构型
`/{hand_controller}/target_percent`	`std_msgs/Float64` (`0.0`–`1.0`)	在那两组 Home 构型之间按关节混合

腰部（`basic_joint_controller`）¶

需要 waist_lifting_enabled 且处于 MOVEJ。README 话题以控制器名为命名空间（分体上常见 /body_joint_controller/…）。API_REFERENCE 在全身上把同样名称映射到 /ocs2_wbc_controller/…。

话题	类型	典型用途
`/{controller}/waist_lifting`	`std_msgs/Float64`	相对当前位姿的高度增量（米）
`/{controller}/waist_lifting_pose_relative`	`std_msgs/Float64MultiArray`	局部 `[dx, dz, dphi]`（不做世界/地面 TF）
`/{controller}/waist_lifting_pose_absolute`	`std_msgs/Float64MultiArray`	绝对 `[x, z, phi]`（TF 坐标系可配置）
`/{controller}/waist_lifting_command`	`std_msgs/Float64`	升降速度系数 `[-1, 1]`
`/{controller}/waist_turning_command`	`std_msgs/Float64`	转向速度系数 `[-1, 1]`

腰部绝对位姿默认对齐 FiveAges W2（base_footprint / body_base）。README 中 ARX Lift / Lift 2S 示例：waist_lifting_type: single_joint，坐标系 base_link / lift_link。仅高度命令（waist_lifting、waist_lifting_command、target_joint_position）不使用这些坐标系。

API_REFERENCE 已记载的各行 Python 方法名：ros2_robot_interface。

Lift 2S 厂商 /body_control

ARX Lift 2S 厂商底盘/升降（/body_control）与这些 OCS2 / basic_joint_controller 话题是另一套栈。ros2_robot_interface 使用 MoveJ 表里的身体关节话题，而不是该厂商命令。

话题 vs Action vs Service¶

同一运动可以是话题（发出即走）、Action（目标、进度反馈、结果），有时还有服务（一次请求/响应）。定义见 arms_ros2_control_msgs README。控制器实际 advertise 哪些名字以该控制器为准；下表列出该 README 中的类型。

种类	行为	示例
话题	发出即继续；无结果	`/left_target/stamped`、`/{controller}/target_joint_trajectory`
Action	发送目标；等待结果（以及可选的 `progress`）	`ExecuteLinear`、`JointTrajectory`、`WaistLiftingPose`
Service	一次请求 / 响应	`ExecutePath`、`KinematicsService`

Python 封装：ros2_robot_interface。msgs README 里若干 srv 类型没有对应 Python 方法。

全身模式（`/mode_command`）¶

在全身 / WBC 栈上，/mode_command 是 std_msgs/String。它不是 /fsm_command。可用性与上面的身体/头部笛卡尔话题相同：分体和默认 mock 演示不会启动这条路径。典型 Python：send_mode_command，再对照 /ocs2_wbc_controller/current_state（arms_ros2_control_msgs/WbcCurrentState）调用 wait_until_mode_commands_applied。API_REFERENCE：FSM 通常已在 OCS2，否则控制器可能忽略该 mode。API_REFERENCE 不会把 HEAD_* 字符串映射到 send_mode_command；头部 6D 由 WbcCurrentState.head_state 门控。

已文档化的命令字符串，以及它们检查的 WbcCurrentState 字段（API_REFERENCE 的 MODE_COMMAND_TO_WBC_EXPECT）：

`/mode_command`	`WbcCurrentState` 字段
`BODY_*`（例如 `BODY_TRACKING`、`BODY_FREE`；`BODY_VERTICAL` 是别名）	`body_state`
`ARMS_COUPLED` / `ARMS_INDEPENDENT`	`bimanual_state`
`BASE_LOCK` / `BASE_UNLOCK`	`base_state`

msgs README 中的 WbcCurrentState 常量（不是 /fsm_command 上额外的 FSM 整数）：

字段	取值
`base_state`	`BASE_LOCKED=0` / `BASE_UNLOCKED=1`
`body_state`	`BODY_FREE=0` / `VERTICAL=1` / `TRACKING=2` / `LOCKED=3` / `CUSTOM_LOCKED=5`（`4` 曾是旧锁头值，不再发布）
`bimanual_state`	`BIMANUAL_INDEPENDENT=0` / `BIMANUAL_COUPLED=1`
`left_arm_state` / `right_arm_state`	`ARM_DISABLED=0` / `ARM_ENABLED=1`
`head_state`	`HEAD_DISABLED=0` / `HEAD_TRACKING=1` / `HEAD_GAZE=2` / `HEAD_FORWARD=3`

能力位（WbcCapability：移动底盘、身体相对、头部 6D、中点注视等）在同一 README。额外的 WBC FSM 状态开通后见私有 ocs2_wbc_controller README。

Action¶

来自 msgs README。ROS2RobotInterfaceConfig 上默认的手臂 Action 名指向 ocs2_arm_controller；控制器带命名空间时请覆盖配置。腰部 Action 名会自动检测（/ocs2_wbc_controller/waist_lifting_pose 或 /body_joint_controller/waist_lifting_pose）。

名称	类型	典型路径	作用
`ExecuteLinear`	action	`/ocs2_arm_controller/execute_linear`	参数化 MoveL（`LinearMessage` 目标）
`MovecUseIK`	action	`/ocs2_arm_controller/execute_circle_use_ik`	MoveC（`CircleMessage`；三点法或参数法）
`JointTrajectory`	action	`/ocs2_arm_controller/joint_trajectory_with_para`	参数化 MoveJ（`JointWaypoint[]`）
`WaistLiftingPose`	action	`…/waist_lifting_pose`	腰部位姿；目标 `MODE_ABSOLUTE=0` / `MODE_RELATIVE=1`

Python：execute_movel_action、execute_movec_action_three_point / execute_movec_action_parametric、execute_joint_trajectory_action / execute_dual_arm_movej_action、execute_waist_lifting_pose_absolute_action / execute_waist_lifting_pose_relative_action。它们会阻塞直到有结果或超时。对应话题（*/stamped、target_joint_trajectory、waist_lifting_pose_*）没有结果。

execute_movel_action 在 auto_switch_fsm=True 时把 FSM 切到 MOVEJ。无 stamp / 有 stamp 的话题则切到 OCS2。

Service¶

来自 msgs README。Service 是一次请求/响应；Action 另有进度。以正在运行的控制器实际 advertise 的名字为准。

名称	类型	作用
`ExecuteLinear`	srv	与 Action 相同的 `LinearMessage`，没有进度
`ExecuteCircle`	srv	`CircleMessage`
`MovecUseIK`	srv	`CircleMessage`（同名 Action 另含时长与进度）
`JointTrajectory`	srv	`joint_names` + `JointWaypoint[]`
`ExecutePath`	srv	左右臂 `nav_msgs/Path` + `trajectory_duration`
`CartesianPath`	srv	左右臂 `Path` + `duration`
`KinematicsService`	srv	正/逆运动学（`operation_type` `"fk"` / `"ik"`）

Python 把 ExecutePath 封装为 execute_path / execute_left_path / execute_right_path（服务名 execute_path）。该 README 中其余 srv 类型没有 ros2_robot_interface 方法——若控制器 advertise 了它们，请用 ROS 2 客户端直接调用。

状态机与话题¶

/fsm_command（std_msgs/Int32）¶

状态与指令话题¶

状态¶

笛卡尔手臂目标（arms_target_manager）¶

身体目标（全身 OCS2）¶

头部六维目标（全身 OCS2）¶

MOVEJ + stamped → IK MoveL¶

MoveJ¶

夹爪（adaptive_gripper_controller）¶

手（basic_joint_controller）¶

腰部（basic_joint_controller）¶

话题 vs Action vs Service¶

全身模式（/mode_command）¶

Action¶

Service¶

相关¶

`/fsm_command`（`std_msgs/Int32`）¶

笛卡尔手臂目标（`arms_target_manager`）¶

夹爪（`adaptive_gripper_controller`）¶

手（`basic_joint_controller`）¶

腰部（`basic_joint_controller`）¶

全身模式（`/mode_command`）¶