OCS2 Arm Controller

通过 OCS2 做机械臂 MPC 的 ROS 2 control 控制器（ocs2_arm_controller/Ocs2ArmController）。

事实来源

下列 FSM 和 launch 名称来自 ocs2_arm_controller/README.md 及软件包 launch/。

  • README：FSM 为 HOME / OCS2 / HOLD

  • 共享的 MoveJ / Home / Hold 原语：libraries/arms_controller_common/

仓库： fiveages-sim/arms_ros2_control

FSM

README 写明：

状态

作用

HOME (StateHome)

将手臂移动到预定义 Home 位置

OCS2 (StateOCS2)

关节位置控制

HOLD (StateHold)

进入该状态时记录并保持当前位置

控制器默认从 HOLD 开始。OCS2 只能回到 HOLD。

README 整数（软件包 README 仍写为在 /control_input 上接收）：

命令

转换

1

HOLD → HOME

2

HOME → HOLD，或 OCS2 → HOLD

3

HOLD → OCS2

操作员与 ros2_robot_interface 把同样的整数发到 /fsm_command（std_msgs/Int32）。这才是全栈 FSM 话题——状态机与话题。arms_target_manager 上的手柄 control_input（arms_ros2_control_msgs/Inputs）是另一种消息，按比例映射到 /left_target/twist。

arms_controller_common 还实现了 MoveJ（StateMoveJ）和 FSMCommandPublisher，后者在 /fsm_command 上发布 std_msgs/Int32（头文件取值：1 HOME、2 HOLD、3 OCS2、4 MOVEJ）。在混合 OCS2/WBC 栈上，Basic Joint Controller 把 3 和 4 都当作 MOVEJ，而本控制器把 3 当作 OCS2、4 当作 MOVEJ — 见 Basic Joint Controller。

分体 launch（分体控制）

split_body.launch.py 是分体控制路径（launch_mode 为 split_body）：

  • Arms: ocs2_arm_controller

  • 身体和头部：basic_joint_controller（setup_body_controllers 处理 body 与 head，例如 body_joint_controller）

  • 可选的手 / 力传感器 broadcaster 由同一 launch 辅助函数拉起

ros2 launch ocs2_arm_controller split_body.launch.py robot:=<robot>

同一软件包还有 full_body.launch.py（launch_mode 为 full_body），当机器人配置类型是 ocs2_wbc_controller/Ocs2WbcController 时会拉起 ocs2_wbc_controller。那是全身控制 — 见 OCS2 WBC Controller。

笛卡尔话题（arms_target_manager）

本控制器 README 没有完整列出末端目标话题表。它们在 arms_target_manager（PoseBasedReferenceManager）：/left_target、/left_target/stamped、/left_target/twist、/left_target/relative，以及右侧 / 双臂对应话题和 /left_current_target。

在本控制器上，MOVEJ + /left_target/stamped（以及右侧 / 双臂）可通过 lina_planning 跑 IK MoveL（StateMoveJ.startLinearTrajectory）。没有该库时 MOVEJ 笛卡尔侧为空操作。ocs2_wbc_controller 的 MOVEJ 没有 IK MoveL（只有关节数组）；full_body.launch.py 会设 enable_movej_cartesian_markers:=false。身体 / 头部笛卡尔（/body_target…、/head_target…）仅 WBC——分体用 basic_joint_controller 管这些关节。

完整表格：FSM 与话题。参数化 MoveL / MoveC / MoveJ 在本控制器上也有 Action（ExecuteLinear、MovecUseIK、JointTrajectory）；Python：ros2_robot_interface。类型：arms_ros2_control_msgs README。

Demo launch (README)

ros2 launch ocs2_arm_controller demo.launch.py type:=AG2F90-C
ros2 launch ocs2_arm_controller demo.launch.py robot:=arx5 type:=r5
ros2 launch ocs2_arm_controller demo.launch.py hardware:=gz type:=AG2F120S
ros2 launch ocs2_arm_controller demo.launch.py hardware:=isaac type:=AG2F90-C

colcon build –packages-up-to ocs2_arm_controller

末端执行器（type / left_type / right_type）

demo.launch.py、split_body.launch.py 和 full_body.launch.py 声明 type，并调用 robot_common_launch 的 create_robot_profile_launch_arguments()。该辅助函数增加 left_type / right_type（以及 use_profile_eef / robot_profile）。没有 gripper:= 参数。

出自 create_eef_side_launch_arguments()：left_type 是左侧末端键（rg75、ag2f90_c、linkerhand_o7 等）。与 right_type 一起用于左右不同的配置；此时不要再传 type:=。

# Symmetric EEF (package README)
ros2 launch ocs2_arm_controller demo.launch.py type:=AG2F90-C

# Different L/R (robot_common_launch README)
ros2 launch ocs2_arm_controller demo.launch.py \
  robot_profile:=/path/to/machine_profile.yaml \
  use_profile_eef:=false \
  left_type:=rg75 right_type:=linkerhand_o7

type 也可以是臂拓扑 left / right / dual（不会展开成 left_type / right_type）。完整合并规则见 robot_common_launch。

配置

参数表（仅启动 vs 运行时）：控制器 ROS 2 参数。机型 YAML 在 {robot}_description/config/ros2_control/ros2_controllers.yaml（树内没有 config/ocs2_arm_controller.yaml）。README 包专有键：home_pos、rest_pos、robot_name、default_gains、pd_gains、坐标系、movel_*。不是参数的旧名：zero_pos、robot_pkg、force_gains。

OCS2 文件（由 robot_name → {robot_name}_description 选定；不是 ROS 2 参数）：

  • 任务文件：{robot_name}_description/config/ocs2/task.info

  • 规划 URDF：经 robot_common_launch 的 xacro（planning_urdf_variant:=xacro、planning_urdf_path）。静态 urdf/{robot}_{robot_type}.urdf 查找已移除（CtrlComponent.cpp）

  • 生成库：{robot_name}_description/config/ocs2/generated

控制模式由 ros2_control 的 command/state interface 自动检测（仅 position，或当 position、velocity、effort、kp、kd 齐全时为力控/MIX）。

Panthera HT / 方舟无限的 VR 遥操作走这条 MIT / MIX 路径。天玑 / 珞石的 VR 柔顺是位置命令 + 厂商 HI 阻抗，不是这张 MIX 表 — VR 遥操作。