247 lines
8.8 KiB
Markdown
247 lines
8.8 KiB
Markdown
# 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` 中：
|
||
|
||
```bash
|
||
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/` 的目录：
|
||
|
||
```bash
|
||
export REALMAN_SDK_ROOT=/path/to/RM_API2/Python
|
||
```
|
||
|
||
## 公共 API
|
||
|
||
```python
|
||
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 场景：
|
||
|
||
```python
|
||
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()
|
||
```
|
||
|
||
后端无关双臂控制：
|
||
|
||
```python
|
||
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 状态，因此
|
||
只能由一个控制线程使用。未来的双臂控制器应为每条机械臂分别持有一对实例。
|
||
|
||
## 验证
|
||
|
||
运行快速单元测试：
|
||
|
||
```bash
|
||
REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
|
||
conda run -n qp python -m pytest -q
|
||
```
|
||
|
||
运行完整、严格的第一阶段基准测试：
|
||
|
||
```bash
|
||
REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
|
||
conda run -n qp rm75-stage1-validate
|
||
```
|
||
|
||
如需进行小规模冒烟测试，请添加 `--quick`。报告将写入 `artifacts/stage1/`，并且
|
||
该目录有意设置为由 Git 忽略。
|
||
|
||
验收标准和最近一次完整结果请参见
|
||
[STAGE1_VALIDATION.md](STAGE1_VALIDATION.md)。
|
||
|
||
运行第二阶段完整 headless 验证：
|
||
|
||
```bash
|
||
REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
|
||
conda run -n qp rm75-stage2-validate --arm left
|
||
```
|
||
|
||
启动双臂实时可视化，只驱动左臂：
|
||
|
||
```bash
|
||
conda run -n qp rm75-stage2-demo --arm left --trajectory combined
|
||
```
|
||
|
||
移动到指定目标点。目标位置位于受控机械臂基座坐标系，单位为米；未提供 RPY 时保持
|
||
起始姿态：
|
||
|
||
```bash
|
||
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
|
||
```
|
||
|
||
也可用弧度指定目标姿态：
|
||
|
||
```bash
|
||
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
|
||
```
|
||
|
||
手动拖动受控机械臂：
|
||
|
||
```bash
|
||
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](STAGE2_VALIDATION.md)。
|
||
|
||
运行第三阶段完整 headless 验证：
|
||
|
||
```bash
|
||
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：
|
||
|
||
```bash
|
||
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 运行可使用：
|
||
|
||
```bash
|
||
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](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`，将双臂恢复到本次启动时求得的
|
||
初始关节状态；也可调用：
|
||
|
||
```bash
|
||
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()` 实现确定性的运动学播放。
|