建议实现
- 在主工作空间新增 kuavo_moveit_bridge 包，提供：
-
action：/kuavo_arm_controller/follow_joint_trajectory
-
输入类型：control_msgs/FollowJointTrajectoryAction
-
输出：/kuavo_arm_traj
-
订阅：/sensors_data_raw
-
发布：/joint_states
- 将 MoveIt 控制器配置改成一个支持双臂和单臂局部 goal 的 14 关节控制器：
controller_list:
- name: kuavo_arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
-
zarm_l1_joint
-
zarm_l2_joint
-
zarm_l3_joint
-
zarm_l4_joint
-
zarm_l5_joint
-
zarm_l6_joint
-
zarm_l7_joint
-
zarm_r1_joint
-
zarm_r2_joint
-
zarm_r3_joint
-
zarm_r4_joint
-
zarm_r5_joint
-
zarm_r6_joint
-
zarm_r7_joint
- 桥接节点执行 goal 时需要：
-
校验关节名、时间递增、NaN、位置和速度限制。
-
对单臂 goal 用另一只手的实际位置补齐为 14 关节。
-
按 time_from_start 在 100～200 Hz 插值。
-
将位置和速度由弧度转换成度后发布 /kuavo_arm_traj。
-
调用 /arm_traj_change_mode，切换到 control_mode: 2。
-
MPC 模式下还要调用 /enable_wbc_arm_trajectory_control；该功能默认关闭，见 src/humanoid-control/humanoid_controllers/include/humanoid_controllers/humanoidController.h:719。
-
根据实际编码器误差产生 action feedback，并在达到目标容差后返回成功。
-
cancel、反馈超时或误差过大时立即停止轨迹，保持当前实测位置。
- 真机反馈索引不能照抄现有 GUI 的 joint_q12:26。S52 的第 12 项是腰关节，双臂实际位于 13～26，见 src/humanoid-control/humanoid_controllers/config/kuavo_v52/mpc/task.info:46。
桥接应读取：
arm_start = /legRealDof + /waistRealDof
arm_count = /armRealDof
这些参数由控制器设置，见 src/humanoid-control/humanoid_controllers/src/humanoidController.cpp:299。现有 GUI 在 S52 上存在包含腰关节、遗漏右臂第七轴的风险。
- 增加 real_robot_moveit.launch，只启动：
-
S52 对应的 robot_description
-
真机状态桥接
-
robot_state_publisher
-
action 桥接
-
move_group，使用 simple controller
-
MoveIt RViz
不能再启动 fake joint state publisher。
模型方面还需处理
当前 MoveIt 配置只适用于 S52，并且使用了一份复制的 URDF。应由 kuavo_assets/models/biped_s52 生成 MoveIt 兼容模型，避免真机模型升级后规划模型不一致。
SRDF 当前末端是 zarm_l7_link/zarm_r7_link，见 kuavo_moveit/src/kuavo_moveit_config/config/biped_s52.srdf:12，而项目实际 IK 末端使用 zarm_l7_end_effector/zarm_r7_end_effector，见
src/kuavo_assets/config/kuavo_v52/kuavo.json:159。如果要做笛卡尔末端规划，需要统一这个工具坐标系。
RViz 操作语义
标准操作应是：
-
在 RViz 拖动橙色目标模型。
-
点击 Plan 检查轨迹和碰撞。
-
点击 Execute 或 Plan and Execute 后真机运动。