YikaiFu-cart/acRealman_xr
Template
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
0d7e30cb7be1acc9c209fe219bdb57e9033bd6f8
acRealman_xr/ik_qp
History

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。

运行过程中可在 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() 实现确定性的运动学播放。