acRealman_xr/ik_qp/README.md

RM75-B 运动学、QP IK 与 MuJoCo 验证

本目录是独立 Python 算法包，用于验证 RM75-B 的运动学、逆运动学和后端无关遥操作 控制。核心包不依赖 ROS 2，也不会建立真实机器人连接；第三阶段由 xr_rm_teleop 提供 薄 ROS 消息适配层并使用 MuJoCo 后端。

第一阶段包含：

• 由 Pinocchio 加载的标准单臂 RM75-B URDF。
• 基于 SE(3) 的正运动学、局部坐标雅可比矩阵和位姿残差。
• 支持热启动的 OSQP 微分逆运动学。
• 作为独立参考的 RealMan API2 Algo FK。
• 物理关节限位配置和项目专用的遥操作关节限位配置。
• 由两份标准单臂模型组成的双臂装配模型。
• 可生成 JSON、CSV 和 Markdown 报告的确定性验证流程。

第二阶段在完整双臂场景中使用第一阶段求解器驱动一侧机械臂，包含：

• 由标准单臂 URDF 复制两条运动链生成的规范化 14 轴 MJCF。
• 来自上传双臂模型的安装变换、平台、挂架、夹爪和 19 个 OBJ 资源。
• 默认控制左臂、固定右臂的纯运动学 MuJoCo 播放。
• headless EGL 自动验证和 GLFW 实时 viewer 演示。
• MuJoCo、Pinocchio 与 RealMan Algo FK 的三方对照。

第三阶段包含：

• 右臂 FK、IK 和连续轨迹快速验收。
• 按左右工具 TCP 求解的双臂仿真初始姿态。
• 与 XrController.msg 字段兼容、但不导入 ROS 的 grip 相对位姿控制核心。
• 双臂 SE(3) 目标、QP IK、关节限位、速度限制和故障联停状态机。
• 可由未来 RealmanRobot 实现复用的 RobotBackend 协议。

碰撞规避、动力学伺服和真实机器人控制仍不在当前范围内。

环境

经过验证的环境定义在 environment.yml 中：

cd /home/robot/WS_xr/src/ik_qp
conda env update -f environment.yml
conda run -n qp python -m pip install -e . --no-deps

环境文件会从 PyPI 安装 Robotic_Arm 1.1.5，通常无需额外设置。若需要强制使用 本地 API2 SDK，可为验证程序指定包含 Robotic_Arm/ 的目录：

export REALMAN_SDK_ROOT=/path/to/RM_API2/Python

公共 API

from rm75_ik import (
    DualArmAssembly,
    RM75IkSolver,
    RM75Kinematics,
    RealManFkReference,
    teleop_joint_limits,
)

kinematics = RM75Kinematics(limits=teleop_joint_limits())
solver = RM75IkSolver(kinematics)
target = kinematics.forward(target_q_rad)
result = solver.solve(target, current_q_rad)

if result.success:
    solution_q_rad = result.q

双臂 MuJoCo 场景：

from rm75_ik import DualArmMuJoCo

scene = DualArmMuJoCo(controlled_arm="left")
scene.set_arm_configuration("left", solution_q_rad)
world_flange_pose = scene.get_flange_pose("left")
image = scene.render()

后端无关双臂控制：

from rm75_ik import DualArmQpTeleopController, load_dual_arm_profiles
from rm75_ik.mujoco_robot import MujocoRobot

profiles = load_dual_arm_profiles(teleop_yaml, peripheral_yaml)
robot = MujocoRobot(profiles)
controller = DualArmQpTeleopController(robot, profiles)

对于任何失败状态，IkResult.q 均为 None。不得将失败或未经验证的结果发送给 机器人。

每一对 RM75Kinematics/RM75IkSolver 都持有可变的 Pinocchio 和 OSQP 状态，因此 只能由一个控制线程使用。未来的双臂控制器应为每条机械臂分别持有一对实例。

验证

运行快速单元测试：

REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
  conda run -n qp python -m pytest -q

运行完整、严格的第一阶段基准测试：

REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
  conda run -n qp rm75-stage1-validate

如需进行小规模冒烟测试，请添加 --quick。报告将写入 artifacts/stage1/，并且 该目录有意设置为由 Git 忽略。

验收标准和最近一次完整结果请参见 STAGE1_VALIDATION.md。

运行第二阶段完整 headless 验证：

REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
  conda run -n qp rm75-stage2-validate --arm left

启动双臂实时可视化，只驱动左臂：

conda run -n qp rm75-stage2-demo --arm left --trajectory combined

移动到指定目标点。目标位置位于受控机械臂基座坐标系，单位为米；未提供 RPY 时保持 起始姿态：

conda run -n qp rm75-stage2-demo \
  --arm left \
  --target-position 0.45 0.0 0.3375 \
  --duration 8 \
  --wait-before 2 \
  --hold-after 3

也可用弧度指定目标姿态：

conda run -n qp rm75-stage2-demo \
  --arm left \
  --target-position 0.45 0.0 0.3375 \
  --target-rpy -3.14159 0.52360 -3.14159 \
  --duration 8

手动拖动受控机械臂：

conda run -n qp rm75-stage2-demo --arm left --manual-drag

在 Viewer 中双击选中连杆，然后按住 Ctrl 并使用鼠标右键拖动。该模式使用零重力、 有阻尼的 MuJoCo 动力学，只用于检查关节活动与模型装配，不属于 IK 验收结果。

支持的演示轨迹为 joint、line、arc、orientation 和 combined。无桌面环境时 可添加 --headless --output stage2_demo.png。阶段二报告和截图写入 artifacts/stage2/，验收结果见 STAGE2_VALIDATION.md。

运行第三阶段完整 headless 验证：

conda run -n qp rm75-stage3-validate \
  --sdk-root /path/to/RM_API2/Python \
  --teleop-config ../xr_rm_bringup/config/dual_arm_rm75.yaml \
  --peripheral-config ../xr_rm_bringup/config/peripherals_rm75.yaml

在 ROS 工作空间中启动 PICO/UDP、双臂 QP 和共享 MuJoCo viewer：

conda activate qp
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py

该 launch 将 robot_backend 固定为 mujoco，不会连接真实机械臂。headless 运行可使用：

ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py \
  show_viewer:=false mujoco_gl:=egl

常用 launch 参数：

参数	默认值	说明
udp_host	0.0.0.0	XR UDP 监听地址
udp_port	15000	XR UDP 监听端口
udp_timer_hz	200.0	UDP receiver 轮询频率
control_rate_hz	90.0	双臂 QP 控制周期频率
show_viewer	true	是否显示共享 MuJoCo viewer
mujoco_gl	glfw	MuJoCo 渲染后端；无桌面环境建议使用 egl

启动成功后，日志应先显示 UDP receiver 的监听地址，再显示左右臂从 initial_tcp_pose 初始化的残差，最后出现 dual-arm QP teleop ready。第三阶段结果见 STAGE3_VALIDATION.md。

PICO 在线控制使用 src/rm75_ik/solver.py 中的正式 QP 求解器。每个机械臂可在 dual_arm_rm75.yaml 中独立配置以下参数：

参数	当前值	作用
qp_w_limit_mid	0.00002	按关节范围归一化，将关节从软限位附近拉向范围中心
qp_joint_motion_weights	[1,1,1,1,0.3,0.3,0.2]	QP 阻尼权重；越小越积极参与冗余运动
qp_joint_step_limits_rad	[0.05,0.05,0.05,0.05,0.08,0.08,0.10]	每次 QP 内部迭代的关节步长上限，单位为 rad

中心惩罚使用 diag(1 / (q_upper - q_lower)^2) 消除不同关节范围造成的量级差异，并与 原有硬关节限位同时生效。qp_joint_step_limits_rad 只影响 IK 内部收敛；最终发送给 MuJoCo 的每周期关节变化仍受 joint_max_speed / control_rate_hz 限制。上述参数不会为 初始化 IK 自动启用，避免改变启动逆解支路，也不构成碰撞规避保证。

运行过程中可在 MuJoCo viewer 中按 R 或 Home，将双臂恢复到本次启动时求得的 初始关节状态；也可调用：

ros2 service call /xr_rm/qp/reset_to_initial std_srvs/srv/Trigger "{}"

Reset 会清除旧的 grip 相对位姿和控制故障。为避免旧输入在复位后立即重新驱动机械臂， 控制器必须收到左右手柄新的 grip=false 消息后才恢复为 idle。MuJoCo passive viewer 自带的 Reload 按钮仍为灰色；它用于重新加载模型，不用于控制本项目的运行时状态。

模型说明

单臂 URDF 是 RM75-B 运动链几何参数的唯一来源。导入的双臂 URDF 仅用于提供左右 安装变换；求解器不使用其中的镜像关节限位和固化的关节零位偏移。

导入的双臂 URDF 中，右侧基座的视觉原点与运动学原点相差约 1 mm。第一阶段采用 第一关节的运动学原点，并叠加文档规定的 240.5 mm 基座至第一关节偏移。

第二阶段不会把原始双臂 URDF 的镜像关节角直接交给 MuJoCo。MJCF 的左右关节均采用 标准 RealMan 弧度定义；原始双臂模型只作为安装和视觉来源。当前场景没有 actuator， 通过设置 qpos 并调用 mj_forward() 实现确定性的运动学播放。