FSM and Topics¶
Finite-state machines in this stack live in arms_ros2_control. Shared primitives are in libraries/arms_controller_common/ (StateHome, StateHold, StateMoveJ, FSMCommandPublisher). Cartesian EE topics are owned by arms_target_manager (PoseBasedReferenceManager).
Source of truth
/fsm_command is std_msgs/Int32, not String, and not wheeled-arm strings such as stand / walk / arm_teleop.
/fsm_state is also std_msgs/Int32 (latched) — ros2_robot_interface API_REFERENCE and the panthera-ht README.
/mode_command is std_msgs/String on the WBC stack (send_mode_command in ros2_robot_interface: BODY_TRACKING, ARMS_COUPLED, BASE_LOCK, …). It is not an FSM String and not a stand-in for /fsm_command.
Cartesian EE goals are /left_target, /left_target/stamped, /left_target/twist, and /left_target/relative (and the right / dual counterparts), not a stack-wide /target_pose. Per-controller extras: the controller pages and package READMEs. Python mapping: ros2_robot_interface.
/fsm_command (std_msgs/Int32)¶
FSMCommandPublisher publishes std_msgs/Int32 on /fsm_command. Shared integers:
Value |
Constant / label |
Role |
|---|---|---|
|
|
Move to a preset home configuration |
|
|
Hold; safe stop. Typical return path from OCS2 / MOVEJ |
|
|
Arm / WBC MPC (Cartesian targets). On standalone |
|
|
Direct joint-position mode (canonical MOVEJ on mixed OCS2/WBC stacks) |
|
|
Compliance / FT force mode in ros2_robot_interface ( |
|
|
While already in HOME: cycle to the next |
|
|
While in HOME: select configuration index |
Exact transitions depend on the running controller:
Controller |
States |
Commands |
|---|---|---|
HOME / HOLD / MOVEJ |
README: |
|
HOME / OCS2 / HOLD |
README integers: |
|
全身 stack |
Private package; extra states are in that README after access. Public Python still uses the same |
On mixed stacks, head / split-waist basic_joint_controller treats 3 and 4 as MOVEJ, while the arm / WBC controller treats 3 as OCS2 and 4 as MOVEJ.
The ocs2_arm_controller README still names /control_input for those 1/2/3 integers. That is not the stack-wide FSM topic. Joystick control_input (arms_ros2_control_msgs/Inputs) on arms_target_manager is a different message: it is scaled into /left_target/twist / /right_target/twist.
ros2 topic pub --once /fsm_command std_msgs/msg/Int32 "data: 2" # HOLD
Split vs whole-body launches (and the Lift2S quick_start split body / full body menu) are on 分体控制 vs 全身控制. Teleop is a separate interface, not an extra FSM state — Isomorphic Teleop.
Status and command topics¶
Names below are those the public ros2_robot_interface config and API_REFERENCE subscribe / publish, plus the public controller READMEs. connect() rewrites split vs WBC joint prefixes when both are present (WBC wins).
State¶
Topic |
Type |
Typical use |
|---|---|---|
|
|
All joints (arms, head, waist, grippers) |
|
|
Current EE pose ( |
|
|
Active Cartesian command echo; arrival checks ( |
|
|
WBC current body pose |
|
|
WBC body command echo ( |
|
|
WBC Head 6D final-target echo; |
|
|
WBC constraint snapshot; used to confirm |
ros2 topic echo /joint_states
ros2 topic echo /left_current_pose
ros2 topic echo /left_current_target
Cartesian arm targets (arms_target_manager)¶
Left arm; right arm is symmetric (/right_target…). From the arms_target_manager README PoseBasedReferenceManager table.
Unstamped /left_target replaces the current EE goal with one pose (VR / high-rate teleop). Stamped /left_target/stamped runs TF into the controller frame and interpolates a MoveL sequence (vision pick / RViz absolute). Far unstamped jumps can jerk.
Topic |
Type |
Typical use |
|---|---|---|
|
|
Immediate absolute pose in |
|
|
Absolute MoveL; non-base |
|
|
Velocity stream (m/s, rad/s) in base; latch + |
|
|
One-shot relative increment (m, rad) + MoveL; |
|
|
Dual-arm; Path poses |
|
|
Dual-arm waypoint path (Python API marks this topic deprecated in favor of the |
relative uses TwistStamped: twist is a displacement increment, not a velocity. Non-base frame_id rotates linear / angular into base before composing. twist stays a bare Twist (velocity, base only). angular.{x,y,z} is roll / pitch / yaw (R' = RΔ(yaw)·RΔ(pitch)·RΔ(roll)·R).
Body targets (全身 OCS2)¶
Whole-body (WBC) availability
/body_target*, /head_target*, /mode_command, and WbcCurrentState (/ocs2_wbc_controller/current_state) need ocs2_wbc_controller and that robot’s 全身 launch/config (full_body.launch.py when the controller type is ocs2_wbc_controller/Ocs2WbcController). Default mock demos (demo.launch.py) and 分体 (split_body.launch.py) do not imply these features are present.
The controller is a private submodule; extra FSM states stay in that README after access. Which constraints are live is machine-dependent (WbcCapability in the msgs README: mobile base, body relative, head 6D, midpoint gaze, …). Public Taku mock uses split_body.launch.py robot:=taku (arm MPC + body/head basic_joint_controller + adaptive_gripper_controller) — that is not WBC. full_body.launch.py robot:=taku needs the private ocs2_wbc_controller submodule; shipping config/ocs2/fixed_base_tcp.info in the public description is not enough. When that config and the private module are present, Taku full-body default headMode is HEAD_GAZE (gaze on head_camera_mid_optical_frame); target_manager.yaml has enable_head_control: false so the head follows OCS2 rather than a joint-space marker. Split-body still teleops the head through head_joint_controller. Taku body-relative rest is around x≈−0.21, not Bot2 [0, 0.25]. Head 6D tracking (HEAD_TRACKING) is only there when the controller reports head_tracking_ee_enabled.
These Cartesian body topics are on the 全身 / WBC path (full_body.launch.py, body_target_enabled). 分体 (split_body.launch.py) drives the waist with basic_joint_controller joint topics (/body_joint_controller/target_joint_position, …), not this set.
Topic |
Type |
Typical use |
|---|---|---|
|
|
Immediate absolute body pose |
|
|
Absolute body MoveL (RViz TRACKING) |
|
|
One-shot relative body increment + MoveL ( |
|
|
Active body command echo |
Python send_body_target* / send_body_relative also publish /mode_command BODY_TRACKING (skipped if already in that mode). Dual-arm /dual_target/stamped may carry a third body pose on WBC only; MOVEJ dual send ignores a body pose if three are present.
Head 6D targets (全身 OCS2)¶
WBC Head XYZ+RPY. arms_target_manager inserts the 6D marker when FSM is OCS2 and WbcCurrentState.head_state is HEAD_TRACKING. Leaving that mode removes the marker. HEAD_GAZE (midpoint gaze) uses the same gate and hides the marker. split_body does not turn this marker on. 分体 head joints stay on /head_joint_controller/target_joint_position.
Topic |
Type |
Typical use |
|---|---|---|
|
|
WBC Head 6D final target, continuous |
|
|
WBC Head 6D interpolated target, one-shot |
|
|
Final-target echo; |
API_REFERENCE maps head joints (send_head_joint_positions), not /head_target. head_state constants: msgs README (HEAD_DISABLED / HEAD_TRACKING / HEAD_GAZE / HEAD_FORWARD).
MOVEJ + stamped → IK MoveL¶
PoseBasedReferenceManager is the sole holder of left / right / dual_target/stamped and the TF buffer. While FSM is OCS2, stamped writes the Cartesian reference buffer. While FSM is not OCS2, stamped is forwarded to StateMoveJ.startLinearTrajectory (lina MoveL + per-point IK), same semantics as execute_linear. Empty vel/acc/jerk fall back to controller cartesian_defaults (README defaults: max_linear_velocity=0.25, ik_type=AUTO, time_mode=false). Without lina_planning the MOVEJ side is a no-op.
This IK MoveL path is on ocs2_arm_controller. ocs2_wbc_controller MOVEJ is joint-only (no IK MoveL). full_body.launch.py sets enable_movej_cartesian_markers:=false on WBC, so MOVEJ arm markers stay hidden. Python send_target_stamped publishes the same stamped topic; keep FSM at MOVEJ (turn off pose auto-switch to OCS2) for the IK path. The RViz Joint panel MOVEJ path still publishes joint arrays only.
MoveJ¶
Topic |
Type |
Typical use |
|---|---|---|
|
|
全身 / |
|
|
分体 arms / |
|
|
Single-arm OCS2 |
|
|
分体 waist |
|
|
Head ( |
|
|
Multi-waypoint MoveJ. Split examples: |
Gripper (adaptive_gripper_controller)¶
Three independent channels; each command takes effect immediately. From the adaptive_gripper_controller README. Example names: controller left_gripper_controller, joint left_gripper_joint.
Topic |
Type |
Typical use |
|---|---|---|
|
|
Direct stroke (rad or m). No force feedback. Clamped to URDF limits |
|
|
|
|
|
Linear blend closed↔open. Closing direction enables force feedback; opening does not |
Force feedback runs only on the switch / percent channels, when use_effort_interface is true, the motion is closing, and |effort| > force_threshold. Then the remaining stroke is scaled by force_feedback_ratio (0.0 stop here, 1.0 continue to the original target). Direct position_command never uses that path.
Hand (basic_joint_controller)¶
Requires target_command_enabled and MOVEJ. Open/close poses are Home configurations.
Topic |
Type |
Typical use |
|---|---|---|
|
|
|
|
|
Per-joint blend between those two Home configs |
Waist (basic_joint_controller)¶
Requires waist_lifting_enabled and MOVEJ. README topics are namespaced to the controller (often /body_joint_controller/… on 分体). API_REFERENCE maps the same names under /ocs2_wbc_controller/… on 全身.
Topic |
Type |
Typical use |
|---|---|---|
|
|
Height delta (m) from the current pose |
|
|
Local |
|
|
Absolute |
|
|
Lifting velocity factor |
|
|
Turning velocity factor |
Waist absolute-pose defaults match FiveAges W2 (base_footprint / body_base). README example for ARX Lift / Lift 2S: waist_lifting_type: single_joint with base_link / lift_link. Height-only commands (waist_lifting, waist_lifting_command, target_joint_position) do not use those frames.
Python method names for every row that API_REFERENCE documents: ros2_robot_interface.
Lift 2S vendor /body_control
ARX Lift 2S vendor chassis/lift (/body_control) is a different stack from these OCS2 / basic_joint_controller topics. ros2_robot_interface uses the body joint topics in the MoveJ table, not that vendor command.
Topic vs Action vs Service¶
Same motion can exist as a topic (fire-and-forget), an action (goal, progress feedback, result), and sometimes a service (one-shot RPC). Definitions: arms_ros2_control_msgs README. Which names a controller actually advertises is on that controller; the tables below list the types in that README.
Kind |
Behaviour |
Example |
|---|---|---|
Topic |
Publish and continue; no result |
|
Action |
Send a goal; wait for result (and optional |
|
Service |
One request / response |
|
Python wrappers: ros2_robot_interface. Several srv types in the msgs README have no Python method.
Whole-body mode (/mode_command)¶
/mode_command is std_msgs/String on the 全身 / WBC stack. It is not /fsm_command. Same availability as the body/head Cartesian topics above: 分体 and default mock demos do not start this path. Typical Python: send_mode_command, then wait_until_mode_commands_applied against /ocs2_wbc_controller/current_state (arms_ros2_control_msgs/WbcCurrentState). API_REFERENCE: FSM is usually already OCS2, or the controller may ignore the mode. API_REFERENCE does not map HEAD_* strings onto send_mode_command; head 6D is gated by WbcCurrentState.head_state.
Documented command strings and the WbcCurrentState fields they check (API_REFERENCE MODE_COMMAND_TO_WBC_EXPECT):
|
|
|---|---|
|
|
|
|
|
|
WbcCurrentState constants from the msgs README (not extra FSM integers on /fsm_command):
Field |
Values |
|---|---|
|
|
|
|
|
|
|
|
|
|
Capability bits (WbcCapability: mobile base, body relative, head 6D, midpoint gaze, …) are in the same README. Extra WBC FSM states stay in the private ocs2_wbc_controller README after access.
Actions¶
From the msgs README. Default arm action names on ROS2RobotInterfaceConfig point at ocs2_arm_controller; override the config if the controller has a namespace. Waist action name is auto-detected (/ocs2_wbc_controller/waist_lifting_pose or /body_joint_controller/waist_lifting_pose).
Name |
Type |
Typical path |
Role |
|---|---|---|---|
|
action |
|
Parameterized MoveL ( |
|
action |
|
MoveC ( |
|
action |
|
Parameterized MoveJ ( |
|
action |
|
Waist pose; goal |
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. These block until a result or timeout. The matching topics (*/stamped, target_joint_trajectory, waist_lifting_pose_*) have no result.
execute_movel_action with auto_switch_fsm=True switches FSM to MOVEJ. Unstamped / stamped topics switch to OCS2.
Services¶
From the msgs README. Service is a single request/response; action adds progress. Names a running controller advertises win.
Name |
Type |
Role |
|---|---|---|
|
srv |
Same |
|
srv |
|
|
srv |
|
|
srv |
|
|
srv |
Left/right |
|
srv |
Left/right |
|
srv |
FK / IK ( |
Python wraps ExecutePath as execute_path / execute_left_path / execute_right_path (service name execute_path). The other srv types in that README have no ros2_robot_interface method — call them with ROS 2 clients if the controller advertises them.