Skip to main content

轮臂单手增量摇操躯干说明

v62（5W 平台）半身 IK + 单手摇操躯干说明​

更新时间：2026-06-16 适用平台：仅 v62（5W），强脑灵巧手（qiangnao）末端 范围：v62 半身 arm-only IK（纯手臂 14 DOF）+ 单手摇杆摇操躯干（升降 / 前倾 / 转身）

⚠️ 版本限制：IK 节点本身按 /robot_version 通用加载 kuavo_v{N}/kuavo.json，但「半身 14 DOF」所需资产目前只有 v62 配齐：arm_only_urdf 字段仅存在于 kuavo_v62/kuavo.json，对应的 biped_v3_arm_only.urdf（14 DOF 纯双臂）也仅 biped_s62/ 提供，且 drake_velocity_ik_geometry 为 v62 专门标定。在其他版本上传 use_arm_only_ik:=true 会 fallback 到该版本的全臂 arm_urdf（见 Quest3IkIncrementalROS.cpp:2239），半身解算失效。故本功能当前仅 v62 可用。

0. 功能概述​

相比常规轮臂全身跟踪 / 增量遥操作，本功能新增两个相互解耦的能力：

能力开关参数启动的节点作用
半身 arm-only IKuse_arm_only_ik:=truewheel_arm_only_ik_ros_uni_cpp_node只解算手臂 14 DOF，不做 chest 控制、不发 cmd_vel，加载 arm_only_urdf
单手摇操躯干single_hand_mode:=truetorso_joystick_node.py用一只手的摇杆 + A/B 键增量控制躯干升降 / 前倾 / 转身

两者通过全局 rosparam /single_hand_mode 与底盘转发器协调：单手锁定（grip > 0.7）时抑制 cmd_vel 转发，让躯干控制独占躯干链路（/cmd_torso_vel / /cmd_torso_delta），避免 chest 关节命令与躯干命令互相覆盖。

1. 启动注意事项​

  • 启动前请将 ip_address 替换为实际设备 IP。
  • 控制器（终端 1）必须先启动并就绪，IK 节点（终端 2）会自动等待控制器服务可用后再配置（与增量遥操作一致，详见 轮臂增量遥操作说明.md §0.2）。
  • 半身 IK 与单手摇操躯干可分别独立开启；同时摇操躯干请务必带上 single_hand_mode:=true，否则 torso_joystick_node 不会启动。
  • 本功能目前仅适配 5W + 强脑灵巧手（qiangnao）；其他末端（jodell / lejuclaw）的 grip→trigger 接管尚未适配。

2. VR 软件端（Quest 3）​

  • VR 软件版本要求与下载链接同 轮臂增量遥操作说明.md §2。

3. 启动命令​

3.1 仿真 + 半身 IK + 单手摇操躯干（双终端）​

1) 终端 1：

roslaunch humanoid_controllers load_kuavo_mujoco_sim_wheel.launch

2) 终端 2：

roslaunch noitom_hi5_hand_udp_python launch_quest3_ik.launch \
  ip_address:=xxx.xxx.xxx.xxx \
  use_cpp_incremental_ik:=true \
  use_arm_only_ik:=true \
  single_hand_mode:=true

3.2 实物 + 半身 IK + 单手摇操躯干（双终端）​

1) 终端 1：

roslaunch humanoid_controllers load_kuavo_real_wheel.launch

2) 终端 2（同仿真）：

roslaunch noitom_hi5_hand_udp_python launch_quest3_ik.launch \
  ip_address:=xxx.xxx.xxx.xxx \
  use_cpp_incremental_ik:=true \
  use_arm_only_ik:=true \
  single_hand_mode:=true

⚠️ 启动文件区分：终端 1 必须使用 load_kuavo_real_wheel.launch（纯控制器），切勿使用 load_kuavo_real_wheel_vr.launch（该 launch 内部会同时拉起 IK + VR 链路，与本功能的两终端分离启动方式冲突，导致重复 VR 连接、手臂持续摆动等问题）。实物与仿真均遵循此规则。

3.3 仅半身 IK，不摇操躯干​

去掉 single_hand_mode:=true 即可（torso_joystick_node 不启动，grip 不接管，底盘 cmd_vel 正常转发）：

roslaunch noitom_hi5_hand_udp_python launch_quest3_ik.launch \
  ip_address:=xxx.xxx.xxx.xxx \
  use_cpp_incremental_ik:=true \
  use_arm_only_ik:=true

4. 参数说明​

参数默认值说明
ip_address""Quest3 遥操作链路目标 IP，必填。
use_cpp_incremental_ikfalse启用 C++ 增量 IK，本功能必须 true。
use_arm_only_ikfalse启用 v62 半身 arm-only IK（纯手臂 14 DOF），与 wheel_ik 解耦。
single_hand_modefalse启用单手摇操躯干独立节点；同时让底盘转发器在 grip 锁定时让位躯干控制。
control_torsofalse是否做 chest 关节跟踪，半身摇操躯干场景保持默认 false。

4.1 节点启动条件（launch 内部逻辑）​

条件启动节点说明
use_cpp_incremental_ik && use_arm_only_ikik_ros_uni_cpp_node（type=wheel_arm_only_ik_ros_uni_cpp_node）v62 半身 IK，跟 wheel_ik 解耦
single_hand_modetorso_joystick_node单手摇操躯干
全局 param /single_hand_mode—让底盘转发器在 grip 锁定时抑制 cmd_vel

4.2 摇操躯干运动参数（tools/torso_joystick_controller.py 常量）​

常量默认值含义
GRIP_PRESS_TH0.7grip > 0.7 锁定主控手
GRIP_RELEASE_TH0.3grip < 0.3 释放主控手（0.7/0.3 滞回）
STICK_DEAD_ZONE0.1摇杆死区
SCALE_HEIGHT0.10升降速度 m/s
SCALE_PITCH0.30前倾角速度 rad/s
Z_MIN / Z_MAX0.0 / 0.32升降相对初始位姿限幅 m
PITCH_MIN / PITCH_MAX-0.5235 / 0.0前倾限幅 rad（VR 内部：负值前倾，0 为直立）
YAW_MAX0.5235yaw 限幅 rad（±30°，仅摇杆积分；按钮转身仍走 ±π）
YAW_TARGETπ每次 A/B 键转身增量 rad（±180°）

如需长期调灵敏度，建议直接改 src/manipulation_nodes/motion_capture_ik/scripts/tools/torso_joystick_controller.py 顶部这些常量默认值。

5. 手柄操作要点​

核心机制：单手仲裁 —— 先按住 grip（> 0.7）的那只手成为「主控手」，独占躯干控制；松开（< 0.3）后才释放、可换手。主控手锁定期间，该侧 grip 不再控手指，而是切到 trigger（扳机）接管所有手指。

操作项触发方式行为说明
激活手臂遥操作双手按下 X + A 键进入手臂增量跟踪，建议等待约 5 秒再开始动作。
锁定主控手按住任一手柄 grip（> 0.7）先按住的手成为主控手，激活躯干控制。
升降主控手摇杆 上/下推Y 轴正向 → 升高，反向 → 降低（增量，限幅 0.0 ~ 0.32 m）。
前倾主控手摇杆 前/后推X 轴正向 → 前倾，增量积分，限幅 ±30°。
转身主控手 first / second 键左主控手：X 键 yaw -= π，Y 键 yaw += π；右主控手：A 键 yaw -= π，B 键 yaw += π。相对当前 yaw 增量，结果归一化到 [-π, π]，互逆可复位。
手指抓取主控手 trigger（扳机）主控手锁定后 grip 改作模式键，扳机接管该手所有手指开合。
释放 / 换手主控手 grip 松开（< 0.3）仅主控手 grip 松开才释放锁定，可换另一只手接管。
底盘移动非锁定状态 + 摇杆未锁定主控手时底盘 cmd_vel 正常转发；一旦 grip 锁定即抑制底盘转发，让位躯干控制。

⚠️ 转身安全提示：执行转身（yaw 旋转）操作前，请先将手臂抬高、远离折叠臂/躯干部分，避免腰旋转时手臂碰到折叠臂。转身过程中也请注意手臂姿态，确认不会与身体发生干涉后再操作。

6. 关键话题​

话题方向说明
/leju_quest_bone_poses输入VR 骨骼姿态
/quest_joystick_data输入手柄按键 / 摇杆，半身 IK 节点与 torso_joystick_node 都订阅
/torso_open_loop_state输入躯干开环 4D 位姿（RM 发布），各输入源读取做 workspace 钳制
/kuavo_arm_traj输出半身 IK 关节指令（/kuavo_arm_traj_cpp remap 而来）
/cmd_torso_vel输出躯干升降/前倾速度（geometry_msgs/Twist），发往 RM 做 sticky 积分
/cmd_torso_delta输出躯干偏航增量（geometry_msgs/Twist，oneshot；A/B 按键边沿），发往 RM
/cmd_lb_torso_pose输出复位成功后可再发一次绝对零位（兼容观测）；复位主路径为服务
/mobile_manipulator_reset_torso服务双击 grip 触发的躯干复位
/cmd_vel输出底盘速度；grip 锁定时被抑制

7. 参数调优（运行时）​

与增量遥操作一致，IK 响应可在线调：

rosparam set /ik_ros_uni_cpp_node/quest3/fhan_r 3000.0     # 响应速度
rosparam set /ik_ros_uni_cpp_node/quest3/fhan_kh0 3.0      # 平滑度

注：v62 新节点 ROS name 为 ik_ros_uni_cpp_node（与常规 C++ IK 节点同名以复用 quest3/* 参数命名空间），调参时注意 namespace 前缀。

浏览量 --