Implement dual-arm teleoperation with RM75 QP controller and MuJoCo backend
This commit is contained in:
@@ -0,0 +1,23 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(rm75_ik)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(ament_cmake_python REQUIRED)
|
||||
|
||||
ament_python_install_package(rm75_ik PACKAGE_DIR src/rm75_ik)
|
||||
|
||||
install(FILES
|
||||
kine_ctrl/urdf_rm75/RM75-B.urdf
|
||||
models/dual_arm_mujoco_fixed.urdf
|
||||
DESTINATION share/${PROJECT_NAME}/models
|
||||
)
|
||||
|
||||
install(DIRECTORY kine_ctrl/urdf_rm75/meshes/
|
||||
DESTINATION share/${PROJECT_NAME}/models/rm75_meshes
|
||||
)
|
||||
|
||||
install(DIRECTORY models/dual_arm_obj/
|
||||
DESTINATION share/${PROJECT_NAME}/models/dual_arm_obj
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+138
-6
@@ -1,7 +1,8 @@
|
||||
# RM75-B 第一阶段运动学与 QP IK
|
||||
# RM75-B 运动学、QP IK 与 MuJoCo 验证
|
||||
|
||||
本目录是一个独立的离线 Python 包，用于验证 RM75-B 的运动学与逆运动学。它不接入
|
||||
ROS 2 遥操作控制链路，也不会建立机器人连接。
|
||||
本目录是独立 Python 算法包，用于验证 RM75-B 的运动学、逆运动学和后端无关遥操作
|
||||
控制。核心包不依赖 ROS 2，也不会建立真实机器人连接；第三阶段由 `xr_rm_teleop` 提供
|
||||
薄 ROS 消息适配层并使用 MuJoCo 后端。
|
||||
|
||||
第一阶段包含：
|
||||
|
||||
@@ -13,7 +14,23 @@ ROS 2 遥操作控制链路，也不会建立机器人连接。
|
||||
- 由两份标准单臂模型组成的双臂装配模型。
|
||||
- 可生成 JSON、CSV 和 Markdown 报告的确定性验证流程。
|
||||
|
||||
MuJoCo、MJCF、碰撞规避和真实机器人控制明确不在本阶段范围内。
|
||||
第二阶段在完整双臂场景中使用第一阶段求解器驱动一侧机械臂，包含：
|
||||
|
||||
- 由标准单臂 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` 协议。
|
||||
|
||||
碰撞规避、动力学伺服和真实机器人控制仍不在当前范围内。
|
||||
|
||||
## 环境
|
||||
|
||||
@@ -25,8 +42,8 @@ conda env update -f environment.yml
|
||||
conda run -n qp python -m pip install -e . --no-deps
|
||||
```
|
||||
|
||||
RealMan API2 SDK 是外部二进制依赖，不会复制到本包中。请为验证程序指定包含
|
||||
`Robotic_Arm/` 的目录：
|
||||
环境文件会从 PyPI 安装 `Robotic_Arm 1.1.5`，通常无需额外设置。若需要强制使用
|
||||
本地 API2 SDK，可为验证程序指定包含 `Robotic_Arm/` 的目录：
|
||||
|
||||
```bash
|
||||
export REALMAN_SDK_ROOT=/path/to/RM_API2/Python
|
||||
@@ -52,6 +69,28 @@ 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`。不得将失败或未经验证的结果发送给
|
||||
机器人。
|
||||
|
||||
@@ -80,6 +119,95 @@ REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
|
||||
验收标准和最近一次完整结果请参见
|
||||
[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)。
|
||||
|
||||
## 模型说明
|
||||
|
||||
单臂 URDF 是 RM75-B 运动链几何参数的唯一来源。导入的双臂 URDF 仅用于提供左右
|
||||
@@ -87,3 +215,7 @@ REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
|
||||
|
||||
导入的双臂 URDF 中，右侧基座的视觉原点与运动学原点相差约 1 mm。第一阶段采用
|
||||
第一关节的运动学原点，并叠加文档规定的 240.5 mm 基座至第一关节偏移。
|
||||
|
||||
第二阶段不会把原始双臂 URDF 的镜像关节角直接交给 MuJoCo。MJCF 的左右关节均采用
|
||||
标准 RealMan 弧度定义；原始双臂模型只作为安装和视觉来源。当前场景没有 actuator，
|
||||
通过设置 `qpos` 并调用 `mj_forward()` 实现确定性的运动学播放。
|
||||
|
||||
+36
-40
@@ -1,62 +1,58 @@
|
||||
# RM75-B Stage-1 Validation Record
|
||||
# RM75-B 第一阶段验证记录
|
||||
|
||||
Date: 2026-06-29
|
||||
Random seed: `20260629`
|
||||
RealMan API2 C API: `v1.1.5`
|
||||
- 日期：2026-06-29
|
||||
- 随机种子：`20260629`
|
||||
- RealMan API2 C API 版本：`v1.1.5`
|
||||
|
||||
## Acceptance Result
|
||||
## 验收结果
|
||||
|
||||
The complete strict benchmark passed every required check with zero recorded
|
||||
failure samples.
|
||||
完整的严格基准测试通过了全部必需检查，未记录到失败样本。
|
||||
|
||||
| Check | Samples | Result |
|
||||
| 检查项 | 样本数 | 结果 |
|
||||
|---|---:|---:|
|
||||
| Physical-limit FK | 10,000 | PASS |
|
||||
| Teleop-limit FK | 10,000 | PASS |
|
||||
| Algo finite-difference Jacobian | 200 | PASS |
|
||||
| Physical-limit near-seed IK | 1,000 / 1,000 | PASS |
|
||||
| Teleop-limit near-seed IK | 1,000 / 1,000 | PASS |
|
||||
| Continuous IK | 10,000 / 10,000 | PASS |
|
||||
| Eight-seed global recovery | 200 / 200 | PASS |
|
||||
| Documented singularity families | 12 | PASS |
|
||||
| Dual-arm assembly FK | 100 per arm | PASS |
|
||||
| Project tool-frame FK | 100 per tool | PASS |
|
||||
| 物理限位 FK | 10,000 | 通过 |
|
||||
| 遥操作限位 FK | 10,000 | 通过 |
|
||||
| Algo 有限差分雅可比矩阵 | 200 | 通过 |
|
||||
| 物理限位邻近种子 IK | 1,000 / 1,000 | 通过 |
|
||||
| 遥操作限位邻近种子 IK | 1,000 / 1,000 | 通过 |
|
||||
| 连续轨迹 IK | 10,000 / 10,000 | 通过 |
|
||||
| 八种子全局恢复 | 200 / 200 | 通过 |
|
||||
| 文档所列奇异位形族 | 12 | 通过 |
|
||||
| 双臂装配模型 FK | 每条机械臂 100 | 通过 |
|
||||
| 项目工具坐标系 FK | 每个工具 100 | 通过 |
|
||||
|
||||
Key measurements:
|
||||
关键测量结果：
|
||||
|
||||
- Maximum physical-limit FK error: `0.003868 mm`, `0.001027 deg`.
|
||||
- Maximum teleop-limit FK error: `0.003681 mm`, `0.000957 deg`.
|
||||
- Maximum Jacobian relative/absolute error: `6.97e-5` / `1.70e-4`.
|
||||
- Near-seed IK P99/max time: `2.44 ms` / `7.43 ms`.
|
||||
- Maximum continuous joint step: `0.003216 rad` (`0.184 deg`).
|
||||
- Random single-seed IK success rate: `74%` (diagnostic only).
|
||||
- Eight-seed recovery success rate: `100%`.
|
||||
- Right dual-arm visual/kinematic origin difference: `1.0000004 mm`.
|
||||
- 物理限位 FK 最大误差：`0.003868 mm`、`0.001027 deg`。
|
||||
- 遥操作限位 FK 最大误差：`0.003681 mm`、`0.000957 deg`。
|
||||
- 雅可比矩阵最大相对误差/绝对误差：`6.97e-5` / `1.70e-4`。
|
||||
- 邻近种子 IK 的 P99/最大耗时：`2.44 ms` / `7.43 ms`。
|
||||
- 最大连续关节步长：`0.003216 rad`（`0.184 deg`）。
|
||||
- 随机单种子 IK 成功率：`74%`（仅用于诊断）。
|
||||
- 八种子恢复成功率：`100%`。
|
||||
- 双臂模型右侧视觉原点与运动学原点之差：`1.0000004 mm`。
|
||||
|
||||
## Error Definitions
|
||||
## 误差定义
|
||||
|
||||
Position error:
|
||||
位置误差：
|
||||
|
||||
```text
|
||||
||p_result - p_target||
|
||||
```
|
||||
|
||||
Orientation error:
|
||||
姿态误差：
|
||||
|
||||
```text
|
||||
||log(R_result^T R_target)||
|
||||
```
|
||||
|
||||
IK success is accepted only after applying RealMan Algo FK to the returned joint
|
||||
configuration. Pinocchio does not validate its own IK result.
|
||||
仅当对返回的关节配置应用 RealMan Algo FK 并通过检查后，才接受该 IK 结果为成功。
|
||||
Pinocchio 不用于验证其自身产生的 IK 结果。
|
||||
|
||||
The validator asks the numerical solver to converge to `0.9 mm / 0.09 deg`, then
|
||||
applies the independent acceptance limits `1 mm / 0.1 deg`. This guard band
|
||||
prevents boundary false positives caused by the small measured model difference.
|
||||
验证程序要求数值求解器收敛至 `0.9 mm / 0.09 deg`，随后应用独立的验收限值
|
||||
`1 mm / 0.1 deg`。该保护带用于避免测得的微小模型差异在边界处造成假阳性。
|
||||
|
||||
## Boundaries
|
||||
|
||||
This result validates geometry, FK, local Jacobians, numerical IK and fixed tool
|
||||
or mounting transforms. It does not validate dynamics, self-collision,
|
||||
environment collision, torque limits, communication latency or hardware safety.
|
||||
## 验证边界
|
||||
|
||||
本结果验证了几何模型、FK、局部坐标雅可比矩阵、数值 IK，以及固定的工具变换或
|
||||
安装变换。它不验证动力学、自碰撞、环境碰撞、力矩限制、通信延迟或硬件安全性。
|
||||
|
||||
@@ -0,0 +1,75 @@
|
||||
# RM75-B 第二阶段 MuJoCo 验证记录
|
||||
|
||||
日期：2026-06-30
|
||||
随机种子：`20260630`
|
||||
受控机械臂：`left`
|
||||
固定机械臂：`right`
|
||||
MuJoCo：`3.10.0`
|
||||
RealMan API2 C API：`v1.1.5`
|
||||
|
||||
## 验收结果
|
||||
|
||||
完整 headless 基准全部通过，记录失败样本为 0。
|
||||
|
||||
| 检查项 | 样本 | 结果 |
|
||||
|---|---:|---:|
|
||||
| 规范化双臂模型结构 | 14 轴、27 个 mesh | PASS |
|
||||
| MuJoCo/Pinocchio/Algo FK | 10,000 | PASS |
|
||||
| 单点 IK 至 MuJoCo | 1,000 / 1,000 | PASS |
|
||||
| 连续 IK | 10,000 / 10,000 | PASS |
|
||||
| 预定义轨迹类型 | 7 / 7 | PASS |
|
||||
| RGB 与 segmentation | 3 帧 | PASS |
|
||||
| 固定右臂关节变化 | 0 rad | PASS |
|
||||
|
||||
关键测量值：
|
||||
|
||||
- MuJoCo/Pinocchio 最大误差：`5.69e-13 m`、`1.84e-12 rad`。
|
||||
- MuJoCo/RealMan Algo 最大误差：`0.003921 mm`、`0.001065 deg`。
|
||||
- 单点 IK 成功率：`100%`。
|
||||
- 单点 IK P99/最大耗时：`2.54 ms` / `4.52 ms`。
|
||||
- 连续 IK 成功率：`100%`。
|
||||
- 连续 IK 最大位置/姿态误差：`0.9000 mm`、`0.0900 deg`。
|
||||
- 连续 IK 最大关节步长：`0.002912 rad`（`0.167 deg`）。
|
||||
- 固定右臂最大 qpos 变化：`0 rad`。
|
||||
|
||||
## 场景定义
|
||||
|
||||
规范化 MJCF 使用第一阶段 RM75-B URDF 生成左右两条标准 7 轴链。上传双臂压缩包提供：
|
||||
|
||||
- 左右机械臂安装变换；
|
||||
- 平台、挂架和夹爪视觉；
|
||||
- 19 个 OBJ 资源，全部由 MuJoCo 编译检查。
|
||||
|
||||
默认左臂从 `[0,30,0,60,0,60,0] deg` 开始运动。右臂固定在项目配置中的初始姿态：
|
||||
|
||||
```text
|
||||
[-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67] deg
|
||||
```
|
||||
|
||||
原始双臂 URDF 的镜像限位和零位偏置不进入求解或控制接口。
|
||||
|
||||
## 视觉验证
|
||||
|
||||
起点、中点和终点分别生成 RGB 与 segmentation 图像。每一帧均满足：
|
||||
|
||||
- RGB 像素方差大于阈值；
|
||||
- segmentation 中同时存在左右机械臂 geom；
|
||||
- 双臂前景未接触图像边界。
|
||||
|
||||
实时 viewer 使用 GLFW；自动验证使用 EGL。MuJoCo 3.10 的 passive viewer 使用显式
|
||||
`close()` 并等待渲染线程退出，避免 context-manager 关闭时的 GLFW 退出竞态。
|
||||
|
||||
Demo 另外提供两种交互方式：
|
||||
|
||||
- 指定受控臂基座坐标系中的目标位置/姿态，显示最终目标 marker，经过 SE(3) 插值和逐点
|
||||
QP IK 后播放完整运动，并输出 MuJoCo 回读误差。
|
||||
- `--manual-drag` 将受控臂切换为零重力、有阻尼的动力学步进，可在 Viewer 中选择连杆后
|
||||
使用 `Ctrl + 鼠标右键` 拖动；固定臂仍由程序锁定。
|
||||
|
||||
手动拖动是模型交互检查，不计入第一阶段 IK 或第二阶段算法成功率。
|
||||
|
||||
## 边界
|
||||
|
||||
本结果验证双臂场景中的单臂 IK、关节播放、末端位姿回读和视觉呈现。模型没有 actuator，
|
||||
visual geom 不参与碰撞，因此本阶段不验证动力学、重力补偿、位置伺服、碰撞检测、通信延迟
|
||||
或真实硬件安全。
|
||||
@@ -0,0 +1,79 @@
|
||||
# RM75-B 第三阶段双臂 QP 遥操验证记录
|
||||
|
||||
日期：2026-06-30
|
||||
随机种子：`20260630`
|
||||
快速验证机械臂：`right`
|
||||
MuJoCo：`3.10.0`
|
||||
RealMan API2 C API：`v1.1.5`
|
||||
|
||||
## 验收结果
|
||||
|
||||
完整 headless 基准全部通过，记录失败样本为 0。
|
||||
|
||||
| 检查项 | 样本 | 结果 |
|
||||
|---|---:|---:|
|
||||
| 右臂 MuJoCo/Pinocchio/Algo FK | 2,000 | PASS |
|
||||
| 右臂近邻 IK | 200 / 200 | PASS |
|
||||
| 右臂连续 IK | 1,000 / 1,000 | PASS |
|
||||
| 双臂工具 TCP 初始化 | 左右各 1 | PASS |
|
||||
| 双 grip 相对位姿控制 | 10 帧 | PASS |
|
||||
| RGB/segmentation 与双臂最终帧 | 7 张 | PASS |
|
||||
| ROS2 双臂 QP MuJoCo 启动冒烟测试 | 1 次 | PASS |
|
||||
|
||||
关键测量值：
|
||||
|
||||
- MuJoCo/Pinocchio 最大误差：`5.38e-13 m`、`1.52e-12 rad`。
|
||||
- MuJoCo/RealMan Algo 最大误差：`0.003721 mm`、`0.000964 deg`。
|
||||
- 右臂近邻 IK 成功率：`100%`，P99/最大耗时：`3.61 ms / 5.18 ms`。
|
||||
- 右臂连续 IK 成功率：`100%`，最大关节步长：`0.153 deg`。
|
||||
- grip 首帧左右关节变化均小于 `1e-10 rad`。
|
||||
|
||||
## 初始 TCP
|
||||
|
||||
`dual_arm_rm75.yaml` 的 `initial_tcp_pose` 被视为活动工具 TCP。左臂使用 `minisci`
|
||||
的 `190 mm` 工具变换，右臂使用 `omnipic` 的 `160 mm` 工具变换，再由 QP IK 求出
|
||||
初始关节角。
|
||||
|
||||
| 机械臂 | 位置残差 | 姿态残差 |
|
||||
|---|---:|---:|
|
||||
| left | `0.226 mm` | `0.00714 deg` |
|
||||
| right | `0.422 mm` | `0.01259 deg` |
|
||||
|
||||
配置中的旧 `initial_joint_pose` 无法在标准 RM75-B 模型中复现这些 TCP：计入工具后，
|
||||
左、右位置误差约为 `123 mm`、`95 mm`。因此它们只进入诊断报告，不用于 MuJoCo 初始化。
|
||||
|
||||
## 控制边界
|
||||
|
||||
独立核心按以下顺序处理每个控制周期：
|
||||
|
||||
```text
|
||||
XrController-compatible sample
|
||||
-> grip 起点锁定与左右坐标映射
|
||||
-> TCP 工作空间、滤波和速度限制
|
||||
-> 工具 TCP 到法兰变换
|
||||
-> Pinocchio SE(3) + OSQP IK
|
||||
-> 关节限位与关节速度限制
|
||||
-> RobotBackend.command_joint_positions()
|
||||
```
|
||||
|
||||
正常松开 grip 只停止对应侧；任一活动臂输入超时、IK 失败或后端异常都会使双臂进入
|
||||
FAULT 并共同停止。恢复前必须收到左右两侧新的 grip 释放消息。
|
||||
|
||||
## ROS2 启动验证
|
||||
|
||||
在 `qp` Conda 环境和已构建的 ROS2 Humble 工作空间中执行：
|
||||
|
||||
```bash
|
||||
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 show_viewer:=false
|
||||
```
|
||||
|
||||
验证中使用 `mujoco_gl` 的默认值 `glfw`，无需在命令行显式传入。UDP receiver 成功监听
|
||||
`0.0.0.0:15000`，左右臂分别以 `0.226 mm`、`0.422 mm` 的位置残差完成初始化，随后
|
||||
双臂控制节点进入 `dual-arm QP teleop ready` 状态。该测试只验证节点启动和 MuJoCo
|
||||
初始化，不包含 XR 数据输入、持续遥操作或真实机械臂行为。
|
||||
|
||||
第三阶段只实现 `MujocoRobot`。`RealmanRobot` 仅由 `RobotBackend` 协议约束，尚未实现，
|
||||
也未进行真实机械臂测试。本阶段不验证碰撞规避、动力学跟踪或硬件安全。
|
||||
@@ -6,9 +6,13 @@ dependencies:
|
||||
- numpy=1.23.5
|
||||
- scipy=1.10.1
|
||||
- pinocchio=2.6.20
|
||||
- catkin_pkg
|
||||
- empy=3.3.4
|
||||
- pip
|
||||
- pip:
|
||||
- osqp==0.6.2.post8
|
||||
- PyYAML==6.0.3
|
||||
- pytest==7.4.4
|
||||
|
||||
- mujoco==3.10.0
|
||||
- Pillow==12.2.0
|
||||
- Robotic_Arm==1.1.5
|
||||
|
||||
+49
-265
@@ -1,297 +1,81 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Pure Position Control for MuJoCo - No velocity commands, no forces
|
||||
Direct joint position control with smoothing
|
||||
"""
|
||||
"""Compatibility adapter for the original MuJoCo controller import path."""
|
||||
|
||||
from time import sleep
|
||||
|
||||
import mujoco
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
from rm75_ik.mujoco_backend import DualArmMuJoCo
|
||||
|
||||
|
||||
class MuJoCoPositionController:
|
||||
"""
|
||||
Pure position control - directly sets joint positions
|
||||
No velocity commands, no forces - completely stable
|
||||
"""
|
||||
"""Legacy facade backed by the threadless normalized dual-arm scene."""
|
||||
|
||||
def __init__(self, urdf_path="./urdf_rm75/RM75-B.urdf", smoothness=0.05, enable_viewer=True):
|
||||
"""
|
||||
Args:
|
||||
urdf_path: Path to URDF file
|
||||
smoothness: Motion smoothness (0.02=very smooth, 0.1=fast)
|
||||
enable_viewer: Show MuJoCo viewer
|
||||
"""
|
||||
# Load model
|
||||
self.model = mujoco.MjModel.from_xml_path(urdf_path)
|
||||
self.data = mujoco.MjData(self.model)
|
||||
|
||||
self.time_interval = 0.02
|
||||
|
||||
print(f'time interval: {self.model.opt.timestep}')
|
||||
|
||||
# Robot info
|
||||
self.n_joints = self.model.njnt
|
||||
|
||||
# Get joint limits
|
||||
self.joint_lower_limits = []
|
||||
self.joint_upper_limits = []
|
||||
for i in range(self.n_joints):
|
||||
self.joint_lower_limits.append(self.model.jnt_range[i, 0])
|
||||
self.joint_upper_limits.append(self.model.jnt_range[i, 1])
|
||||
|
||||
print(f"Loaded robot: {self.n_joints} joints")
|
||||
for i in range(self.n_joints):
|
||||
print(
|
||||
f" {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]")
|
||||
|
||||
# Target joint angles (in radians)
|
||||
self.target_joints = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Smoothing factor (0-1, lower = smoother)
|
||||
self.smoothness = smoothness
|
||||
|
||||
# Thread safety
|
||||
self.command_lock = threading.Lock()
|
||||
self.feedback_lock = threading.Lock()
|
||||
self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
self.max_ang_inc = 0.02
|
||||
|
||||
# Control flags
|
||||
self.running = False
|
||||
self.simulation_thread = None
|
||||
|
||||
# Viewer
|
||||
self.viewer = None
|
||||
if enable_viewer:
|
||||
try:
|
||||
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
|
||||
print("Viewer launched")
|
||||
except Exception as e:
|
||||
print(f"Viewer warning: {e}")
|
||||
self.start()
|
||||
def __init__(
|
||||
self,
|
||||
urdf_path=None,
|
||||
smoothness=0.05,
|
||||
enable_viewer=True,
|
||||
controlled_arm="left",
|
||||
):
|
||||
del urdf_path, smoothness
|
||||
self.backend = DualArmMuJoCo(controlled_arm=controlled_arm)
|
||||
self.controlled_arm = controlled_arm
|
||||
self.viewer = (
|
||||
mujoco.viewer.launch_passive(self.backend.model, self.backend.data)
|
||||
if enable_viewer
|
||||
else None
|
||||
)
|
||||
|
||||
def start(self):
|
||||
"""Start the simulation thread"""
|
||||
if self.running:
|
||||
return
|
||||
|
||||
self.running = True
|
||||
self.simulation_thread = threading.Thread(target=self._simulation_loop, daemon=True)
|
||||
self.simulation_thread.start()
|
||||
print("Simulation thread started")
|
||||
return None
|
||||
|
||||
def stop(self):
|
||||
"""Stop the simulation thread"""
|
||||
self.running = False
|
||||
if self.simulation_thread:
|
||||
self.simulation_thread.join(timeout=2.0)
|
||||
if self.viewer:
|
||||
if self.viewer is not None:
|
||||
self.viewer.close()
|
||||
print("Simulation stopped")
|
||||
sleep(0.2)
|
||||
self.viewer = None
|
||||
|
||||
def send_command(self, joint_positions):
|
||||
"""
|
||||
Send target joint positions
|
||||
|
||||
Args:
|
||||
joint_positions: Array of target joint angles (radians)
|
||||
"""
|
||||
cmd = np.array(joint_positions[:self.n_joints], dtype=np.float64)
|
||||
|
||||
# Apply joint limits
|
||||
for i in range(self.n_joints):
|
||||
cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
|
||||
|
||||
with self.command_lock:
|
||||
self.target_joints = cmd
|
||||
self.backend.set_arm_configuration(
|
||||
self.controlled_arm, np.asarray(joint_positions, dtype=float)
|
||||
)
|
||||
if self.viewer is not None:
|
||||
self.viewer.sync()
|
||||
|
||||
def get_feedback(self):
|
||||
"""Get current joint positions"""
|
||||
with self.feedback_lock:
|
||||
return self.current_feedback_joint.copy()
|
||||
return self.backend.get_arm_configuration(self.controlled_arm)
|
||||
|
||||
def get_target(self):
|
||||
"""Get current target positions"""
|
||||
with self.command_lock:
|
||||
return self.target_joints.copy()
|
||||
|
||||
def _simulation_loop(self):
|
||||
"""
|
||||
Main simulation loop - PURE POSITION CONTROL
|
||||
No velocity commands, no forces - just direct position setting
|
||||
"""
|
||||
last_time = time.time()
|
||||
|
||||
# For smooth interpolation
|
||||
current_joints = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
while self.running:
|
||||
# Get target command
|
||||
with self.command_lock:
|
||||
target = self.target_joints.copy()
|
||||
|
||||
# Get current positions
|
||||
current_joints = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Smooth interpolation toward target
|
||||
# This creates natural motion without velocity commands
|
||||
alpha = self.smoothness
|
||||
|
||||
next_joints = current_joints + np.clip(alpha * (target - current_joints) , -self.max_ang_inc, self.max_ang_inc)
|
||||
|
||||
# DIRECT POSITION CONTROL - Set joint positions
|
||||
self.data.qpos[:self.n_joints] = next_joints
|
||||
|
||||
# IMPORTANT: Set velocities to zero to prevent physics from moving joints
|
||||
# This ensures pure kinematic control
|
||||
self.data.qvel[:self.n_joints] = 0
|
||||
|
||||
# Step physics (this will apply gravity, collisions, etc. to other bodies)
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
|
||||
# After step, ensure our joint positions are maintained
|
||||
# (Physics might have altered them slightly)
|
||||
self.data.qpos[:self.n_joints] = next_joints
|
||||
self.data.qvel[:self.n_joints] = 0
|
||||
|
||||
# Update feedback
|
||||
with self.feedback_lock:
|
||||
self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Sync viewer
|
||||
if self.viewer:
|
||||
self.viewer.sync()
|
||||
|
||||
# Maintain real-time speed
|
||||
elapsed = time.time() - last_time
|
||||
sleep_time = self.time_interval - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
last_time = time.time()
|
||||
return self.get_feedback()
|
||||
|
||||
def move_to_joints(self, target, duration=1.0):
|
||||
"""
|
||||
Move to target joints over specified duration
|
||||
|
||||
Args:
|
||||
target: Target joint joints
|
||||
duration: Time to complete movement (seconds)
|
||||
"""
|
||||
start_js = self.get_feedback()
|
||||
end_js = np.array(target[:self.n_joints])
|
||||
|
||||
# Apply limits
|
||||
for i in range(self.n_joints):
|
||||
end_js[i] = np.clip(end_js[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
|
||||
|
||||
n_steps = int(duration / self.time_interval)
|
||||
|
||||
print(f" Moving over {duration}s ({n_steps} steps)")
|
||||
|
||||
for step in range(n_steps):
|
||||
alpha = (step + 1) / n_steps
|
||||
# Use easing for smoother motion
|
||||
ease_alpha = 1 - (1 - alpha) ** 2 # Quadratic ease-out
|
||||
current_target = start_js + ease_alpha * (end_js - start_js)
|
||||
self.send_command(current_target)
|
||||
time.sleep(self.time_interval)
|
||||
|
||||
# Ensure exact target
|
||||
self.send_command(end_js)
|
||||
time.sleep(0.1)
|
||||
start = self.get_feedback()
|
||||
target_q = np.asarray(target, dtype=float)
|
||||
points = max(2, int(round(duration * 90.0)))
|
||||
blend = 0.5 - 0.5 * np.cos(np.linspace(0.0, np.pi, points))
|
||||
trajectory = start[None, :] + blend[:, None] * (target_q - start)[None, :]
|
||||
self.backend.play_trajectory(
|
||||
trajectory,
|
||||
dt=duration / points,
|
||||
realtime=True,
|
||||
viewer=self.viewer,
|
||||
)
|
||||
|
||||
def wait_until_reached(self, tolerance=0.01, timeout=10.0):
|
||||
"""
|
||||
Wait until robot reaches target position
|
||||
|
||||
Args:
|
||||
tolerance: Position error tolerance (radians)
|
||||
timeout: Maximum wait time (seconds)
|
||||
"""
|
||||
start_time = time.time()
|
||||
|
||||
while time.time() - start_time < timeout:
|
||||
current = self.get_feedback()
|
||||
target = self.get_target()
|
||||
error = np.max(np.abs(target - current))
|
||||
|
||||
if error < tolerance:
|
||||
return True
|
||||
|
||||
time.sleep(0.01)
|
||||
|
||||
return False
|
||||
del tolerance, timeout
|
||||
return True
|
||||
|
||||
def print_state(self):
|
||||
"""Print current robot state"""
|
||||
joints = self.get_feedback()
|
||||
target = self.get_target()
|
||||
print("Current joints (rad):", [f"{p:.3f}" for p in joints], "...")
|
||||
print("Target joints (rad): ", [f"{t:.3f}" for t in target], "...")
|
||||
print("Current joints (rad):", self.get_feedback().tolist())
|
||||
|
||||
|
||||
# Demo
|
||||
def demo_position_control():
|
||||
"""Demonstrate pure position control"""
|
||||
|
||||
urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
|
||||
|
||||
if not Path(urdf_path).exists():
|
||||
print(f"Error: URDF not found at {urdf_path}")
|
||||
return
|
||||
|
||||
print("=" * 60)
|
||||
print("Pure Position Control Demo")
|
||||
print("=" * 60)
|
||||
|
||||
# Create controller
|
||||
robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
|
||||
robot.start()
|
||||
time.sleep(1)
|
||||
|
||||
print("\n[Test 1] Move joint 1 to 45 degrees")
|
||||
robot.send_command([0.785, 0, 0, 0, 0, 0, 0])
|
||||
robot.wait_until_reached()
|
||||
robot.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 2] Move joint 2 to -30 degrees")
|
||||
robot.send_command([0, -0.524, 0, 0, 0, 0, 0])
|
||||
robot.wait_until_reached()
|
||||
robot.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 3] Move multiple joints simultaneously")
|
||||
robot.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
|
||||
robot.wait_until_reached()
|
||||
robot.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 4] Return home")
|
||||
robot.send_command([0, 0, 0, 0, 0, 0, 0])
|
||||
robot.wait_until_reached()
|
||||
robot.print_state()
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print("✓ All tests passed! Robot is stable and controllable.")
|
||||
print("=" * 60)
|
||||
print("\nInteractive mode - close viewer to exit")
|
||||
|
||||
try:
|
||||
while robot.viewer and robot.viewer.is_running():
|
||||
time.sleep(0.1)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
robot.stop()
|
||||
|
||||
from rm75_ik.stage2_demo import main
|
||||
|
||||
return main([])
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
demo_position_control()
|
||||
raise SystemExit(demo_position_control())
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rm75_ik</name>
|
||||
<version>0.3.0</version>
|
||||
<description>Independent RM75 Pinocchio, OSQP and MuJoCo control package.</description>
|
||||
<maintainer email="user@example.com">Yikai Fu</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>ament_cmake_python</buildtool_depend>
|
||||
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-scipy</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
|
||||
|
||||
[project]
|
||||
name = "rm75-ik-qp"
|
||||
version = "0.1.0"
|
||||
version = "0.3.0"
|
||||
description = "Validated Pinocchio and OSQP inverse kinematics for RealMan RM75-B"
|
||||
readme = "README.md"
|
||||
requires-python = "==3.10.*"
|
||||
@@ -14,6 +14,9 @@ dependencies = [
|
||||
"osqp==0.6.2.post8",
|
||||
"pin==2.6.20",
|
||||
"PyYAML==6.0.3",
|
||||
"mujoco==3.10.0",
|
||||
"Pillow==12.2.0",
|
||||
"Robotic_Arm==1.1.5",
|
||||
]
|
||||
|
||||
[project.optional-dependencies]
|
||||
@@ -21,10 +24,13 @@ test = ["pytest==7.4.4"]
|
||||
|
||||
[project.scripts]
|
||||
rm75-stage1-validate = "rm75_ik.cli:main"
|
||||
rm75-stage2-validate = "rm75_ik.stage2_cli:main"
|
||||
rm75-stage2-demo = "rm75_ik.stage2_demo:main"
|
||||
rm75-stage3-validate = "rm75_ik.stage3_cli:main"
|
||||
|
||||
[tool.setuptools]
|
||||
package-dir = {"" = "src"}
|
||||
data-files = {"share/rm75_ik/models" = ["kine_ctrl/urdf_rm75/RM75-B.urdf", "models/dual_arm_mujoco_fixed.urdf"]}
|
||||
data-files = {"share/rm75_ik/models" = ["kine_ctrl/urdf_rm75/RM75-B.urdf", "models/dual_arm_mujoco_fixed.urdf"], "share/rm75_ik/models/rm75_meshes" = ["kine_ctrl/urdf_rm75/meshes/*.STL"], "share/rm75_ik/models/dual_arm_obj" = ["models/dual_arm_obj/*.obj"]}
|
||||
|
||||
[tool.setuptools.packages.find]
|
||||
where = ["src"]
|
||||
@@ -32,4 +38,3 @@ where = ["src"]
|
||||
[tool.pytest.ini_options]
|
||||
testpaths = ["tests"]
|
||||
addopts = "-ra"
|
||||
|
||||
|
||||
@@ -1,7 +1,17 @@
|
||||
from .dual_arm import DualArmAssembly, DualArmMounts, load_dual_arm_mounts
|
||||
from .kinematics import RM75Kinematics, default_urdf_path, pose_errors, validate_se3
|
||||
from .mujoco_model import build_normalized_dual_mjcf
|
||||
from .realman_reference import RealManFkReference
|
||||
from .solver import RM75IkSolver, deterministic_recovery_seeds
|
||||
from .robot_backend import DualArmJointState, RobotBackend
|
||||
from .teleop_config import ArmTeleopProfile, load_dual_arm_profiles
|
||||
from .teleop_control import (
|
||||
ControllerSample,
|
||||
ControlCycleResult,
|
||||
DualArmQpTeleopController,
|
||||
RelativePoseMapper,
|
||||
SafetyState,
|
||||
)
|
||||
from .types import (
|
||||
IkOptions,
|
||||
IkResult,
|
||||
@@ -14,21 +24,49 @@ from .types import (
|
||||
|
||||
__all__ = [
|
||||
"DualArmAssembly",
|
||||
"DualArmJointState",
|
||||
"DualArmMounts",
|
||||
"DualArmMuJoCo",
|
||||
"DualArmQpTeleopController",
|
||||
"ControllerSample",
|
||||
"ControlCycleResult",
|
||||
"IkOptions",
|
||||
"IkResult",
|
||||
"IkStatus",
|
||||
"JointLimits",
|
||||
"InitialPoseDiagnostic",
|
||||
"MujocoRobot",
|
||||
"PlaybackResult",
|
||||
"RM75IkSolver",
|
||||
"RM75Kinematics",
|
||||
"RealManFkReference",
|
||||
"RelativePoseMapper",
|
||||
"RobotBackend",
|
||||
"SafetyState",
|
||||
"ArmTeleopProfile",
|
||||
"default_urdf_path",
|
||||
"build_normalized_dual_mjcf",
|
||||
"deterministic_recovery_seeds",
|
||||
"joint_limit_profile",
|
||||
"load_dual_arm_mounts",
|
||||
"load_dual_arm_profiles",
|
||||
"physical_joint_limits",
|
||||
"pose_errors",
|
||||
"teleop_joint_limits",
|
||||
"validate_se3",
|
||||
]
|
||||
|
||||
|
||||
def __getattr__(name):
|
||||
if name in {"DualArmMuJoCo", "PlaybackResult"}:
|
||||
from .mujoco_backend import DualArmMuJoCo, PlaybackResult
|
||||
|
||||
return {"DualArmMuJoCo": DualArmMuJoCo, "PlaybackResult": PlaybackResult}[name]
|
||||
if name in {"MujocoRobot", "InitialPoseDiagnostic"}:
|
||||
from .mujoco_robot import InitialPoseDiagnostic, MujocoRobot
|
||||
|
||||
return {
|
||||
"MujocoRobot": MujocoRobot,
|
||||
"InitialPoseDiagnostic": InitialPoseDiagnostic,
|
||||
}[name]
|
||||
raise AttributeError(name)
|
||||
|
||||
@@ -21,15 +21,22 @@ def default_dual_source_path() -> Path:
|
||||
)
|
||||
if source_path.is_file():
|
||||
return source_path
|
||||
installed_path = (
|
||||
installed_candidates = [
|
||||
Path(sysconfig.get_path("data"))
|
||||
/ "share"
|
||||
/ "rm75_ik"
|
||||
/ "models"
|
||||
/ "dual_arm_mujoco_fixed.urdf"
|
||||
)
|
||||
if installed_path.is_file():
|
||||
return installed_path
|
||||
]
|
||||
resolved = Path(__file__).resolve()
|
||||
if len(resolved.parents) > 4:
|
||||
installed_candidates.append(
|
||||
resolved.parents[4]
|
||||
/ "share/rm75_ik/models/dual_arm_mujoco_fixed.urdf"
|
||||
)
|
||||
for installed_path in installed_candidates:
|
||||
if installed_path.is_file():
|
||||
return installed_path
|
||||
raise FileNotFoundError("dual_arm_mujoco_fixed.urdf was not found")
|
||||
|
||||
|
||||
@@ -129,4 +136,3 @@ class DualArmAssembly:
|
||||
if arm == "right":
|
||||
return self.mounts.right_base * local
|
||||
raise ValueError("arm must be 'left' or 'right'")
|
||||
|
||||
|
||||
@@ -23,15 +23,21 @@ def default_urdf_path() -> Path:
|
||||
)
|
||||
if source_path.is_file():
|
||||
return source_path
|
||||
installed_path = (
|
||||
installed_candidates = [
|
||||
Path(sysconfig.get_path("data"))
|
||||
/ "share"
|
||||
/ "rm75_ik"
|
||||
/ "models"
|
||||
/ "RM75-B.urdf"
|
||||
)
|
||||
if installed_path.is_file():
|
||||
return installed_path
|
||||
]
|
||||
resolved = Path(__file__).resolve()
|
||||
if len(resolved.parents) > 4:
|
||||
installed_candidates.append(
|
||||
resolved.parents[4] / "share/rm75_ik/models/RM75-B.urdf"
|
||||
)
|
||||
for installed_path in installed_candidates:
|
||||
if installed_path.is_file():
|
||||
return installed_path
|
||||
raise FileNotFoundError("RM75-B.urdf was not found in source or installed data")
|
||||
|
||||
|
||||
@@ -121,4 +127,3 @@ class RM75Kinematics:
|
||||
pin.ReferenceFrame.LOCAL,
|
||||
)
|
||||
return np.asarray(jacobian).copy()
|
||||
|
||||
|
||||
@@ -0,0 +1,247 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from time import perf_counter, sleep
|
||||
from typing import Iterable, Optional
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
|
||||
from .kinematics import validate_se3
|
||||
from .mujoco_model import build_normalized_dual_mjcf
|
||||
from .types import JointLimits, physical_joint_limits
|
||||
|
||||
|
||||
CONTROLLED_HOME_Q_RAD = np.deg2rad([0.0, 30.0, 0.0, 60.0, 0.0, 60.0, 0.0])
|
||||
PROJECT_INITIAL_Q_RAD = {
|
||||
"left": np.deg2rad([-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]),
|
||||
"right": np.deg2rad([-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]),
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PlaybackResult:
|
||||
samples: int
|
||||
elapsed_sec: float
|
||||
max_joint_step_rad: float
|
||||
final_flange_pose: pin.SE3
|
||||
|
||||
|
||||
class DualArmMuJoCo:
|
||||
"""Threadless, kinematic MuJoCo backend for the normalized dual RM75 scene."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
controlled_arm: str = "left",
|
||||
inactive_q_rad: Optional[np.ndarray] = None,
|
||||
limits: Optional[JointLimits] = None,
|
||||
) -> None:
|
||||
if controlled_arm not in {"left", "right"}:
|
||||
raise ValueError("controlled_arm must be 'left' or 'right'")
|
||||
self.controlled_arm = controlled_arm
|
||||
self.inactive_arm = "right" if controlled_arm == "left" else "left"
|
||||
self.limits = limits or physical_joint_limits()
|
||||
self.mjcf_xml, self.assets = build_normalized_dual_mjcf()
|
||||
self.model = mujoco.MjModel.from_xml_string(self.mjcf_xml, self.assets)
|
||||
self.data = mujoco.MjData(self.model)
|
||||
self._qpos_addresses = {
|
||||
arm: np.array(
|
||||
[
|
||||
self.model.jnt_qposadr[
|
||||
mujoco.mj_name2id(
|
||||
self.model,
|
||||
mujoco.mjtObj.mjOBJ_JOINT,
|
||||
f"{arm}_joint_{joint_index}",
|
||||
)
|
||||
]
|
||||
for joint_index in range(1, 8)
|
||||
],
|
||||
dtype=int,
|
||||
)
|
||||
for arm in ("left", "right")
|
||||
}
|
||||
self._dof_addresses = {
|
||||
arm: np.array(
|
||||
[
|
||||
self.model.jnt_dofadr[
|
||||
mujoco.mj_name2id(
|
||||
self.model,
|
||||
mujoco.mjtObj.mjOBJ_JOINT,
|
||||
f"{arm}_joint_{joint_index}",
|
||||
)
|
||||
]
|
||||
for joint_index in range(1, 8)
|
||||
],
|
||||
dtype=int,
|
||||
)
|
||||
for arm in ("left", "right")
|
||||
}
|
||||
self._site_ids = {
|
||||
arm: mujoco.mj_name2id(
|
||||
self.model, mujoco.mjtObj.mjOBJ_SITE, f"{arm}_flange"
|
||||
)
|
||||
for arm in ("left", "right")
|
||||
}
|
||||
self._target_mocap_ids = {}
|
||||
for arm in ("left", "right"):
|
||||
marker_body_id = mujoco.mj_name2id(
|
||||
self.model, mujoco.mjtObj.mjOBJ_BODY, f"{arm}_target_marker"
|
||||
)
|
||||
mocap_id = int(self.model.body_mocapid[marker_body_id])
|
||||
if mocap_id < 0:
|
||||
raise ValueError(f"{arm}_target_marker is not a MuJoCo mocap body")
|
||||
self._target_mocap_ids[arm] = mocap_id
|
||||
|
||||
inactive = (
|
||||
PROJECT_INITIAL_Q_RAD[self.inactive_arm]
|
||||
if inactive_q_rad is None
|
||||
else np.asarray(inactive_q_rad, dtype=float)
|
||||
)
|
||||
self.set_arm_configuration(self.inactive_arm, inactive)
|
||||
self.set_arm_configuration(self.controlled_arm, CONTROLLED_HOME_Q_RAD)
|
||||
for arm in ("left", "right"):
|
||||
self.set_arm_target_marker(arm, self.get_flange_pose(arm))
|
||||
self._manual_inactive_q: Optional[np.ndarray] = None
|
||||
|
||||
@staticmethod
|
||||
def _validate_arm(arm: str) -> None:
|
||||
if arm not in {"left", "right"}:
|
||||
raise ValueError("arm must be 'left' or 'right'")
|
||||
|
||||
def _validate_q(self, q_rad: np.ndarray) -> np.ndarray:
|
||||
q = np.asarray(q_rad, dtype=float)
|
||||
if q.shape != (7,):
|
||||
raise ValueError(f"arm configuration must have shape (7,), got {q.shape}")
|
||||
if not np.all(np.isfinite(q)):
|
||||
raise ValueError("arm configuration must be finite")
|
||||
if not self.limits.contains(q):
|
||||
raise ValueError(f"configuration is outside {self.limits.name} joint limits")
|
||||
return q.copy()
|
||||
|
||||
def set_arm_configuration(self, arm: str, q_rad: np.ndarray) -> None:
|
||||
self._validate_arm(arm)
|
||||
q = self._validate_q(q_rad)
|
||||
self.data.qpos[self._qpos_addresses[arm]] = q
|
||||
self.data.qvel[:] = 0.0
|
||||
mujoco.mj_forward(self.model, self.data)
|
||||
|
||||
def set_dual_configuration(
|
||||
self, left_q_rad: np.ndarray, right_q_rad: np.ndarray
|
||||
) -> None:
|
||||
left_q = self._validate_q(left_q_rad)
|
||||
right_q = self._validate_q(right_q_rad)
|
||||
self.data.qpos[self._qpos_addresses["left"]] = left_q
|
||||
self.data.qpos[self._qpos_addresses["right"]] = right_q
|
||||
self.data.qvel[:] = 0.0
|
||||
mujoco.mj_forward(self.model, self.data)
|
||||
|
||||
def get_arm_configuration(self, arm: str) -> np.ndarray:
|
||||
self._validate_arm(arm)
|
||||
return self.data.qpos[self._qpos_addresses[arm]].copy()
|
||||
|
||||
def get_flange_pose(self, arm: str) -> pin.SE3:
|
||||
self._validate_arm(arm)
|
||||
site_id = self._site_ids[arm]
|
||||
return pin.SE3(
|
||||
self.data.site_xmat[site_id].reshape(3, 3).copy(),
|
||||
self.data.site_xpos[site_id].copy(),
|
||||
)
|
||||
|
||||
def set_target_marker(self, target_se3: pin.SE3) -> None:
|
||||
self.set_arm_target_marker(self.controlled_arm, target_se3)
|
||||
|
||||
def set_arm_target_marker(self, arm: str, target_se3: pin.SE3) -> None:
|
||||
self._validate_arm(arm)
|
||||
validate_se3(target_se3, "target_se3")
|
||||
quaternion = pin.Quaternion(target_se3.rotation)
|
||||
mocap_id = self._target_mocap_ids[arm]
|
||||
self.data.mocap_pos[mocap_id] = target_se3.translation
|
||||
self.data.mocap_quat[mocap_id] = [
|
||||
quaternion.w,
|
||||
quaternion.x,
|
||||
quaternion.y,
|
||||
quaternion.z,
|
||||
]
|
||||
mujoco.mj_forward(self.model, self.data)
|
||||
|
||||
def _validated_trajectory(self, q_trajectory: Iterable[np.ndarray]) -> np.ndarray:
|
||||
trajectory = np.asarray(list(q_trajectory), dtype=float)
|
||||
if trajectory.ndim != 2 or trajectory.shape[1:] != (7,):
|
||||
raise ValueError("q_trajectory must have shape (N, 7)")
|
||||
if trajectory.shape[0] == 0:
|
||||
raise ValueError("q_trajectory must contain at least one sample")
|
||||
if not np.all(np.isfinite(trajectory)):
|
||||
raise ValueError("q_trajectory must be finite")
|
||||
if np.any(trajectory < self.limits.lower) or np.any(
|
||||
trajectory > self.limits.upper
|
||||
):
|
||||
raise ValueError(f"trajectory exceeds {self.limits.name} joint limits")
|
||||
return trajectory
|
||||
|
||||
def play_trajectory(
|
||||
self,
|
||||
q_trajectory: Iterable[np.ndarray],
|
||||
dt: float = 1.0 / 90.0,
|
||||
realtime: bool = False,
|
||||
viewer=None,
|
||||
) -> PlaybackResult:
|
||||
if not np.isfinite(dt) or dt <= 0.0:
|
||||
raise ValueError("dt must be finite and positive")
|
||||
trajectory = self._validated_trajectory(q_trajectory)
|
||||
started = perf_counter()
|
||||
previous = self.get_arm_configuration(self.controlled_arm)
|
||||
max_step = 0.0
|
||||
for q in trajectory:
|
||||
frame_started = perf_counter()
|
||||
max_step = max(max_step, float(np.max(np.abs(q - previous))))
|
||||
self.set_arm_configuration(self.controlled_arm, q)
|
||||
previous = q
|
||||
if viewer is not None:
|
||||
viewer.sync()
|
||||
if realtime:
|
||||
remaining = dt - (perf_counter() - frame_started)
|
||||
if remaining > 0.0:
|
||||
sleep(remaining)
|
||||
return PlaybackResult(
|
||||
samples=len(trajectory),
|
||||
elapsed_sec=perf_counter() - started,
|
||||
max_joint_step_rad=max_step,
|
||||
final_flange_pose=self.get_flange_pose(self.controlled_arm),
|
||||
)
|
||||
|
||||
def configure_manual_drag(self, damping: float = 1.5) -> None:
|
||||
if not np.isfinite(damping) or damping <= 0.0:
|
||||
raise ValueError("manual-drag damping must be finite and positive")
|
||||
self.model.opt.gravity[:] = 0.0
|
||||
self.model.dof_damping[self._dof_addresses[self.controlled_arm]] = damping
|
||||
self.data.qvel[:] = 0.0
|
||||
self.data.xfrc_applied[:] = 0.0
|
||||
self._manual_inactive_q = self.get_arm_configuration(self.inactive_arm)
|
||||
mujoco.mj_forward(self.model, self.data)
|
||||
|
||||
def step_manual_drag(self) -> None:
|
||||
if self._manual_inactive_q is None:
|
||||
raise RuntimeError("configure_manual_drag() must be called first")
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
self.data.qpos[self._qpos_addresses[self.inactive_arm]] = self._manual_inactive_q
|
||||
self.data.qvel[self._dof_addresses[self.inactive_arm]] = 0.0
|
||||
mujoco.mj_forward(self.model, self.data)
|
||||
|
||||
def render(
|
||||
self,
|
||||
width: int = 1280,
|
||||
height: int = 720,
|
||||
*,
|
||||
segmentation: bool = False,
|
||||
) -> np.ndarray:
|
||||
if width <= 0 or height <= 0:
|
||||
raise ValueError("render dimensions must be positive")
|
||||
renderer = mujoco.Renderer(self.model, height=height, width=width)
|
||||
try:
|
||||
if segmentation:
|
||||
renderer.enable_segmentation_rendering()
|
||||
renderer.update_scene(self.data, camera="overview")
|
||||
return renderer.render().copy()
|
||||
finally:
|
||||
renderer.close()
|
||||
@@ -0,0 +1,413 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import xml.etree.ElementTree as ET
|
||||
from pathlib import Path
|
||||
from typing import Dict, Iterable, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
|
||||
from .dual_arm import default_dual_source_path, load_dual_arm_mounts
|
||||
from .kinematics import default_urdf_path
|
||||
from .types import physical_joint_limits
|
||||
|
||||
|
||||
def _numbers(text: str, expected: int) -> np.ndarray:
|
||||
values = np.fromstring(text, sep=" ", dtype=float)
|
||||
if values.shape != (expected,):
|
||||
raise ValueError(f"expected {expected} numeric values, got {text!r}")
|
||||
return values
|
||||
|
||||
|
||||
def _format(values: Iterable[float]) -> str:
|
||||
return " ".join(f"{float(value):.12g}" for value in values)
|
||||
|
||||
|
||||
def _quaternion_from_rpy(rpy: np.ndarray) -> np.ndarray:
|
||||
quaternion = pin.Quaternion(pin.rpy.rpyToMatrix(*rpy))
|
||||
return np.array([quaternion.w, quaternion.x, quaternion.y, quaternion.z])
|
||||
|
||||
|
||||
def _se3_attributes(transform: pin.SE3) -> Dict[str, str]:
|
||||
quaternion = pin.Quaternion(transform.rotation)
|
||||
return {
|
||||
"pos": _format(transform.translation),
|
||||
"quat": _format([quaternion.w, quaternion.x, quaternion.y, quaternion.z]),
|
||||
}
|
||||
|
||||
|
||||
def _origin_attributes(parent: ET.Element) -> Dict[str, str]:
|
||||
origin = parent.find("origin")
|
||||
if origin is None:
|
||||
return {"pos": "0 0 0", "quat": "1 0 0 0"}
|
||||
xyz = _numbers(origin.get("xyz", "0 0 0"), 3)
|
||||
rpy = _numbers(origin.get("rpy", "0 0 0"), 3)
|
||||
return {"pos": _format(xyz), "quat": _format(_quaternion_from_rpy(rpy))}
|
||||
|
||||
|
||||
def _mesh_asset_name(filename: str, prefix: str) -> str:
|
||||
stem = Path(filename).stem.lower()
|
||||
return f"{prefix}_{re.sub(r'[^a-z0-9_]+', '_', stem)}"
|
||||
|
||||
|
||||
def _dual_obj_directory(dual_source_path: Path) -> Path:
|
||||
source_candidate = dual_source_path.parent / "dual_arm_obj"
|
||||
if source_candidate.is_dir():
|
||||
return source_candidate
|
||||
raise FileNotFoundError(
|
||||
f"dual-arm OBJ directory was not found beside {dual_source_path}"
|
||||
)
|
||||
|
||||
|
||||
def _single_mesh_directory(single_urdf_path: Path) -> Path:
|
||||
candidates = (
|
||||
single_urdf_path.parent / "meshes",
|
||||
single_urdf_path.parent / "rm75_meshes",
|
||||
)
|
||||
for candidate in candidates:
|
||||
if candidate.is_dir():
|
||||
return candidate
|
||||
raise FileNotFoundError(
|
||||
f"RM75-B STL mesh directory was not found beside {single_urdf_path}"
|
||||
)
|
||||
|
||||
|
||||
def _add_inertial(body: ET.Element, link: ET.Element) -> None:
|
||||
inertial = link.find("inertial")
|
||||
if inertial is None:
|
||||
raise ValueError(f"link {link.get('name')!r} has no inertial data")
|
||||
mass_element = inertial.find("mass")
|
||||
inertia_element = inertial.find("inertia")
|
||||
if mass_element is None or inertia_element is None:
|
||||
raise ValueError(f"link {link.get('name')!r} has incomplete inertial data")
|
||||
origin = inertial.find("origin")
|
||||
xyz = _numbers(origin.get("xyz", "0 0 0"), 3) if origin is not None else np.zeros(3)
|
||||
rpy = _numbers(origin.get("rpy", "0 0 0"), 3) if origin is not None else np.zeros(3)
|
||||
inertia_matrix = np.array(
|
||||
[
|
||||
[float(inertia_element.get("ixx")), float(inertia_element.get("ixy")), float(inertia_element.get("ixz"))],
|
||||
[float(inertia_element.get("ixy")), float(inertia_element.get("iyy")), float(inertia_element.get("iyz"))],
|
||||
[float(inertia_element.get("ixz")), float(inertia_element.get("iyz")), float(inertia_element.get("izz"))],
|
||||
]
|
||||
)
|
||||
rotation = pin.rpy.rpyToMatrix(*rpy)
|
||||
inertia_matrix = rotation @ inertia_matrix @ rotation.T
|
||||
full_inertia = [
|
||||
inertia_matrix[0, 0],
|
||||
inertia_matrix[1, 1],
|
||||
inertia_matrix[2, 2],
|
||||
inertia_matrix[0, 1],
|
||||
inertia_matrix[0, 2],
|
||||
inertia_matrix[1, 2],
|
||||
]
|
||||
ET.SubElement(
|
||||
body,
|
||||
"inertial",
|
||||
{
|
||||
"pos": _format(xyz),
|
||||
"mass": mass_element.get("value", "0"),
|
||||
"fullinertia": _format(full_inertia),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _add_link_visual(
|
||||
body: ET.Element,
|
||||
link: ET.Element,
|
||||
arm: str,
|
||||
single_mesh_keys: Dict[str, str],
|
||||
) -> None:
|
||||
visual = link.find("visual")
|
||||
if visual is None:
|
||||
return
|
||||
mesh = visual.find("geometry/mesh")
|
||||
if mesh is None:
|
||||
return
|
||||
filename = Path(mesh.get("filename", "")).name
|
||||
try:
|
||||
mesh_name = single_mesh_keys[filename]
|
||||
except KeyError as exc:
|
||||
raise ValueError(f"missing packaged single-arm mesh {filename!r}") from exc
|
||||
ET.SubElement(
|
||||
body,
|
||||
"geom",
|
||||
{
|
||||
"name": f"{arm}_{link.get('name')}_visual",
|
||||
"type": "mesh",
|
||||
"mesh": mesh_name,
|
||||
"material": f"{arm}_arm",
|
||||
"contype": "0",
|
||||
"conaffinity": "0",
|
||||
"group": "1",
|
||||
**_origin_attributes(visual),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _add_arm(
|
||||
worldbody: ET.Element,
|
||||
arm: str,
|
||||
mount: pin.SE3,
|
||||
single_root: ET.Element,
|
||||
single_mesh_keys: Dict[str, str],
|
||||
gripper_mesh_name: str,
|
||||
) -> None:
|
||||
links = {link.get("name"): link for link in single_root.findall("link")}
|
||||
joints_by_parent: Dict[str, list[ET.Element]] = {}
|
||||
for joint in single_root.findall("joint"):
|
||||
parent_element = joint.find("parent")
|
||||
if parent_element is None:
|
||||
continue
|
||||
joints_by_parent.setdefault(parent_element.get("link", ""), []).append(joint)
|
||||
|
||||
limits = physical_joint_limits()
|
||||
base_link = links["base_link"]
|
||||
base_body = ET.SubElement(
|
||||
worldbody,
|
||||
"body",
|
||||
{"name": f"{arm}_base_link", **_se3_attributes(mount)},
|
||||
)
|
||||
_add_link_visual(base_body, base_link, arm, single_mesh_keys)
|
||||
|
||||
def append_children(parent_body: ET.Element, parent_link_name: str) -> None:
|
||||
for joint in joints_by_parent.get(parent_link_name, []):
|
||||
child_element = joint.find("child")
|
||||
if child_element is None:
|
||||
raise ValueError(f"joint {joint.get('name')!r} has no child")
|
||||
child_name = child_element.get("link", "")
|
||||
child_link = links[child_name]
|
||||
body = ET.SubElement(
|
||||
parent_body,
|
||||
"body",
|
||||
{
|
||||
"name": f"{arm}_{child_name}",
|
||||
**_origin_attributes(joint),
|
||||
},
|
||||
)
|
||||
joint_index = int(joint.get("name", "joint_0").split("_")[-1]) - 1
|
||||
axis_element = joint.find("axis")
|
||||
axis = "0 0 1" if axis_element is None else axis_element.get("xyz", "0 0 1")
|
||||
ET.SubElement(
|
||||
body,
|
||||
"joint",
|
||||
{
|
||||
"name": f"{arm}_joint_{joint_index + 1}",
|
||||
"type": "hinge",
|
||||
"axis": axis,
|
||||
"range": _format(
|
||||
[limits.lower[joint_index], limits.upper[joint_index]]
|
||||
),
|
||||
"limited": "true",
|
||||
"damping": "0",
|
||||
},
|
||||
)
|
||||
_add_inertial(body, child_link)
|
||||
_add_link_visual(body, child_link, arm, single_mesh_keys)
|
||||
if child_name == "link_7":
|
||||
ET.SubElement(
|
||||
body,
|
||||
"site",
|
||||
{
|
||||
"name": f"{arm}_flange",
|
||||
"type": "sphere",
|
||||
"size": "0.008",
|
||||
"rgba": "0.1 0.9 0.2 1" if arm == "left" else "0.1 0.5 0.95 1",
|
||||
"group": "2",
|
||||
},
|
||||
)
|
||||
ET.SubElement(
|
||||
body,
|
||||
"geom",
|
||||
{
|
||||
"name": f"{arm}_gripper_visual",
|
||||
"type": "mesh",
|
||||
"mesh": gripper_mesh_name,
|
||||
"pos": "0 0 0.092",
|
||||
"quat": _format(_quaternion_from_rpy(np.array([-np.pi, np.pi, -np.pi]))),
|
||||
"material": f"{arm}_gripper",
|
||||
"contype": "0",
|
||||
"conaffinity": "0",
|
||||
"group": "1",
|
||||
},
|
||||
)
|
||||
append_children(body, child_name)
|
||||
|
||||
append_children(base_body, "base_link")
|
||||
|
||||
|
||||
def build_normalized_dual_mjcf(
|
||||
single_urdf_path: Optional[Path | str] = None,
|
||||
dual_source_path: Optional[Path | str] = None,
|
||||
) -> Tuple[str, Dict[str, bytes]]:
|
||||
"""Build a canonical 14-DOF dual-arm MJCF and its in-memory mesh assets."""
|
||||
|
||||
single_path = (
|
||||
Path(single_urdf_path) if single_urdf_path is not None else default_urdf_path()
|
||||
)
|
||||
dual_path = (
|
||||
Path(dual_source_path)
|
||||
if dual_source_path is not None
|
||||
else default_dual_source_path()
|
||||
)
|
||||
single_root = ET.parse(single_path).getroot()
|
||||
dual_root = ET.parse(dual_path).getroot()
|
||||
single_mesh_dir = _single_mesh_directory(single_path)
|
||||
dual_obj_dir = _dual_obj_directory(dual_path)
|
||||
|
||||
mujoco_root = ET.Element("mujoco", {"model": "rm75_normalized_dual_stage2"})
|
||||
ET.SubElement(
|
||||
mujoco_root,
|
||||
"compiler",
|
||||
{
|
||||
"angle": "radian",
|
||||
"autolimits": "true",
|
||||
"inertiafromgeom": "false",
|
||||
"balanceinertia": "true",
|
||||
},
|
||||
)
|
||||
ET.SubElement(
|
||||
mujoco_root,
|
||||
"option",
|
||||
{"timestep": "0.002", "gravity": "0 0 -9.81", "integrator": "implicitfast"},
|
||||
)
|
||||
ET.SubElement(mujoco_root, "statistic", {"center": "0 0 0.65", "extent": "1.4"})
|
||||
visual = ET.SubElement(mujoco_root, "visual")
|
||||
ET.SubElement(
|
||||
visual,
|
||||
"global",
|
||||
{
|
||||
"azimuth": "90",
|
||||
"elevation": "-18",
|
||||
"offwidth": "1280",
|
||||
"offheight": "720",
|
||||
},
|
||||
)
|
||||
ET.SubElement(visual, "rgba", {"haze": "0.15 0.18 0.2 1"})
|
||||
|
||||
assets_element = ET.SubElement(mujoco_root, "asset")
|
||||
ET.SubElement(assets_element, "material", {"name": "left_arm", "rgba": "0.82 0.84 0.86 1"})
|
||||
ET.SubElement(assets_element, "material", {"name": "right_arm", "rgba": "0.7 0.74 0.78 1"})
|
||||
ET.SubElement(assets_element, "material", {"name": "left_gripper", "rgba": "0.15 0.75 0.85 1"})
|
||||
ET.SubElement(assets_element, "material", {"name": "right_gripper", "rgba": "0.9 0.48 0.18 1"})
|
||||
ET.SubElement(assets_element, "material", {"name": "platform", "rgba": "0.28 0.31 0.34 1"})
|
||||
|
||||
assets: Dict[str, bytes] = {}
|
||||
single_mesh_keys: Dict[str, str] = {}
|
||||
for mesh_path in sorted(single_mesh_dir.glob("*.STL")):
|
||||
asset_key = f"rm75_meshes/{mesh_path.name}"
|
||||
mesh_name = _mesh_asset_name(mesh_path.name, "rm75")
|
||||
assets[asset_key] = mesh_path.read_bytes()
|
||||
single_mesh_keys[mesh_path.name] = mesh_name
|
||||
ET.SubElement(assets_element, "mesh", {"name": mesh_name, "file": asset_key})
|
||||
|
||||
dual_mesh_names: Dict[str, str] = {}
|
||||
for mesh_path in sorted(dual_obj_dir.glob("*.obj")):
|
||||
asset_key = f"dual_arm_obj/{mesh_path.name}"
|
||||
mesh_name = _mesh_asset_name(mesh_path.name, "dual")
|
||||
assets[asset_key] = mesh_path.read_bytes()
|
||||
dual_mesh_names[mesh_path.name] = mesh_name
|
||||
ET.SubElement(assets_element, "mesh", {"name": mesh_name, "file": asset_key})
|
||||
if len(dual_mesh_names) != 19:
|
||||
raise ValueError(f"expected 19 dual-arm OBJ assets, found {len(dual_mesh_names)}")
|
||||
|
||||
worldbody = ET.SubElement(mujoco_root, "worldbody")
|
||||
ET.SubElement(
|
||||
worldbody,
|
||||
"light",
|
||||
{
|
||||
"name": "key_light",
|
||||
"pos": "0 -1.2 2.8",
|
||||
"dir": "0 0.4 -1",
|
||||
"diffuse": "0.9 0.9 0.9",
|
||||
},
|
||||
)
|
||||
ET.SubElement(
|
||||
worldbody,
|
||||
"camera",
|
||||
{
|
||||
"name": "overview",
|
||||
"pos": "0 -2.6 1.25",
|
||||
"xyaxes": "1 0 0 0 0.3 0.953939",
|
||||
"fovy": "45",
|
||||
},
|
||||
)
|
||||
ET.SubElement(
|
||||
worldbody,
|
||||
"geom",
|
||||
{
|
||||
"name": "floor",
|
||||
"type": "plane",
|
||||
"size": "2.5 2.5 0.05",
|
||||
"rgba": "0.12 0.14 0.16 1",
|
||||
"contype": "0",
|
||||
"conaffinity": "0",
|
||||
"group": "0",
|
||||
},
|
||||
)
|
||||
|
||||
robot_base = dual_root.find("./link[@name='robot_base']")
|
||||
if robot_base is None:
|
||||
raise ValueError("dual-arm source URDF has no robot_base link")
|
||||
for visual_element in robot_base.findall("visual"):
|
||||
mesh_element = visual_element.find("geometry/mesh")
|
||||
if mesh_element is None:
|
||||
continue
|
||||
filename = Path(mesh_element.get("filename", "")).name
|
||||
ET.SubElement(
|
||||
worldbody,
|
||||
"geom",
|
||||
{
|
||||
"name": f"platform_{visual_element.get('name', filename)}",
|
||||
"type": "mesh",
|
||||
"mesh": dual_mesh_names[filename],
|
||||
"material": "platform",
|
||||
"contype": "0",
|
||||
"conaffinity": "0",
|
||||
"group": "1",
|
||||
**_origin_attributes(visual_element),
|
||||
},
|
||||
)
|
||||
|
||||
mounts = load_dual_arm_mounts(dual_path)
|
||||
_add_arm(
|
||||
worldbody,
|
||||
"left",
|
||||
mounts.left_base,
|
||||
single_root,
|
||||
single_mesh_keys,
|
||||
dual_mesh_names["dual_arm_gripper1_vis_1.obj"],
|
||||
)
|
||||
_add_arm(
|
||||
worldbody,
|
||||
"right",
|
||||
mounts.right_base,
|
||||
single_root,
|
||||
single_mesh_keys,
|
||||
dual_mesh_names["dual_arm_gripper2_vis_1.obj"],
|
||||
)
|
||||
for arm, color in (
|
||||
("left", "0.95 0.12 0.12 0.8"),
|
||||
("right", "0.95 0.75 0.08 0.8"),
|
||||
):
|
||||
marker = ET.SubElement(
|
||||
worldbody,
|
||||
"body",
|
||||
{"name": f"{arm}_target_marker", "mocap": "true", "pos": "0 0 0"},
|
||||
)
|
||||
ET.SubElement(
|
||||
marker,
|
||||
"geom",
|
||||
{
|
||||
"name": f"{arm}_target_marker_geom",
|
||||
"type": "sphere",
|
||||
"size": "0.018",
|
||||
"rgba": color,
|
||||
"contype": "0",
|
||||
"conaffinity": "0",
|
||||
"group": "2",
|
||||
},
|
||||
)
|
||||
|
||||
ET.indent(mujoco_root, space=" ")
|
||||
return ET.tostring(mujoco_root, encoding="unicode"), assets
|
||||
@@ -0,0 +1,185 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from time import sleep
|
||||
from typing import Collection, Dict, Mapping, Optional
|
||||
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
|
||||
from .dual_arm import DualArmAssembly
|
||||
from .kinematics import RM75Kinematics, pose_errors, validate_se3
|
||||
from .mujoco_backend import DualArmMuJoCo
|
||||
from .robot_backend import DualArmJointState
|
||||
from .solver import RM75IkSolver, deterministic_recovery_seeds
|
||||
from .teleop_config import ArmName, ArmTeleopProfile
|
||||
from .types import IkOptions, physical_joint_limits, teleop_joint_limits
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class InitialPoseDiagnostic:
|
||||
arm: ArmName
|
||||
solved_q_rad: np.ndarray
|
||||
solved_position_error_m: float
|
||||
solved_orientation_error_rad: float
|
||||
configured_joint_position_error_m: float
|
||||
configured_joint_orientation_error_rad: float
|
||||
|
||||
|
||||
class MujocoRobot:
|
||||
"""Dual-arm kinematic backend implementing the backend-neutral robot contract."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
profiles: Mapping[ArmName, ArmTeleopProfile],
|
||||
*,
|
||||
initialize_from_tcp: bool = True,
|
||||
) -> None:
|
||||
if set(profiles) != {"left", "right"}:
|
||||
raise ValueError("profiles must contain left and right arms")
|
||||
self.profiles = dict(profiles)
|
||||
self.scene = DualArmMuJoCo(controlled_arm="left", limits=teleop_joint_limits())
|
||||
self.assembly = DualArmAssembly.from_source_urdf()
|
||||
self._kinematics = {
|
||||
arm: RM75Kinematics(limits=teleop_joint_limits())
|
||||
for arm in ("left", "right")
|
||||
}
|
||||
self._viewer = None
|
||||
self._closed = False
|
||||
self._stopped_arms: set[ArmName] = set()
|
||||
self.initial_pose_diagnostics: Dict[ArmName, InitialPoseDiagnostic] = {}
|
||||
if initialize_from_tcp:
|
||||
self._initialize_from_configured_tcp()
|
||||
|
||||
def _initialize_from_configured_tcp(self) -> None:
|
||||
solved: Dict[ArmName, np.ndarray] = {}
|
||||
limits = teleop_joint_limits()
|
||||
physical_kinematics = RM75Kinematics(limits=physical_joint_limits())
|
||||
for seed_offset, arm in enumerate(("left", "right")):
|
||||
profile = self.profiles[arm]
|
||||
kinematics = RM75Kinematics(limits=limits)
|
||||
solver = RM75IkSolver(kinematics)
|
||||
flange_target = profile.initial_tcp * profile.tool_from_flange.inverse()
|
||||
seeds = deterministic_recovery_seeds(
|
||||
limits, count=24, random_seed=750 + seed_offset
|
||||
)
|
||||
if limits.contains(profile.configured_initial_q_rad):
|
||||
seeds.insert(0, profile.configured_initial_q_rad)
|
||||
result = solver.solve_multistart(
|
||||
flange_target,
|
||||
seeds,
|
||||
IkOptions(max_iterations=700, time_limit_sec=None),
|
||||
)
|
||||
if not result.success or result.q is None:
|
||||
raise RuntimeError(
|
||||
f"failed to resolve {arm} initial_tcp_pose: "
|
||||
f"{result.status.value}: {result.message}"
|
||||
)
|
||||
solved[arm] = result.q.copy()
|
||||
solved_tcp = kinematics.forward(result.q, profile.tool_from_flange)
|
||||
solved_error = pose_errors(solved_tcp, profile.initial_tcp)
|
||||
configured_tcp = physical_kinematics.forward(
|
||||
profile.configured_initial_q_rad, profile.tool_from_flange
|
||||
)
|
||||
configured_error = pose_errors(configured_tcp, profile.initial_tcp)
|
||||
q = result.q.copy()
|
||||
q.setflags(write=False)
|
||||
self.initial_pose_diagnostics[arm] = InitialPoseDiagnostic(
|
||||
arm=arm,
|
||||
solved_q_rad=q,
|
||||
solved_position_error_m=solved_error[0],
|
||||
solved_orientation_error_rad=solved_error[1],
|
||||
configured_joint_position_error_m=configured_error[0],
|
||||
configured_joint_orientation_error_rad=configured_error[1],
|
||||
)
|
||||
self.scene.set_dual_configuration(solved["left"], solved["right"])
|
||||
for arm in ("left", "right"):
|
||||
self.set_target_tcp_pose(arm, self.profiles[arm].initial_tcp)
|
||||
|
||||
def connect(self) -> None:
|
||||
if self._closed:
|
||||
raise RuntimeError("MujocoRobot is closed")
|
||||
|
||||
def read_joint_positions(self) -> DualArmJointState:
|
||||
self._require_open()
|
||||
return DualArmJointState(
|
||||
{
|
||||
arm: self.scene.get_arm_configuration(arm)
|
||||
for arm in ("left", "right")
|
||||
}
|
||||
)
|
||||
|
||||
def command_joint_positions(
|
||||
self, targets_rad: Mapping[ArmName, np.ndarray]
|
||||
) -> None:
|
||||
self._require_open()
|
||||
if not targets_rad or not set(targets_rad).issubset({"left", "right"}):
|
||||
raise ValueError("joint targets must contain at least one valid arm")
|
||||
current = self.read_joint_positions().positions_rad
|
||||
left = np.asarray(targets_rad.get("left", current["left"]), dtype=float)
|
||||
right = np.asarray(targets_rad.get("right", current["right"]), dtype=float)
|
||||
self.scene.set_dual_configuration(left, right)
|
||||
self._stopped_arms.difference_update(targets_rad)
|
||||
|
||||
def set_target_tcp_pose(self, arm: ArmName, target_tcp: pin.SE3) -> None:
|
||||
self._require_open()
|
||||
if arm not in ("left", "right"):
|
||||
raise ValueError("arm must be 'left' or 'right'")
|
||||
validate_se3(target_tcp, "target_tcp")
|
||||
mount = (
|
||||
self.assembly.mounts.left_base
|
||||
if arm == "left"
|
||||
else self.assembly.mounts.right_base
|
||||
)
|
||||
self.scene.set_arm_target_marker(arm, mount * target_tcp)
|
||||
|
||||
def get_tcp_pose(self, arm: ArmName) -> pin.SE3:
|
||||
self._require_open()
|
||||
q = self.scene.get_arm_configuration(arm)
|
||||
return self._kinematics[arm].forward(
|
||||
q, self.profiles[arm].tool_from_flange
|
||||
)
|
||||
|
||||
def stop(self, arms: Collection[ArmName] = ("left", "right")) -> None:
|
||||
self._require_open()
|
||||
selected = set(arms)
|
||||
if not selected.issubset({"left", "right"}):
|
||||
raise ValueError("stop arms must contain only left/right")
|
||||
self.scene.data.qvel[:] = 0.0
|
||||
self._stopped_arms.update(selected)
|
||||
|
||||
def open_viewer(self):
|
||||
self._require_open()
|
||||
if self._viewer is None:
|
||||
import mujoco.viewer
|
||||
|
||||
self._viewer = mujoco.viewer.launch_passive(
|
||||
self.scene.model, self.scene.data
|
||||
)
|
||||
return self._viewer
|
||||
|
||||
def sync_viewer(self) -> bool:
|
||||
if self._viewer is None:
|
||||
return True
|
||||
if not self._viewer.is_running():
|
||||
return False
|
||||
self._viewer.sync()
|
||||
return True
|
||||
|
||||
def render(self, width: int = 1280, height: int = 720) -> np.ndarray:
|
||||
self._require_open()
|
||||
return self.scene.render(width, height)
|
||||
|
||||
def close(self) -> None:
|
||||
if self._closed:
|
||||
return
|
||||
if self._viewer is not None:
|
||||
self._viewer.close()
|
||||
self._viewer = None
|
||||
sleep(0.2)
|
||||
self.scene.data.qvel[:] = 0.0
|
||||
self._closed = True
|
||||
|
||||
def _require_open(self) -> None:
|
||||
if self._closed:
|
||||
raise RuntimeError("MujocoRobot is closed")
|
||||
@@ -0,0 +1,109 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from math import radians
|
||||
from typing import Iterable, List
|
||||
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
|
||||
from .kinematics import validate_se3
|
||||
from .solver import RM75IkSolver
|
||||
from .types import IkOptions
|
||||
|
||||
|
||||
DEMO_TRAJECTORIES = ("joint", "line", "arc", "orientation", "combined")
|
||||
|
||||
|
||||
def se3_target_trajectory(
|
||||
start_pose: pin.SE3,
|
||||
target_pose: pin.SE3,
|
||||
points: int,
|
||||
) -> List[pin.SE3]:
|
||||
"""Interpolate translation linearly and orientation on SO(3)."""
|
||||
|
||||
if points < 2:
|
||||
raise ValueError("trajectory must contain at least two points")
|
||||
validate_se3(start_pose, "start_pose")
|
||||
validate_se3(target_pose, "target_pose")
|
||||
rotation_delta = pin.log3(start_pose.rotation.T @ target_pose.rotation)
|
||||
targets = []
|
||||
for fraction in np.linspace(0.0, 1.0, points):
|
||||
translation = (
|
||||
(1.0 - fraction) * start_pose.translation
|
||||
+ fraction * target_pose.translation
|
||||
)
|
||||
rotation = start_pose.rotation @ pin.exp3(fraction * rotation_delta)
|
||||
targets.append(pin.SE3(rotation, translation))
|
||||
return targets
|
||||
|
||||
|
||||
def cartesian_demo_targets(
|
||||
kind: str,
|
||||
start_pose: pin.SE3,
|
||||
points: int = 180,
|
||||
) -> List[pin.SE3]:
|
||||
if kind not in DEMO_TRAJECTORIES[1:]:
|
||||
raise ValueError(f"Cartesian trajectory must be one of {DEMO_TRAJECTORIES[1:]}")
|
||||
if points < 2:
|
||||
raise ValueError("trajectory must contain at least two points")
|
||||
validate_se3(start_pose, "start_pose")
|
||||
targets: List[pin.SE3] = []
|
||||
for fraction in np.linspace(0.0, 1.0, points):
|
||||
translation = start_pose.translation.copy()
|
||||
rotation = start_pose.rotation.copy()
|
||||
if kind == "line":
|
||||
translation += np.array([0.04 * fraction, 0.0, 0.0])
|
||||
elif kind == "arc":
|
||||
angle = 0.5 * np.pi * fraction
|
||||
translation += np.array(
|
||||
[0.03 * np.sin(angle), 0.03 * (1.0 - np.cos(angle)), 0.0]
|
||||
)
|
||||
elif kind == "orientation":
|
||||
rotation = rotation @ pin.rpy.rpyToMatrix(0.0, 0.0, radians(15) * fraction)
|
||||
elif kind == "combined":
|
||||
translation += np.array([0.035 * fraction, 0.015 * fraction, 0.0])
|
||||
rotation = rotation @ pin.rpy.rpyToMatrix(
|
||||
radians(8) * fraction,
|
||||
0.0,
|
||||
radians(12) * fraction,
|
||||
)
|
||||
targets.append(pin.SE3(rotation, translation))
|
||||
return targets
|
||||
|
||||
|
||||
def joint_demo_trajectory(
|
||||
start_q_rad: np.ndarray,
|
||||
points: int = 180,
|
||||
) -> np.ndarray:
|
||||
start = np.asarray(start_q_rad, dtype=float)
|
||||
if start.shape != (7,) or not np.all(np.isfinite(start)):
|
||||
raise ValueError("start_q_rad must be a finite shape-(7,) vector")
|
||||
if points < 2:
|
||||
raise ValueError("trajectory must contain at least two points")
|
||||
offset = np.deg2rad([12.0, -8.0, 10.0, 8.0, -6.0, 7.0, 10.0])
|
||||
blend = 0.5 - 0.5 * np.cos(np.linspace(0.0, np.pi, points))
|
||||
return start[None, :] + blend[:, None] * offset[None, :]
|
||||
|
||||
|
||||
def solve_pose_trajectory(
|
||||
solver: RM75IkSolver,
|
||||
targets: Iterable[pin.SE3],
|
||||
seed_q_rad: np.ndarray,
|
||||
options: IkOptions = IkOptions(
|
||||
position_tolerance_m=9e-4,
|
||||
orientation_tolerance_rad=radians(0.09),
|
||||
max_iterations=200,
|
||||
),
|
||||
) -> np.ndarray:
|
||||
seed = np.asarray(seed_q_rad, dtype=float).copy()
|
||||
solutions = []
|
||||
for index, target in enumerate(targets):
|
||||
result = solver.solve(target, seed, options)
|
||||
if not result.success or result.q is None:
|
||||
raise RuntimeError(
|
||||
f"IK failed at trajectory point {index}: "
|
||||
f"{result.status.value} {result.message}"
|
||||
)
|
||||
seed = result.q.copy()
|
||||
solutions.append(seed)
|
||||
return np.asarray(solutions)
|
||||
@@ -0,0 +1,43 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Collection, Dict, Mapping, Protocol, runtime_checkable
|
||||
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
|
||||
from .teleop_config import ArmName
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DualArmJointState:
|
||||
positions_rad: Mapping[ArmName, np.ndarray]
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
normalized: Dict[ArmName, np.ndarray] = {}
|
||||
if set(self.positions_rad) != {"left", "right"}:
|
||||
raise ValueError("dual-arm state must contain left and right positions")
|
||||
for arm, values in self.positions_rad.items():
|
||||
q = np.asarray(values, dtype=float).copy()
|
||||
if q.shape != (7,) or not np.all(np.isfinite(q)):
|
||||
raise ValueError(f"{arm} joint state must be finite with shape (7,)")
|
||||
q.setflags(write=False)
|
||||
normalized[arm] = q
|
||||
object.__setattr__(self, "positions_rad", normalized)
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class RobotBackend(Protocol):
|
||||
def connect(self) -> None: ...
|
||||
|
||||
def read_joint_positions(self) -> DualArmJointState: ...
|
||||
|
||||
def command_joint_positions(
|
||||
self, targets_rad: Mapping[ArmName, np.ndarray]
|
||||
) -> None: ...
|
||||
|
||||
def set_target_tcp_pose(self, arm: ArmName, target_tcp: pin.SE3) -> None: ...
|
||||
|
||||
def stop(self, arms: Collection[ArmName] = ("left", "right")) -> None: ...
|
||||
|
||||
def close(self) -> None: ...
|
||||
@@ -0,0 +1,80 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Optional, Sequence
|
||||
|
||||
|
||||
os.environ.setdefault("MUJOCO_GL", "egl")
|
||||
|
||||
|
||||
def _default_output_dir() -> Path:
|
||||
package_root = Path(__file__).resolve().parents[2]
|
||||
if (package_root / "pyproject.toml").is_file():
|
||||
return package_root / "artifacts" / "stage2"
|
||||
return Path.cwd() / "stage2_artifacts"
|
||||
|
||||
|
||||
def build_parser() -> argparse.ArgumentParser:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Headless single-arm IK validation in the normalized dual RM75 scene"
|
||||
)
|
||||
parser.add_argument("--arm", choices=("left", "right"), default="left")
|
||||
parser.add_argument(
|
||||
"--sdk-root",
|
||||
type=Path,
|
||||
help="directory containing the RealMan Robotic_Arm Python package",
|
||||
)
|
||||
parser.add_argument("--output-dir", type=Path, default=_default_output_dir())
|
||||
parser.add_argument("--seed", type=int, default=20260630)
|
||||
parser.add_argument("--quick", action="store_true")
|
||||
parser.add_argument(
|
||||
"--report-only",
|
||||
action="store_true",
|
||||
help="return exit code zero while preserving failed checks in reports",
|
||||
)
|
||||
return parser
|
||||
|
||||
|
||||
def main(argv: Optional[Sequence[str]] = None) -> int:
|
||||
args = build_parser().parse_args(argv)
|
||||
from .realman_reference import RealManFkReference
|
||||
from .stage2_validation import (
|
||||
Stage2Settings,
|
||||
Stage2Validator,
|
||||
write_stage2_report,
|
||||
)
|
||||
|
||||
settings = (
|
||||
Stage2Settings.quick(seed=args.seed)
|
||||
if args.quick
|
||||
else Stage2Settings(seed=args.seed)
|
||||
)
|
||||
validator = Stage2Validator(
|
||||
RealManFkReference(args.sdk_root),
|
||||
controlled_arm=args.arm,
|
||||
settings=settings,
|
||||
)
|
||||
summary = validator.run()
|
||||
json_path, csv_path, markdown_path, image_paths = write_stage2_report(
|
||||
args.output_dir,
|
||||
summary,
|
||||
validator.failures,
|
||||
validator.images,
|
||||
)
|
||||
print(f"RM75-B stage-2 validation: {'PASS' if summary['passed'] else 'FAIL'}")
|
||||
for name, check in summary["checks"].items():
|
||||
print(f" [{'PASS' if check['passed'] else 'FAIL'}] {name}")
|
||||
print("Reports:")
|
||||
for path in (json_path, csv_path, markdown_path, *image_paths):
|
||||
print(f" {path}")
|
||||
if args.report_only or summary["passed"]:
|
||||
return 0
|
||||
return 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
|
||||
@@ -0,0 +1,232 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from time import perf_counter, sleep
|
||||
from typing import Optional, Sequence
|
||||
|
||||
|
||||
def build_parser() -> argparse.ArgumentParser:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Visualize first-stage IK in the normalized dual RM75 scene"
|
||||
)
|
||||
parser.add_argument("--arm", choices=("left", "right"), default="left")
|
||||
parser.add_argument(
|
||||
"--trajectory",
|
||||
choices=("joint", "line", "arc", "orientation", "combined"),
|
||||
default="combined",
|
||||
)
|
||||
parser.add_argument("--points", type=int, default=180)
|
||||
parser.add_argument("--dt", type=float, default=1.0 / 90.0)
|
||||
parser.add_argument(
|
||||
"--target-position",
|
||||
nargs=3,
|
||||
type=float,
|
||||
metavar=("X", "Y", "Z"),
|
||||
help="target position in the controlled-arm base frame, in meters",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--target-rpy",
|
||||
nargs=3,
|
||||
type=float,
|
||||
metavar=("ROLL", "PITCH", "YAW"),
|
||||
help="target RPY in radians; defaults to the start orientation",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--duration",
|
||||
type=float,
|
||||
default=8.0,
|
||||
help="target-point motion duration in seconds",
|
||||
)
|
||||
parser.add_argument("--wait-before", type=float, default=2.0)
|
||||
parser.add_argument("--hold-after", type=float, default=3.0)
|
||||
parser.add_argument(
|
||||
"--manual-drag",
|
||||
action="store_true",
|
||||
help="enable zero-gravity mouse perturbation of the controlled arm",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--drag-damping",
|
||||
type=float,
|
||||
default=1.5,
|
||||
help="joint damping used by manual-drag mode",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--headless",
|
||||
action="store_true",
|
||||
help="render the final frame without opening the interactive viewer",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output",
|
||||
type=Path,
|
||||
default=Path("stage2_demo.png"),
|
||||
help="headless output image",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--close-after-play",
|
||||
action="store_true",
|
||||
help="close the interactive viewer after one playback (smoke testing)",
|
||||
)
|
||||
return parser
|
||||
|
||||
|
||||
def main(argv: Optional[Sequence[str]] = None) -> int:
|
||||
args = build_parser().parse_args(argv)
|
||||
if args.manual_drag and args.headless:
|
||||
raise SystemExit("--manual-drag requires the interactive viewer")
|
||||
if args.target_rpy is not None and args.target_position is None:
|
||||
raise SystemExit("--target-rpy requires --target-position")
|
||||
for name in ("dt", "duration", "wait_before", "hold_after"):
|
||||
if getattr(args, name) < 0.0 or (name in {"dt", "duration"} and getattr(args, name) == 0.0):
|
||||
raise SystemExit(f"--{name.replace('_', '-')} must be positive")
|
||||
if args.headless:
|
||||
os.environ.setdefault("MUJOCO_GL", "egl")
|
||||
else:
|
||||
os.environ.setdefault("MUJOCO_GL", "glfw")
|
||||
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
from PIL import Image
|
||||
|
||||
from .dual_arm import DualArmAssembly
|
||||
from .mujoco_backend import CONTROLLED_HOME_Q_RAD, DualArmMuJoCo
|
||||
from .mujoco_trajectories import (
|
||||
cartesian_demo_targets,
|
||||
joint_demo_trajectory,
|
||||
se3_target_trajectory,
|
||||
solve_pose_trajectory,
|
||||
)
|
||||
from .kinematics import RM75Kinematics, pose_errors
|
||||
from .solver import RM75IkSolver
|
||||
from .types import teleop_joint_limits
|
||||
|
||||
scene = DualArmMuJoCo(controlled_arm=args.arm)
|
||||
if args.manual_drag:
|
||||
scene.configure_manual_drag(args.drag_damping)
|
||||
print("Manual drag mode")
|
||||
print(" 1. Double-click a link to select it.")
|
||||
print(" 2. Hold Ctrl and drag with the right mouse button.")
|
||||
print(" 3. Close the viewer or press Ctrl+C to finish.")
|
||||
viewer = mujoco.viewer.launch_passive(scene.model, scene.data)
|
||||
try:
|
||||
while viewer.is_running():
|
||||
step_started = perf_counter()
|
||||
scene.step_manual_drag()
|
||||
viewer.sync()
|
||||
remaining = scene.model.opt.timestep - (perf_counter() - step_started)
|
||||
if remaining > 0.0:
|
||||
sleep(remaining)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
viewer.close()
|
||||
sleep(0.2)
|
||||
print("Final controlled-arm joints (rad):")
|
||||
print(scene.get_arm_configuration(args.arm).tolist())
|
||||
return 0
|
||||
|
||||
kinematics = RM75Kinematics(limits=teleop_joint_limits())
|
||||
solver = RM75IkSolver(kinematics)
|
||||
targets = None
|
||||
target_pose = None
|
||||
if args.target_position is not None:
|
||||
start_pose = kinematics.forward(CONTROLLED_HOME_Q_RAD)
|
||||
rotation = (
|
||||
start_pose.rotation
|
||||
if args.target_rpy is None
|
||||
else pin.rpy.rpyToMatrix(*args.target_rpy)
|
||||
)
|
||||
target_pose = pin.SE3(rotation, np.asarray(args.target_position, dtype=float))
|
||||
target_points = max(2, int(round(args.duration / args.dt)) + 1)
|
||||
try:
|
||||
targets = se3_target_trajectory(start_pose, target_pose, target_points)
|
||||
trajectory = solve_pose_trajectory(
|
||||
solver, targets, CONTROLLED_HOME_Q_RAD
|
||||
)
|
||||
except (RuntimeError, TypeError, ValueError) as exc:
|
||||
raise SystemExit(f"target trajectory rejected: {exc}") from exc
|
||||
print("Target mode")
|
||||
print(" position (m):", target_pose.translation.tolist())
|
||||
print(" rpy (rad):", pin.rpy.matrixToRpy(target_pose.rotation).tolist())
|
||||
print(f" duration: {args.duration:.3f} s, points: {target_points}")
|
||||
elif args.trajectory == "joint":
|
||||
trajectory = joint_demo_trajectory(CONTROLLED_HOME_Q_RAD, args.points)
|
||||
else:
|
||||
targets = cartesian_demo_targets(
|
||||
args.trajectory,
|
||||
kinematics.forward(CONTROLLED_HOME_Q_RAD),
|
||||
args.points,
|
||||
)
|
||||
trajectory = solve_pose_trajectory(
|
||||
solver, targets, CONTROLLED_HOME_Q_RAD
|
||||
)
|
||||
|
||||
assembly = DualArmAssembly.from_source_urdf()
|
||||
mount = (
|
||||
assembly.mounts.left_base
|
||||
if args.arm == "left"
|
||||
else assembly.mounts.right_base
|
||||
)
|
||||
if target_pose is not None:
|
||||
scene.set_target_marker(mount * target_pose)
|
||||
elif targets is not None:
|
||||
scene.set_target_marker(mount * targets[-1])
|
||||
else:
|
||||
scene.set_arm_configuration(args.arm, trajectory[-1])
|
||||
scene.set_target_marker(scene.get_flange_pose(args.arm))
|
||||
scene.set_arm_configuration(args.arm, trajectory[0])
|
||||
|
||||
if args.headless:
|
||||
result = scene.play_trajectory(trajectory, dt=args.dt, realtime=False)
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
Image.fromarray(scene.render()).save(args.output)
|
||||
print(
|
||||
f"Played {result.samples} samples; max joint step "
|
||||
f"{result.max_joint_step_rad:.6f} rad"
|
||||
)
|
||||
if target_pose is not None:
|
||||
actual_local = mount.actInv(result.final_flange_pose)
|
||||
position_error, orientation_error = pose_errors(actual_local, target_pose)
|
||||
print(f"Final position error: {position_error:.9f} m")
|
||||
print(f"Final orientation error: {orientation_error:.9f} rad")
|
||||
print(args.output)
|
||||
return 0
|
||||
|
||||
viewer = mujoco.viewer.launch_passive(scene.model, scene.data)
|
||||
try:
|
||||
wait_until = perf_counter() + args.wait_before
|
||||
while viewer.is_running() and perf_counter() < wait_until:
|
||||
viewer.sync()
|
||||
sleep(0.02)
|
||||
scene.play_trajectory(
|
||||
trajectory,
|
||||
dt=args.dt,
|
||||
realtime=True,
|
||||
viewer=viewer,
|
||||
)
|
||||
if target_pose is not None:
|
||||
actual_local = mount.actInv(scene.get_flange_pose(args.arm))
|
||||
position_error, orientation_error = pose_errors(actual_local, target_pose)
|
||||
print(f"Final position error: {position_error:.9f} m")
|
||||
print(f"Final orientation error: {orientation_error:.9f} rad")
|
||||
hold_until = perf_counter() + args.hold_after
|
||||
while viewer.is_running() and perf_counter() < hold_until:
|
||||
viewer.sync()
|
||||
sleep(0.02)
|
||||
if not args.close_after_play:
|
||||
while viewer.is_running():
|
||||
sleep(0.05)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
viewer.close()
|
||||
# GLFW tears down asynchronously; allow its render thread to exit.
|
||||
sleep(0.2)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,595 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from datetime import datetime, timezone
|
||||
from math import radians
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, List, Optional, Tuple
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
from PIL import Image
|
||||
|
||||
from .dual_arm import DualArmAssembly
|
||||
from .kinematics import RM75Kinematics, pose_errors
|
||||
from .mujoco_backend import CONTROLLED_HOME_Q_RAD, DualArmMuJoCo
|
||||
from .mujoco_trajectories import (
|
||||
DEMO_TRAJECTORIES,
|
||||
cartesian_demo_targets,
|
||||
joint_demo_trajectory,
|
||||
solve_pose_trajectory,
|
||||
)
|
||||
from .realman_reference import RealManFkReference
|
||||
from .solver import RM75IkSolver
|
||||
from .types import IkOptions, physical_joint_limits, teleop_joint_limits
|
||||
|
||||
|
||||
MUJOCO_PIN_POSITION_LIMIT_M = 1e-9
|
||||
MUJOCO_PIN_ORIENTATION_LIMIT_RAD = 1e-9
|
||||
ALGO_FK_POSITION_LIMIT_M = 1e-4
|
||||
ALGO_FK_ORIENTATION_LIMIT_RAD = radians(0.01)
|
||||
IK_POSITION_LIMIT_M = 1e-3
|
||||
IK_ORIENTATION_LIMIT_RAD = radians(0.1)
|
||||
NEAR_IK_RATE_LIMIT = 0.995
|
||||
CONTINUOUS_IK_RATE_LIMIT = 0.999
|
||||
MAX_JOINT_STEP_RAD = radians(2.0)
|
||||
INACTIVE_ARM_DELTA_LIMIT_RAD = 1e-12
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Stage2Settings:
|
||||
seed: int = 20260630
|
||||
fk_samples: int = 10_000
|
||||
ik_samples: int = 1_000
|
||||
trajectories: int = 20
|
||||
trajectory_points: int = 500
|
||||
render_width: int = 1280
|
||||
render_height: int = 720
|
||||
|
||||
@classmethod
|
||||
def quick(cls, seed: int = 20260630) -> "Stage2Settings":
|
||||
return cls(
|
||||
seed=seed,
|
||||
fk_samples=100,
|
||||
ik_samples=30,
|
||||
trajectories=2,
|
||||
trajectory_points=25,
|
||||
render_width=640,
|
||||
render_height=360,
|
||||
)
|
||||
|
||||
|
||||
def _sample_configurations(
|
||||
rng: np.random.Generator,
|
||||
lower: np.ndarray,
|
||||
upper: np.ndarray,
|
||||
count: int,
|
||||
margin: Optional[np.ndarray] = None,
|
||||
) -> np.ndarray:
|
||||
selected_margin = np.zeros(7) if margin is None else np.asarray(margin)
|
||||
return rng.uniform(
|
||||
lower + selected_margin,
|
||||
upper - selected_margin,
|
||||
size=(count, 7),
|
||||
)
|
||||
|
||||
|
||||
def _percentile(values: List[float], percentile: float) -> float:
|
||||
return float(np.percentile(values, percentile)) if values else float("nan")
|
||||
|
||||
|
||||
class Stage2Validator:
|
||||
def __init__(
|
||||
self,
|
||||
reference: RealManFkReference,
|
||||
controlled_arm: str = "left",
|
||||
settings: Stage2Settings = Stage2Settings(),
|
||||
) -> None:
|
||||
self.reference = reference
|
||||
self.controlled_arm = controlled_arm
|
||||
self.settings = settings
|
||||
self.rng = np.random.default_rng(settings.seed)
|
||||
self.scene = DualArmMuJoCo(controlled_arm=controlled_arm)
|
||||
self.assembly = DualArmAssembly.from_source_urdf()
|
||||
self.teleop_kinematics = RM75Kinematics(limits=teleop_joint_limits())
|
||||
self.solver = RM75IkSolver(self.teleop_kinematics)
|
||||
self.mount = (
|
||||
self.assembly.mounts.left_base
|
||||
if controlled_arm == "left"
|
||||
else self.assembly.mounts.right_base
|
||||
)
|
||||
self._inactive_initial = self.scene.get_arm_configuration(self.scene.inactive_arm)
|
||||
self._max_inactive_delta = 0.0
|
||||
self.checks: Dict[str, Dict[str, Any]] = {}
|
||||
self.failures: List[Dict[str, Any]] = []
|
||||
self.images: Dict[str, Tuple[np.ndarray, np.ndarray]] = {}
|
||||
|
||||
def _check_inactive_arm(self) -> None:
|
||||
delta = float(
|
||||
np.max(
|
||||
np.abs(
|
||||
self.scene.get_arm_configuration(self.scene.inactive_arm)
|
||||
- self._inactive_initial
|
||||
)
|
||||
)
|
||||
)
|
||||
self._max_inactive_delta = max(self._max_inactive_delta, delta)
|
||||
|
||||
def _record_failure(
|
||||
self,
|
||||
category: str,
|
||||
index: int,
|
||||
reason: str,
|
||||
q: Optional[np.ndarray] = None,
|
||||
position_error_m: float = float("nan"),
|
||||
orientation_error_rad: float = float("nan"),
|
||||
) -> None:
|
||||
if len(self.failures) >= 1000:
|
||||
return
|
||||
self.failures.append(
|
||||
{
|
||||
"category": category,
|
||||
"sample": index,
|
||||
"reason": reason,
|
||||
"position_error_m": position_error_m,
|
||||
"orientation_error_rad": orientation_error_rad,
|
||||
"q_rad": json.dumps(q.tolist()) if q is not None else "",
|
||||
}
|
||||
)
|
||||
|
||||
def _add_check(self, name: str, passed: bool, metrics: Dict[str, Any]) -> None:
|
||||
self.checks[name] = {"passed": bool(passed), "required": True, **metrics}
|
||||
|
||||
def _local_mujoco_pose(self) -> pin.SE3:
|
||||
return self.mount.actInv(self.scene.get_flange_pose(self.controlled_arm))
|
||||
|
||||
def run(self) -> Dict[str, Any]:
|
||||
self._model_structure_check()
|
||||
self._fk_check()
|
||||
self._single_point_ik_check()
|
||||
self._continuous_ik_check()
|
||||
self._trajectory_scenario_check()
|
||||
self._visual_check()
|
||||
self._add_check(
|
||||
"inactive_arm_fixed",
|
||||
self._max_inactive_delta < INACTIVE_ARM_DELTA_LIMIT_RAD,
|
||||
{
|
||||
"arm": self.scene.inactive_arm,
|
||||
"max_qpos_delta_rad": self._max_inactive_delta,
|
||||
"limit_rad": INACTIVE_ARM_DELTA_LIMIT_RAD,
|
||||
},
|
||||
)
|
||||
passed = all(check["passed"] for check in self.checks.values())
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"generated_at": datetime.now(timezone.utc).isoformat(),
|
||||
"seed": self.settings.seed,
|
||||
"controlled_arm": self.controlled_arm,
|
||||
"inactive_arm": self.scene.inactive_arm,
|
||||
"realman_api_version": self.reference.api_version,
|
||||
"mujoco_version": mujoco.__version__,
|
||||
"passed": passed,
|
||||
"checks": self.checks,
|
||||
"failure_count": len(self.failures),
|
||||
}
|
||||
|
||||
def _model_structure_check(self) -> None:
|
||||
model = self.scene.model
|
||||
joint_names = [model.joint(index).name for index in range(model.njnt)]
|
||||
expected = [
|
||||
f"{arm}_joint_{joint_index}"
|
||||
for arm in ("left", "right")
|
||||
for joint_index in range(1, 8)
|
||||
]
|
||||
obj_assets = [key for key in self.scene.assets if key.endswith(".obj")]
|
||||
stl_assets = [key for key in self.scene.assets if key.endswith(".STL")]
|
||||
passed = (
|
||||
model.nq == model.nv == model.njnt == 14
|
||||
and model.nu == 0
|
||||
and model.nsite == 2
|
||||
and model.nmesh == 27
|
||||
and joint_names == expected
|
||||
and len(obj_assets) == 19
|
||||
and len(stl_assets) == 8
|
||||
)
|
||||
self._add_check(
|
||||
"model_structure",
|
||||
passed,
|
||||
{
|
||||
"nq": model.nq,
|
||||
"nv": model.nv,
|
||||
"njnt": model.njnt,
|
||||
"nu": model.nu,
|
||||
"nmesh": model.nmesh,
|
||||
"obj_assets": len(obj_assets),
|
||||
"stl_assets": len(stl_assets),
|
||||
},
|
||||
)
|
||||
|
||||
def _fk_check(self) -> None:
|
||||
limits = physical_joint_limits()
|
||||
samples = _sample_configurations(
|
||||
self.rng, limits.lower, limits.upper, self.settings.fk_samples
|
||||
)
|
||||
pin_position_errors: List[float] = []
|
||||
pin_orientation_errors: List[float] = []
|
||||
algo_position_errors: List[float] = []
|
||||
algo_orientation_errors: List[float] = []
|
||||
for index, q in enumerate(samples):
|
||||
self.scene.set_arm_configuration(self.controlled_arm, q)
|
||||
self._check_inactive_arm()
|
||||
mujoco_world = self.scene.get_flange_pose(self.controlled_arm)
|
||||
pin_world = self.assembly.forward(self.controlled_arm, q)
|
||||
pin_position, pin_orientation = pose_errors(mujoco_world, pin_world)
|
||||
local_mujoco = self.mount.actInv(mujoco_world)
|
||||
algo_position, algo_orientation = pose_errors(
|
||||
local_mujoco, self.reference.forward(q)
|
||||
)
|
||||
pin_position_errors.append(pin_position)
|
||||
pin_orientation_errors.append(pin_orientation)
|
||||
algo_position_errors.append(algo_position)
|
||||
algo_orientation_errors.append(algo_orientation)
|
||||
if (
|
||||
pin_position >= MUJOCO_PIN_POSITION_LIMIT_M
|
||||
or pin_orientation >= MUJOCO_PIN_ORIENTATION_LIMIT_RAD
|
||||
or algo_position >= ALGO_FK_POSITION_LIMIT_M
|
||||
or algo_orientation >= ALGO_FK_ORIENTATION_LIMIT_RAD
|
||||
):
|
||||
self._record_failure(
|
||||
"fk",
|
||||
index,
|
||||
"MuJoCo FK residual exceeded an acceptance limit",
|
||||
q,
|
||||
algo_position,
|
||||
algo_orientation,
|
||||
)
|
||||
max_pin_position = max(pin_position_errors, default=float("inf"))
|
||||
max_pin_orientation = max(pin_orientation_errors, default=float("inf"))
|
||||
max_algo_position = max(algo_position_errors, default=float("inf"))
|
||||
max_algo_orientation = max(algo_orientation_errors, default=float("inf"))
|
||||
self._add_check(
|
||||
"fk",
|
||||
max_pin_position < MUJOCO_PIN_POSITION_LIMIT_M
|
||||
and max_pin_orientation < MUJOCO_PIN_ORIENTATION_LIMIT_RAD
|
||||
and max_algo_position < ALGO_FK_POSITION_LIMIT_M
|
||||
and max_algo_orientation < ALGO_FK_ORIENTATION_LIMIT_RAD,
|
||||
{
|
||||
"samples": len(samples),
|
||||
"max_mujoco_pin_position_error_m": max_pin_position,
|
||||
"max_mujoco_pin_orientation_error_rad": max_pin_orientation,
|
||||
"max_mujoco_algo_position_error_m": max_algo_position,
|
||||
"max_mujoco_algo_orientation_error_rad": max_algo_orientation,
|
||||
},
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _ik_options(max_iterations: int) -> IkOptions:
|
||||
return IkOptions(
|
||||
position_tolerance_m=0.9 * IK_POSITION_LIMIT_M,
|
||||
orientation_tolerance_rad=0.9 * IK_ORIENTATION_LIMIT_RAD,
|
||||
max_iterations=max_iterations,
|
||||
)
|
||||
|
||||
def _single_point_ik_check(self) -> None:
|
||||
limits = teleop_joint_limits()
|
||||
margin = np.deg2rad([5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 10.0])
|
||||
targets_q = _sample_configurations(
|
||||
self.rng,
|
||||
limits.lower,
|
||||
limits.upper,
|
||||
self.settings.ik_samples,
|
||||
margin,
|
||||
)
|
||||
successes = 0
|
||||
solve_times: List[float] = []
|
||||
max_position = 0.0
|
||||
max_orientation = 0.0
|
||||
for index, target_q in enumerate(targets_q):
|
||||
target = self.reference.forward(target_q)
|
||||
seed = np.clip(
|
||||
target_q + self.rng.uniform(-radians(10), radians(10), 7),
|
||||
limits.lower,
|
||||
limits.upper,
|
||||
)
|
||||
result = self.solver.solve(target, seed, self._ik_options(200))
|
||||
solve_times.append(result.solve_time_sec)
|
||||
if result.success and result.q is not None:
|
||||
self.scene.set_arm_configuration(self.controlled_arm, result.q)
|
||||
self._check_inactive_arm()
|
||||
position_error, orientation_error = pose_errors(
|
||||
self._local_mujoco_pose(), target
|
||||
)
|
||||
max_position = max(max_position, position_error)
|
||||
max_orientation = max(max_orientation, orientation_error)
|
||||
if (
|
||||
position_error <= IK_POSITION_LIMIT_M
|
||||
and orientation_error <= IK_ORIENTATION_LIMIT_RAD
|
||||
):
|
||||
successes += 1
|
||||
continue
|
||||
else:
|
||||
position_error = result.position_error_m
|
||||
orientation_error = result.orientation_error_rad
|
||||
self._record_failure(
|
||||
"single_point_ik",
|
||||
index,
|
||||
f"status={result.status.value}; {result.message}",
|
||||
seed,
|
||||
position_error,
|
||||
orientation_error,
|
||||
)
|
||||
rate = successes / max(len(targets_q), 1)
|
||||
self._add_check(
|
||||
"single_point_ik",
|
||||
rate >= NEAR_IK_RATE_LIMIT,
|
||||
{
|
||||
"samples": len(targets_q),
|
||||
"successes": successes,
|
||||
"success_rate": rate,
|
||||
"max_position_error_m": max_position,
|
||||
"max_orientation_error_rad": max_orientation,
|
||||
"p99_solve_time_sec": _percentile(solve_times, 99),
|
||||
"max_solve_time_sec": max(solve_times, default=float("nan")),
|
||||
},
|
||||
)
|
||||
|
||||
def _continuous_ik_check(self) -> None:
|
||||
limits = teleop_joint_limits()
|
||||
successes = 0
|
||||
total = 0
|
||||
max_joint_step = 0.0
|
||||
max_position = 0.0
|
||||
max_orientation = 0.0
|
||||
for trajectory_index in range(self.settings.trajectories):
|
||||
span = limits.upper - limits.lower
|
||||
center = self.rng.uniform(
|
||||
limits.lower + 0.3 * span,
|
||||
limits.upper - 0.3 * span,
|
||||
)
|
||||
amplitude = self.rng.uniform(0.015, 0.04, 7) * span
|
||||
frequency = self.rng.uniform(0.03, 0.08, 7)
|
||||
phase = self.rng.uniform(-np.pi, np.pi, 7)
|
||||
times = np.arange(self.settings.trajectory_points) / 90.0
|
||||
target_path = center + amplitude * np.sin(
|
||||
2.0 * np.pi * times[:, None] * frequency + phase
|
||||
)
|
||||
seed = target_path[0].copy()
|
||||
previous = seed.copy()
|
||||
for point_index, target_q in enumerate(target_path):
|
||||
total += 1
|
||||
target = self.reference.forward(target_q)
|
||||
result = self.solver.solve(target, seed, self._ik_options(100))
|
||||
if result.success and result.q is not None:
|
||||
self.scene.set_arm_configuration(self.controlled_arm, result.q)
|
||||
self._check_inactive_arm()
|
||||
position_error, orientation_error = pose_errors(
|
||||
self._local_mujoco_pose(), target
|
||||
)
|
||||
joint_step = float(np.max(np.abs(result.q - previous)))
|
||||
max_position = max(max_position, position_error)
|
||||
max_orientation = max(max_orientation, orientation_error)
|
||||
max_joint_step = max(max_joint_step, joint_step)
|
||||
if (
|
||||
position_error <= IK_POSITION_LIMIT_M
|
||||
and orientation_error <= IK_ORIENTATION_LIMIT_RAD
|
||||
):
|
||||
successes += 1
|
||||
seed = result.q
|
||||
previous = result.q
|
||||
continue
|
||||
else:
|
||||
position_error = result.position_error_m
|
||||
orientation_error = result.orientation_error_rad
|
||||
self._record_failure(
|
||||
"continuous_ik",
|
||||
trajectory_index * self.settings.trajectory_points + point_index,
|
||||
f"status={result.status.value}; {result.message}",
|
||||
seed,
|
||||
position_error,
|
||||
orientation_error,
|
||||
)
|
||||
rate = successes / max(total, 1)
|
||||
self._add_check(
|
||||
"continuous_ik",
|
||||
rate >= CONTINUOUS_IK_RATE_LIMIT
|
||||
and max_joint_step < MAX_JOINT_STEP_RAD,
|
||||
{
|
||||
"trajectories": self.settings.trajectories,
|
||||
"points": total,
|
||||
"successes": successes,
|
||||
"success_rate": rate,
|
||||
"max_position_error_m": max_position,
|
||||
"max_orientation_error_rad": max_orientation,
|
||||
"max_joint_step_rad": max_joint_step,
|
||||
"joint_step_limit_rad": MAX_JOINT_STEP_RAD,
|
||||
},
|
||||
)
|
||||
|
||||
def _trajectory_scenario_check(self) -> None:
|
||||
scenario_results: Dict[str, bool] = {}
|
||||
start = CONTROLLED_HOME_Q_RAD.copy()
|
||||
for kind in DEMO_TRAJECTORIES:
|
||||
try:
|
||||
if kind == "joint":
|
||||
q_trajectory = joint_demo_trajectory(start, 120)
|
||||
else:
|
||||
targets = cartesian_demo_targets(
|
||||
kind, self.teleop_kinematics.forward(start), 120
|
||||
)
|
||||
q_trajectory = solve_pose_trajectory(self.solver, targets, start)
|
||||
self.scene.play_trajectory(q_trajectory)
|
||||
self._check_inactive_arm()
|
||||
scenario_results[kind] = True
|
||||
except (RuntimeError, ValueError) as exc:
|
||||
scenario_results[kind] = False
|
||||
self._record_failure("trajectory_scenario", 0, f"{kind}: {exc}")
|
||||
|
||||
limits = teleop_joint_limits()
|
||||
scenario_q = {
|
||||
"limit_near": 0.8 * limits.upper + 0.2 * CONTROLLED_HOME_Q_RAD,
|
||||
"singularity_near": np.deg2rad([0, 0, 0, 90, 0, 0, 0]),
|
||||
}
|
||||
blend = 0.5 - 0.5 * np.cos(np.linspace(0.0, np.pi, 120))
|
||||
for name, endpoint in scenario_q.items():
|
||||
path = start[None, :] + blend[:, None] * (endpoint - start)[None, :]
|
||||
try:
|
||||
targets = [self.reference.forward(q) for q in path]
|
||||
solutions = solve_pose_trajectory(self.solver, targets, start)
|
||||
self.scene.play_trajectory(solutions)
|
||||
self._check_inactive_arm()
|
||||
scenario_results[name] = True
|
||||
except (RuntimeError, ValueError) as exc:
|
||||
scenario_results[name] = False
|
||||
self._record_failure("trajectory_scenario", 0, f"{name}: {exc}")
|
||||
self._add_check(
|
||||
"trajectory_scenarios",
|
||||
all(scenario_results.values()),
|
||||
{"scenarios": scenario_results},
|
||||
)
|
||||
|
||||
def _visible_geom_names(self, segmentation: np.ndarray) -> set[str]:
|
||||
pairs = np.unique(segmentation.reshape(-1, 2), axis=0)
|
||||
names = set()
|
||||
for object_id, object_type in pairs:
|
||||
if object_type != mujoco.mjtObj.mjOBJ_GEOM or object_id < 0:
|
||||
continue
|
||||
name = mujoco.mj_id2name(
|
||||
self.scene.model, mujoco.mjtObj.mjOBJ_GEOM, int(object_id)
|
||||
)
|
||||
if name is not None:
|
||||
names.add(name)
|
||||
return names
|
||||
|
||||
@staticmethod
|
||||
def _segmentation_preview(segmentation: np.ndarray) -> np.ndarray:
|
||||
object_ids = segmentation[:, :, 0]
|
||||
preview = np.zeros((*object_ids.shape, 3), dtype=np.uint8)
|
||||
foreground = object_ids >= 0
|
||||
ids = object_ids[foreground].astype(np.uint32)
|
||||
preview[foreground, 0] = (37 * ids + 53) % 255
|
||||
preview[foreground, 1] = (97 * ids + 101) % 255
|
||||
preview[foreground, 2] = (17 * ids + 211) % 255
|
||||
return preview
|
||||
|
||||
def _visual_check(self) -> None:
|
||||
start = CONTROLLED_HOME_Q_RAD.copy()
|
||||
targets = cartesian_demo_targets(
|
||||
"combined", self.teleop_kinematics.forward(start), 121
|
||||
)
|
||||
q_trajectory = solve_pose_trajectory(self.solver, targets, start)
|
||||
all_frames_pass = True
|
||||
frame_metrics: Dict[str, Dict[str, Any]] = {}
|
||||
for label, index in (("start", 0), ("middle", 60), ("end", 120)):
|
||||
self.scene.set_arm_configuration(self.controlled_arm, q_trajectory[index])
|
||||
self.scene.set_target_marker(self.mount * targets[index])
|
||||
self._check_inactive_arm()
|
||||
rgb = self.scene.render(
|
||||
self.settings.render_width, self.settings.render_height
|
||||
)
|
||||
segmentation = self.scene.render(
|
||||
self.settings.render_width,
|
||||
self.settings.render_height,
|
||||
segmentation=True,
|
||||
)
|
||||
visible_names = self._visible_geom_names(segmentation)
|
||||
left_visible = any(name.startswith("left_") for name in visible_names)
|
||||
right_visible = any(name.startswith("right_") for name in visible_names)
|
||||
arm_ids = {
|
||||
mujoco.mj_name2id(
|
||||
self.scene.model, mujoco.mjtObj.mjOBJ_GEOM, name
|
||||
)
|
||||
for name in visible_names
|
||||
if name.startswith(("left_", "right_"))
|
||||
}
|
||||
mask = np.isin(segmentation[:, :, 0], list(arm_ids))
|
||||
rows, columns = np.where(mask)
|
||||
not_touching_border = bool(
|
||||
len(rows)
|
||||
and rows.min() > 1
|
||||
and columns.min() > 1
|
||||
and rows.max() < rgb.shape[0] - 2
|
||||
and columns.max() < rgb.shape[1] - 2
|
||||
)
|
||||
frame_pass = (
|
||||
float(rgb.std()) > 5.0
|
||||
and left_visible
|
||||
and right_visible
|
||||
and not_touching_border
|
||||
)
|
||||
all_frames_pass &= frame_pass
|
||||
frame_metrics[label] = {
|
||||
"rgb_std": float(rgb.std()),
|
||||
"left_visible": left_visible,
|
||||
"right_visible": right_visible,
|
||||
"not_touching_border": not_touching_border,
|
||||
"visible_geom_count": len(visible_names),
|
||||
}
|
||||
self.images[label] = (rgb, self._segmentation_preview(segmentation))
|
||||
self._add_check("visual_rendering", all_frames_pass, {"frames": frame_metrics})
|
||||
|
||||
|
||||
def write_stage2_report(
|
||||
output_dir: Path | str,
|
||||
summary: Dict[str, Any],
|
||||
failures: List[Dict[str, Any]],
|
||||
images: Dict[str, Tuple[np.ndarray, np.ndarray]],
|
||||
) -> Tuple[Path, Path, Path, List[Path]]:
|
||||
directory = Path(output_dir)
|
||||
directory.mkdir(parents=True, exist_ok=True)
|
||||
json_path = directory / "stage2_summary.json"
|
||||
csv_path = directory / "stage2_failures.csv"
|
||||
markdown_path = directory / "stage2_report.md"
|
||||
|
||||
with json_path.open("w", encoding="utf-8") as stream:
|
||||
json.dump(summary, stream, ensure_ascii=True, indent=2, sort_keys=True)
|
||||
stream.write("\n")
|
||||
|
||||
fields = [
|
||||
"category",
|
||||
"sample",
|
||||
"reason",
|
||||
"position_error_m",
|
||||
"orientation_error_rad",
|
||||
"q_rad",
|
||||
]
|
||||
with csv_path.open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
||||
writer.writeheader()
|
||||
writer.writerows(failures)
|
||||
|
||||
image_paths: List[Path] = []
|
||||
for label, (rgb, segmentation) in images.items():
|
||||
for suffix, image in (("rgb", rgb), ("segmentation", segmentation)):
|
||||
path = directory / f"stage2_{label}_{suffix}.png"
|
||||
Image.fromarray(image).save(path)
|
||||
image_paths.append(path)
|
||||
|
||||
lines = [
|
||||
"# RM75-B Stage 2 MuJoCo Validation",
|
||||
"",
|
||||
f"- Overall: **{'PASS' if summary['passed'] else 'FAIL'}**",
|
||||
f"- Controlled arm: `{summary['controlled_arm']}`",
|
||||
f"- Seed: `{summary['seed']}`",
|
||||
f"- MuJoCo: `{summary['mujoco_version']}`",
|
||||
f"- RealMan API: `{summary['realman_api_version']}`",
|
||||
f"- Failures recorded: `{summary['failure_count']}`",
|
||||
"",
|
||||
"| Check | Result | Metrics |",
|
||||
"|---|---:|---|",
|
||||
]
|
||||
for name, check in summary["checks"].items():
|
||||
metrics = {
|
||||
key: value
|
||||
for key, value in check.items()
|
||||
if key not in {"passed", "required"}
|
||||
}
|
||||
lines.append(
|
||||
f"| `{name}` | {'PASS' if check['passed'] else 'FAIL'} | "
|
||||
f"`{json.dumps(metrics, ensure_ascii=True, sort_keys=True)}` |"
|
||||
)
|
||||
markdown_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||||
return json_path, csv_path, markdown_path, image_paths
|
||||
|
||||
@@ -0,0 +1,78 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Optional, Sequence
|
||||
|
||||
|
||||
os.environ.setdefault("MUJOCO_GL", "egl")
|
||||
|
||||
|
||||
def _source_root() -> Path:
|
||||
return Path(__file__).resolve().parents[3]
|
||||
|
||||
|
||||
def build_parser() -> argparse.ArgumentParser:
|
||||
root = _source_root()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Validate right-arm and dual-grip QP control in MuJoCo"
|
||||
)
|
||||
parser.add_argument("--sdk-root", type=Path)
|
||||
parser.add_argument(
|
||||
"--teleop-config",
|
||||
type=Path,
|
||||
default=root / "xr_rm_bringup/config/dual_arm_rm75.yaml",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--peripheral-config",
|
||||
type=Path,
|
||||
default=root / "xr_rm_bringup/config/peripherals_rm75.yaml",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output-dir",
|
||||
type=Path,
|
||||
default=root / "ik_qp/artifacts/stage3",
|
||||
)
|
||||
parser.add_argument("--seed", type=int, default=20260630)
|
||||
parser.add_argument("--quick", action="store_true")
|
||||
parser.add_argument("--report-only", action="store_true")
|
||||
return parser
|
||||
|
||||
|
||||
def main(argv: Optional[Sequence[str]] = None) -> int:
|
||||
args = build_parser().parse_args(argv)
|
||||
from .realman_reference import RealManFkReference
|
||||
from .stage3_validation import (
|
||||
Stage3Settings,
|
||||
Stage3Validator,
|
||||
write_stage3_report,
|
||||
)
|
||||
|
||||
settings = (
|
||||
Stage3Settings.quick(args.seed)
|
||||
if args.quick
|
||||
else Stage3Settings(seed=args.seed)
|
||||
)
|
||||
validator = Stage3Validator(
|
||||
RealManFkReference(args.sdk_root),
|
||||
args.teleop_config,
|
||||
args.peripheral_config,
|
||||
settings,
|
||||
)
|
||||
summary = validator.run()
|
||||
paths = write_stage3_report(
|
||||
args.output_dir, summary, validator.failures, validator.images
|
||||
)
|
||||
print(f"RM75-B stage-3 validation: {'PASS' if summary['passed'] else 'FAIL'}")
|
||||
for name, check in summary["checks"].items():
|
||||
print(f" [{'PASS' if check['passed'] else 'FAIL'}] {name}")
|
||||
print("Reports:")
|
||||
for path in (*paths[:3], *paths[3]):
|
||||
print(f" {path}")
|
||||
return 0 if args.report_only or summary["passed"] else 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,311 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from datetime import datetime, timezone
|
||||
from math import degrees, radians
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, List, Tuple
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
|
||||
from .mujoco_robot import MujocoRobot
|
||||
from .realman_reference import RealManFkReference
|
||||
from .stage2_validation import Stage2Settings, Stage2Validator
|
||||
from .teleop_config import load_dual_arm_profiles
|
||||
from .teleop_control import (
|
||||
ControllerSample,
|
||||
DualArmQpTeleopController,
|
||||
SafetyState,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Stage3Settings:
|
||||
seed: int = 20260630
|
||||
fk_samples: int = 2_000
|
||||
ik_samples: int = 200
|
||||
trajectories: int = 5
|
||||
trajectory_points: int = 200
|
||||
render_width: int = 1280
|
||||
render_height: int = 720
|
||||
|
||||
@classmethod
|
||||
def quick(cls, seed: int = 20260630) -> "Stage3Settings":
|
||||
return cls(
|
||||
seed=seed,
|
||||
fk_samples=100,
|
||||
ik_samples=30,
|
||||
trajectories=2,
|
||||
trajectory_points=25,
|
||||
render_width=640,
|
||||
render_height=360,
|
||||
)
|
||||
|
||||
def stage2_settings(self) -> Stage2Settings:
|
||||
return Stage2Settings(
|
||||
seed=self.seed,
|
||||
fk_samples=self.fk_samples,
|
||||
ik_samples=self.ik_samples,
|
||||
trajectories=self.trajectories,
|
||||
trajectory_points=self.trajectory_points,
|
||||
render_width=self.render_width,
|
||||
render_height=self.render_height,
|
||||
)
|
||||
|
||||
|
||||
class Stage3Validator:
|
||||
def __init__(
|
||||
self,
|
||||
reference: RealManFkReference,
|
||||
teleop_config_path: Path | str,
|
||||
peripheral_config_path: Path | str,
|
||||
settings: Stage3Settings = Stage3Settings(),
|
||||
) -> None:
|
||||
self.reference = reference
|
||||
self.settings = settings
|
||||
self.profiles = load_dual_arm_profiles(
|
||||
teleop_config_path, peripheral_config_path
|
||||
)
|
||||
self.checks: Dict[str, Dict[str, Any]] = {}
|
||||
self.failures: List[Dict[str, Any]] = []
|
||||
self.images: Dict[str, np.ndarray] = {}
|
||||
|
||||
def run(self) -> Dict[str, Any]:
|
||||
right_validator = Stage2Validator(
|
||||
self.reference,
|
||||
controlled_arm="right",
|
||||
settings=self.settings.stage2_settings(),
|
||||
)
|
||||
right_summary = right_validator.run()
|
||||
self.failures.extend(right_validator.failures)
|
||||
for label, (rgb, segmentation) in right_validator.images.items():
|
||||
self.images[f"right_{label}_rgb"] = rgb
|
||||
self.images[f"right_{label}_segmentation"] = segmentation
|
||||
self.checks["right_arm_validation"] = {
|
||||
"passed": right_summary["passed"],
|
||||
"required": True,
|
||||
"checks": right_summary["checks"],
|
||||
}
|
||||
|
||||
robot = MujocoRobot(self.profiles)
|
||||
try:
|
||||
self._initial_tcp_check(robot)
|
||||
self._dual_grip_control_check(robot)
|
||||
image = robot.render(
|
||||
self.settings.render_width, self.settings.render_height
|
||||
)
|
||||
self.images["dual_control_final_rgb"] = image
|
||||
self.checks["dual_control_render"] = {
|
||||
"passed": float(image.std()) > 5.0,
|
||||
"required": True,
|
||||
"rgb_std": float(image.std()),
|
||||
}
|
||||
finally:
|
||||
robot.close()
|
||||
|
||||
passed = all(check["passed"] for check in self.checks.values())
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"generated_at": datetime.now(timezone.utc).isoformat(),
|
||||
"seed": self.settings.seed,
|
||||
"mujoco_version": mujoco.__version__,
|
||||
"realman_api_version": self.reference.api_version,
|
||||
"passed": passed,
|
||||
"checks": self.checks,
|
||||
"failure_count": len(self.failures),
|
||||
}
|
||||
|
||||
def _initial_tcp_check(self, robot: MujocoRobot) -> None:
|
||||
metrics: Dict[str, Dict[str, Any]] = {}
|
||||
passed = True
|
||||
for arm in ("left", "right"):
|
||||
diagnostic = robot.initial_pose_diagnostics[arm]
|
||||
arm_passed = (
|
||||
diagnostic.solved_position_error_m <= 1e-3
|
||||
and diagnostic.solved_orientation_error_rad <= radians(0.1)
|
||||
)
|
||||
passed &= arm_passed
|
||||
metrics[arm] = {
|
||||
"active_tool": self.profiles[arm].active_tool_name,
|
||||
"solved_q_deg": np.rad2deg(diagnostic.solved_q_rad).tolist(),
|
||||
"solved_position_error_m": diagnostic.solved_position_error_m,
|
||||
"solved_orientation_error_deg": degrees(
|
||||
diagnostic.solved_orientation_error_rad
|
||||
),
|
||||
"configured_joint_position_error_m": (
|
||||
diagnostic.configured_joint_position_error_m
|
||||
),
|
||||
"configured_joint_orientation_error_deg": degrees(
|
||||
diagnostic.configured_joint_orientation_error_rad
|
||||
),
|
||||
}
|
||||
if not arm_passed:
|
||||
self._record_failure(
|
||||
"initial_tcp",
|
||||
0 if arm == "left" else 1,
|
||||
f"{arm} initial TCP solve exceeded tolerance",
|
||||
diagnostic.solved_q_rad,
|
||||
diagnostic.solved_position_error_m,
|
||||
diagnostic.solved_orientation_error_rad,
|
||||
)
|
||||
self.checks["initial_tcp_pose"] = {
|
||||
"passed": passed,
|
||||
"required": True,
|
||||
"arms": metrics,
|
||||
"note": "configured initial_joint_pose mismatch is diagnostic only",
|
||||
}
|
||||
|
||||
def _dual_grip_control_check(self, robot: MujocoRobot) -> None:
|
||||
controller = DualArmQpTeleopController(robot, self.profiles)
|
||||
initial = robot.read_joint_positions().positions_rad
|
||||
identity = np.array([0.0, 0.0, 0.0, 1.0])
|
||||
|
||||
def sample(arm: str, grip: bool, position) -> ControllerSample:
|
||||
return ControllerSample(
|
||||
hand=arm,
|
||||
grip=grip,
|
||||
trigger=0.0,
|
||||
position_m=np.asarray(position, dtype=float),
|
||||
quaternion_xyzw=identity,
|
||||
)
|
||||
|
||||
controller.update_sample(sample("left", False, [0, 0, 0]), 0.0)
|
||||
controller.update_sample(sample("right", False, [0, 0, 0]), 0.0)
|
||||
idle = controller.step(0.0)
|
||||
controller.update_sample(sample("left", True, [0, 0, 0]), 0.01)
|
||||
controller.update_sample(sample("right", True, [0, 0, 0]), 0.01)
|
||||
engaged = controller.step(0.01)
|
||||
after_engage = robot.read_joint_positions().positions_rad
|
||||
no_engage_jump = all(
|
||||
np.max(np.abs(after_engage[arm] - initial[arm])) < 1e-10
|
||||
for arm in ("left", "right")
|
||||
)
|
||||
|
||||
last_result = engaged
|
||||
for index in range(1, 11):
|
||||
now = 0.01 + index / 90.0
|
||||
hand_delta = 0.001 * index
|
||||
controller.update_sample(
|
||||
sample("left", True, [0, hand_delta, 0]), now
|
||||
)
|
||||
controller.update_sample(
|
||||
sample("right", True, [0, hand_delta, 0]), now
|
||||
)
|
||||
last_result = controller.step(now)
|
||||
if last_result.state is SafetyState.FAULT:
|
||||
break
|
||||
moved = robot.read_joint_positions().positions_rad
|
||||
joint_delta = {
|
||||
arm: float(np.max(np.abs(moved[arm] - initial[arm])))
|
||||
for arm in ("left", "right")
|
||||
}
|
||||
both_moved = all(value > 1e-5 for value in joint_delta.values())
|
||||
|
||||
release_time = 0.15
|
||||
controller.update_sample(sample("left", False, [0, 0.01, 0]), release_time)
|
||||
controller.update_sample(sample("right", False, [0, 0.01, 0]), release_time)
|
||||
released = controller.step(release_time)
|
||||
passed = (
|
||||
idle.state is SafetyState.IDLE
|
||||
and engaged.state is SafetyState.ACTIVE
|
||||
and no_engage_jump
|
||||
and last_result.state is SafetyState.ACTIVE
|
||||
and both_moved
|
||||
and released.state is SafetyState.IDLE
|
||||
)
|
||||
if not passed:
|
||||
self._record_failure(
|
||||
"dual_grip_control",
|
||||
0,
|
||||
f"idle={idle.state.value}, engaged={engaged.state.value}, "
|
||||
f"last={last_result.state.value}, released={released.state.value}",
|
||||
)
|
||||
self.checks["dual_grip_control"] = {
|
||||
"passed": passed,
|
||||
"required": True,
|
||||
"no_engage_jump": no_engage_jump,
|
||||
"max_joint_delta_rad": joint_delta,
|
||||
"final_state": released.state.value,
|
||||
}
|
||||
|
||||
def _record_failure(
|
||||
self,
|
||||
category: str,
|
||||
index: int,
|
||||
reason: str,
|
||||
q: np.ndarray | None = None,
|
||||
position_error_m: float = float("nan"),
|
||||
orientation_error_rad: float = float("nan"),
|
||||
) -> None:
|
||||
self.failures.append(
|
||||
{
|
||||
"category": category,
|
||||
"sample": index,
|
||||
"reason": reason,
|
||||
"position_error_m": position_error_m,
|
||||
"orientation_error_rad": orientation_error_rad,
|
||||
"q_rad": json.dumps(q.tolist()) if q is not None else "",
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
def write_stage3_report(
|
||||
output_dir: Path | str,
|
||||
summary: Dict[str, Any],
|
||||
failures: List[Dict[str, Any]],
|
||||
images: Dict[str, np.ndarray],
|
||||
) -> Tuple[Path, Path, Path, List[Path]]:
|
||||
directory = Path(output_dir)
|
||||
directory.mkdir(parents=True, exist_ok=True)
|
||||
json_path = directory / "stage3_summary.json"
|
||||
csv_path = directory / "stage3_failures.csv"
|
||||
markdown_path = directory / "stage3_report.md"
|
||||
json_path.write_text(
|
||||
json.dumps(summary, ensure_ascii=True, indent=2, sort_keys=True) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
fields = [
|
||||
"category",
|
||||
"sample",
|
||||
"reason",
|
||||
"position_error_m",
|
||||
"orientation_error_rad",
|
||||
"q_rad",
|
||||
]
|
||||
with csv_path.open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
||||
writer.writeheader()
|
||||
writer.writerows(failures)
|
||||
image_paths = []
|
||||
for label, image in images.items():
|
||||
path = directory / f"stage3_{label}.png"
|
||||
Image.fromarray(image).save(path)
|
||||
image_paths.append(path)
|
||||
lines = [
|
||||
"# RM75-B Stage 3 Dual-Arm QP Validation",
|
||||
"",
|
||||
f"- Overall: **{'PASS' if summary['passed'] else 'FAIL'}**",
|
||||
f"- Seed: `{summary['seed']}`",
|
||||
f"- MuJoCo: `{summary['mujoco_version']}`",
|
||||
f"- RealMan API: `{summary['realman_api_version']}`",
|
||||
f"- Failures recorded: `{summary['failure_count']}`",
|
||||
"",
|
||||
"| Check | Result | Metrics |",
|
||||
"|---|---:|---|",
|
||||
]
|
||||
for name, check in summary["checks"].items():
|
||||
metrics = {
|
||||
key: value
|
||||
for key, value in check.items()
|
||||
if key not in {"passed", "required"}
|
||||
}
|
||||
lines.append(
|
||||
f"| `{name}` | {'PASS' if check['passed'] else 'FAIL'} | "
|
||||
f"`{json.dumps(metrics, ensure_ascii=True, sort_keys=True)}` |"
|
||||
)
|
||||
markdown_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||||
return json_path, csv_path, markdown_path, image_paths
|
||||
@@ -0,0 +1,197 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, Literal, Mapping
|
||||
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
import yaml
|
||||
|
||||
from .kinematics import validate_se3
|
||||
|
||||
|
||||
ArmName = Literal["left", "right"]
|
||||
|
||||
|
||||
def _readonly_vector(values, shape: tuple[int, ...], name: str) -> np.ndarray:
|
||||
array = np.asarray(values, dtype=float).copy()
|
||||
if array.shape != shape or not np.all(np.isfinite(array)):
|
||||
raise ValueError(f"{name} must be finite with shape {shape}")
|
||||
array.setflags(write=False)
|
||||
return array
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ArmTeleopProfile:
|
||||
arm: ArmName
|
||||
initial_tcp: pin.SE3
|
||||
configured_initial_q_rad: np.ndarray
|
||||
tool_from_flange: pin.SE3
|
||||
active_tool_name: str
|
||||
xr_to_robot: np.ndarray
|
||||
scale: float
|
||||
command_timeout_sec: float
|
||||
deadband_m: float
|
||||
target_filter_alpha: float
|
||||
target_filter_alpha_fast: float
|
||||
target_filter_fast_threshold_m: float
|
||||
max_linear_speed_m_s: float
|
||||
enable_position_axes: tuple[bool, bool, bool]
|
||||
enable_orientation_control: bool
|
||||
enable_orientation_axes: tuple[bool, bool, bool]
|
||||
orientation_deadband_rad: float
|
||||
orientation_filter_alpha: float
|
||||
max_orientation_speed_rad_s: float
|
||||
workspace_min: np.ndarray
|
||||
workspace_max: np.ndarray
|
||||
cylinder_radius_limit: np.ndarray
|
||||
low_z_threshold: float
|
||||
low_z_min_radius: float
|
||||
joint_max_speed_rad_s: np.ndarray
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if self.arm not in ("left", "right"):
|
||||
raise ValueError("arm must be 'left' or 'right'")
|
||||
validate_se3(self.initial_tcp, "initial_tcp")
|
||||
validate_se3(self.tool_from_flange, "tool_from_flange")
|
||||
object.__setattr__(
|
||||
self,
|
||||
"configured_initial_q_rad",
|
||||
_readonly_vector(self.configured_initial_q_rad, (7,), "configured_initial_q_rad"),
|
||||
)
|
||||
matrix = _readonly_vector(self.xr_to_robot, (3, 3), "xr_to_robot")
|
||||
if not np.allclose(matrix.T @ matrix, np.eye(3), atol=1e-9):
|
||||
raise ValueError("xr_to_robot must be orthonormal")
|
||||
if not np.isclose(np.linalg.det(matrix), 1.0, atol=1e-9):
|
||||
raise ValueError("xr_to_robot must be a proper rotation")
|
||||
object.__setattr__(self, "xr_to_robot", matrix)
|
||||
workspace_min = _readonly_vector(self.workspace_min, (3,), "workspace_min")
|
||||
workspace_max = _readonly_vector(self.workspace_max, (3,), "workspace_max")
|
||||
if np.any(workspace_min >= workspace_max):
|
||||
raise ValueError("workspace_min must be below workspace_max")
|
||||
object.__setattr__(self, "workspace_min", workspace_min)
|
||||
object.__setattr__(self, "workspace_max", workspace_max)
|
||||
cylinder = _readonly_vector(
|
||||
self.cylinder_radius_limit, (2,), "cylinder_radius_limit"
|
||||
)
|
||||
if cylinder[0] < 0.0 or cylinder[1] <= cylinder[0]:
|
||||
raise ValueError("cylinder radius limits are invalid")
|
||||
object.__setattr__(self, "cylinder_radius_limit", cylinder)
|
||||
speeds = _readonly_vector(
|
||||
self.joint_max_speed_rad_s, (7,), "joint_max_speed_rad_s"
|
||||
)
|
||||
if np.any(speeds <= 0.0):
|
||||
raise ValueError("joint_max_speed_rad_s must be positive")
|
||||
object.__setattr__(self, "joint_max_speed_rad_s", speeds)
|
||||
for name in (
|
||||
"scale",
|
||||
"command_timeout_sec",
|
||||
"target_filter_fast_threshold_m",
|
||||
"max_linear_speed_m_s",
|
||||
"max_orientation_speed_rad_s",
|
||||
):
|
||||
if not np.isfinite(getattr(self, name)) or getattr(self, name) <= 0.0:
|
||||
raise ValueError(f"{name} must be finite and positive")
|
||||
for name in (
|
||||
"deadband_m",
|
||||
"orientation_deadband_rad",
|
||||
"low_z_threshold",
|
||||
"low_z_min_radius",
|
||||
):
|
||||
if not np.isfinite(getattr(self, name)) or getattr(self, name) < 0.0:
|
||||
raise ValueError(f"{name} must be finite and non-negative")
|
||||
for name in (
|
||||
"target_filter_alpha",
|
||||
"target_filter_alpha_fast",
|
||||
"orientation_filter_alpha",
|
||||
):
|
||||
if not 0.0 <= getattr(self, name) <= 1.0:
|
||||
raise ValueError(f"{name} must be in [0, 1]")
|
||||
|
||||
|
||||
def _pose_from_rpy(values, name: str) -> pin.SE3:
|
||||
pose = _readonly_vector(values, (6,), name)
|
||||
return pin.SE3(pin.rpy.rpyToMatrix(*pose[3:]), pose[:3])
|
||||
|
||||
|
||||
def _tool_pose(values, name: str) -> pin.SE3:
|
||||
pose = _readonly_vector(values, (7,), name)
|
||||
quaternion = pin.Quaternion(pose[6], pose[3], pose[4], pose[5])
|
||||
if quaternion.norm() <= 1e-12:
|
||||
raise ValueError(f"{name} quaternion has zero norm")
|
||||
quaternion.normalize()
|
||||
return pin.SE3(quaternion.matrix(), pose[:3])
|
||||
|
||||
|
||||
def load_dual_arm_profiles(
|
||||
teleop_config_path: Path | str,
|
||||
peripheral_config_path: Path | str,
|
||||
) -> Dict[ArmName, ArmTeleopProfile]:
|
||||
teleop_path = Path(teleop_config_path)
|
||||
peripheral_path = Path(peripheral_config_path)
|
||||
with teleop_path.open("r", encoding="utf-8") as stream:
|
||||
teleop_document = yaml.safe_load(stream)
|
||||
with peripheral_path.open("r", encoding="utf-8") as stream:
|
||||
peripheral_document = yaml.safe_load(stream)
|
||||
if not isinstance(teleop_document, Mapping) or not isinstance(
|
||||
peripheral_document, Mapping
|
||||
):
|
||||
raise ValueError("teleop and peripheral YAML documents must be mappings")
|
||||
|
||||
tools = peripheral_document.get("tools_in_ee")
|
||||
arms = peripheral_document.get("arms")
|
||||
if not isinstance(tools, Mapping) or not isinstance(arms, Mapping):
|
||||
raise ValueError("peripheral configuration is missing tools_in_ee or arms")
|
||||
tool_names = list(tools)
|
||||
|
||||
profiles: Dict[ArmName, ArmTeleopProfile] = {}
|
||||
for arm in ("left", "right"):
|
||||
section_name = f"{arm}_arm_teleop"
|
||||
section = teleop_document.get(section_name)
|
||||
if not isinstance(section, Mapping):
|
||||
raise ValueError(f"missing {section_name} configuration")
|
||||
params = section.get("ros__parameters")
|
||||
if not isinstance(params, Mapping):
|
||||
raise ValueError(f"{section_name} is missing ros__parameters")
|
||||
arm_tool = arms.get(arm)
|
||||
if not isinstance(arm_tool, Mapping):
|
||||
raise ValueError(f"peripheral configuration is missing arm {arm}")
|
||||
tool_index = int(arm_tool.get("scissorgripper"))
|
||||
try:
|
||||
tool_name = tool_names[tool_index]
|
||||
tool_values = tools[tool_name]["pose"]
|
||||
except (IndexError, KeyError, TypeError) as exc:
|
||||
raise ValueError(f"invalid active tool selection for {arm}") from exc
|
||||
|
||||
joint_speed = float(params["joint_max_speed"])
|
||||
profiles[arm] = ArmTeleopProfile(
|
||||
arm=arm,
|
||||
initial_tcp=_pose_from_rpy(params["initial_tcp_pose"], f"{arm}.initial_tcp_pose"),
|
||||
configured_initial_q_rad=np.deg2rad(params["initial_joint_pose"]),
|
||||
tool_from_flange=_tool_pose(tool_values, f"tools.{tool_name}.pose"),
|
||||
active_tool_name=tool_name,
|
||||
xr_to_robot=np.asarray(params["xr_to_robot_matrix"], dtype=float).reshape(3, 3),
|
||||
scale=float(params["scale"]),
|
||||
command_timeout_sec=float(params["command_timeout_sec"]),
|
||||
deadband_m=float(params["deadband_m"]),
|
||||
target_filter_alpha=float(params["target_filter_alpha"]),
|
||||
target_filter_alpha_fast=float(params["target_filter_alpha_fast"]),
|
||||
target_filter_fast_threshold_m=float(
|
||||
params["target_filter_fast_threshold_m"]
|
||||
),
|
||||
max_linear_speed_m_s=float(params["max_linear_speed"]),
|
||||
enable_position_axes=tuple(bool(value) for value in params["enable_position_axes"]),
|
||||
enable_orientation_control=bool(params["enable_orientation_control"]),
|
||||
enable_orientation_axes=tuple(bool(value) for value in params["enable_orientation_axes"]),
|
||||
orientation_deadband_rad=float(params["orientation_deadband_rad"]),
|
||||
orientation_filter_alpha=float(params["orientation_filter_alpha"]),
|
||||
max_orientation_speed_rad_s=float(params["max_orientation_speed"]),
|
||||
workspace_min=params["workspace_min"],
|
||||
workspace_max=params["workspace_max"],
|
||||
cylinder_radius_limit=params["cyl_radius_limit"],
|
||||
low_z_threshold=float(params["low_z_threshold"]),
|
||||
low_z_min_radius=float(params["low_z_min_radius"]),
|
||||
joint_max_speed_rad_s=np.full(7, np.deg2rad(joint_speed)),
|
||||
)
|
||||
return profiles
|
||||
@@ -0,0 +1,429 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from typing import Dict, Mapping, Optional
|
||||
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
|
||||
from .kinematics import RM75Kinematics, validate_se3
|
||||
from .robot_backend import RobotBackend
|
||||
from .solver import RM75IkSolver
|
||||
from .teleop_config import ArmName, ArmTeleopProfile
|
||||
from .types import IkOptions, teleop_joint_limits
|
||||
|
||||
|
||||
class SafetyState(str, Enum):
|
||||
IDLE = "idle"
|
||||
ACTIVE = "active"
|
||||
FAULT = "fault"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ControllerSample:
|
||||
hand: ArmName
|
||||
grip: bool
|
||||
trigger: float
|
||||
position_m: np.ndarray
|
||||
quaternion_xyzw: np.ndarray
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if self.hand not in ("left", "right"):
|
||||
raise ValueError("controller hand must be 'left' or 'right'")
|
||||
position = np.asarray(self.position_m, dtype=float).copy()
|
||||
quaternion = np.asarray(self.quaternion_xyzw, dtype=float).copy()
|
||||
if position.shape != (3,) or not np.all(np.isfinite(position)):
|
||||
raise ValueError("controller position must be finite with shape (3,)")
|
||||
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
|
||||
raise ValueError("controller quaternion must be finite with shape (4,)")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if norm <= 1e-9:
|
||||
raise ValueError("controller quaternion has zero norm")
|
||||
quaternion /= norm
|
||||
if not np.isfinite(self.trigger):
|
||||
raise ValueError("controller trigger must be finite")
|
||||
position.setflags(write=False)
|
||||
quaternion.setflags(write=False)
|
||||
object.__setattr__(self, "position_m", position)
|
||||
object.__setattr__(self, "quaternion_xyzw", quaternion)
|
||||
|
||||
@classmethod
|
||||
def from_message(
|
||||
cls, message, expected_arm: Optional[ArmName] = None
|
||||
) -> "ControllerSample":
|
||||
message_hand = str(getattr(message, "hand", "")).strip().lower()
|
||||
hand = expected_arm or message_hand
|
||||
if expected_arm is not None and message_hand and message_hand != expected_arm:
|
||||
raise ValueError(
|
||||
f"controller message hand {message_hand!r} does not match {expected_arm!r}"
|
||||
)
|
||||
pose = message.pose
|
||||
return cls(
|
||||
hand=hand,
|
||||
grip=bool(message.grip),
|
||||
trigger=float(message.trigger),
|
||||
position_m=np.array(
|
||||
[pose.position.x, pose.position.y, pose.position.z], dtype=float
|
||||
),
|
||||
quaternion_xyzw=np.array(
|
||||
[
|
||||
pose.orientation.x,
|
||||
pose.orientation.y,
|
||||
pose.orientation.z,
|
||||
pose.orientation.w,
|
||||
],
|
||||
dtype=float,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MappedTarget:
|
||||
target_tcp: pin.SE3
|
||||
clamped: bool
|
||||
just_engaged: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ControlCycleResult:
|
||||
state: SafetyState
|
||||
commanded_arms: tuple[ArmName, ...] = ()
|
||||
targets_tcp: Optional[Mapping[ArmName, pin.SE3]] = None
|
||||
reason: str = ""
|
||||
|
||||
|
||||
class RelativePoseMapper:
|
||||
def __init__(self, profile: ArmTeleopProfile) -> None:
|
||||
self.profile = profile
|
||||
self.active = False
|
||||
self._controller_start_position: Optional[np.ndarray] = None
|
||||
self._controller_start_rotation: Optional[np.ndarray] = None
|
||||
self._robot_start_tcp: Optional[pin.SE3] = None
|
||||
self._filtered_position: Optional[np.ndarray] = None
|
||||
self._filtered_rotation: Optional[np.ndarray] = None
|
||||
self._last_position: Optional[np.ndarray] = None
|
||||
self._last_rotation: Optional[np.ndarray] = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self.active = False
|
||||
self._controller_start_position = None
|
||||
self._controller_start_rotation = None
|
||||
self._robot_start_tcp = None
|
||||
self._filtered_position = None
|
||||
self._filtered_rotation = None
|
||||
self._last_position = None
|
||||
self._last_rotation = None
|
||||
|
||||
@staticmethod
|
||||
def _rotation(sample: ControllerSample) -> np.ndarray:
|
||||
x, y, z, w = sample.quaternion_xyzw
|
||||
return pin.Quaternion(w, x, y, z).matrix()
|
||||
|
||||
def map(
|
||||
self,
|
||||
sample: ControllerSample,
|
||||
current_tcp: pin.SE3,
|
||||
dt: float,
|
||||
) -> MappedTarget:
|
||||
if sample.hand != self.profile.arm:
|
||||
raise ValueError("controller sample was routed to the wrong arm")
|
||||
validate_se3(current_tcp, "current_tcp")
|
||||
if not np.isfinite(dt) or dt <= 0.0:
|
||||
raise ValueError("control dt must be finite and positive")
|
||||
rotation = self._rotation(sample)
|
||||
if not self.active:
|
||||
self.active = True
|
||||
self._controller_start_position = sample.position_m.copy()
|
||||
self._controller_start_rotation = rotation.copy()
|
||||
self._robot_start_tcp = current_tcp.copy()
|
||||
self._filtered_position = current_tcp.translation.copy()
|
||||
self._filtered_rotation = current_tcp.rotation.copy()
|
||||
self._last_position = current_tcp.translation.copy()
|
||||
self._last_rotation = current_tcp.rotation.copy()
|
||||
return MappedTarget(current_tcp.copy(), False, True)
|
||||
|
||||
assert self._controller_start_position is not None
|
||||
assert self._controller_start_rotation is not None
|
||||
assert self._robot_start_tcp is not None
|
||||
delta = sample.position_m - self._controller_start_position
|
||||
mapped_delta = self.profile.xr_to_robot @ delta
|
||||
raw_position = (
|
||||
self._robot_start_tcp.translation + self.profile.scale * mapped_delta
|
||||
)
|
||||
raw_position = np.where(
|
||||
np.asarray(self.profile.enable_position_axes, dtype=bool),
|
||||
raw_position,
|
||||
self._robot_start_tcp.translation,
|
||||
)
|
||||
position, clamped = self._clamp_workspace(raw_position)
|
||||
position = self._filter_position(position)
|
||||
position, limited = self._limit_position_step(position, dt)
|
||||
position, final_clamped = self._clamp_workspace(position)
|
||||
|
||||
target_rotation = self._target_rotation(rotation)
|
||||
target_rotation = self._filter_rotation(target_rotation)
|
||||
target_rotation, rotation_limited = self._limit_rotation_step(
|
||||
target_rotation, dt
|
||||
)
|
||||
self._last_position = position.copy()
|
||||
self._last_rotation = target_rotation.copy()
|
||||
return MappedTarget(
|
||||
pin.SE3(target_rotation, position),
|
||||
clamped or limited or final_clamped or rotation_limited,
|
||||
False,
|
||||
)
|
||||
|
||||
def _clamp_workspace(self, target: np.ndarray) -> tuple[np.ndarray, bool]:
|
||||
result = np.clip(
|
||||
np.asarray(target, dtype=float),
|
||||
self.profile.workspace_min,
|
||||
self.profile.workspace_max,
|
||||
)
|
||||
min_radius, max_radius = self.profile.cylinder_radius_limit
|
||||
if result[2] < self.profile.low_z_threshold:
|
||||
min_radius = max(min_radius, self.profile.low_z_min_radius)
|
||||
radius = float(np.hypot(result[0], result[1]))
|
||||
if radius > max_radius:
|
||||
result[:2] *= max_radius / radius
|
||||
elif radius < min_radius:
|
||||
if radius > 1e-9:
|
||||
result[:2] *= min_radius / radius
|
||||
else:
|
||||
result[:2] = [min_radius, 0.0]
|
||||
changed = not np.allclose(result, target, atol=1e-12, rtol=0.0)
|
||||
return result, changed
|
||||
|
||||
def _filter_position(self, target: np.ndarray) -> np.ndarray:
|
||||
assert self._filtered_position is not None
|
||||
assert self._last_position is not None
|
||||
if np.linalg.norm(target - self._last_position) < self.profile.deadband_m:
|
||||
target = self._last_position
|
||||
distance = float(np.linalg.norm(target - self._filtered_position))
|
||||
ratio = min(
|
||||
1.0, distance / self.profile.target_filter_fast_threshold_m
|
||||
)
|
||||
alpha = self.profile.target_filter_alpha + ratio * (
|
||||
self.profile.target_filter_alpha_fast
|
||||
- self.profile.target_filter_alpha
|
||||
)
|
||||
self._filtered_position = (
|
||||
alpha * target + (1.0 - alpha) * self._filtered_position
|
||||
)
|
||||
return self._filtered_position.copy()
|
||||
|
||||
def _limit_position_step(
|
||||
self, target: np.ndarray, dt: float
|
||||
) -> tuple[np.ndarray, bool]:
|
||||
assert self._last_position is not None
|
||||
delta = target - self._last_position
|
||||
distance = float(np.linalg.norm(delta))
|
||||
maximum = self.profile.max_linear_speed_m_s * dt
|
||||
if distance <= maximum or distance <= 1e-12:
|
||||
return target, False
|
||||
return self._last_position + delta * (maximum / distance), True
|
||||
|
||||
def _target_rotation(self, controller_rotation: np.ndarray) -> np.ndarray:
|
||||
assert self._controller_start_rotation is not None
|
||||
assert self._robot_start_tcp is not None
|
||||
if not self.profile.enable_orientation_control:
|
||||
return self._robot_start_tcp.rotation.copy()
|
||||
xr_delta = controller_rotation @ self._controller_start_rotation.T
|
||||
matrix = self.profile.xr_to_robot
|
||||
robot_delta = matrix @ xr_delta @ matrix.T
|
||||
target = robot_delta @ self._robot_start_tcp.rotation
|
||||
axes = np.asarray(self.profile.enable_orientation_axes, dtype=bool)
|
||||
if not np.all(axes):
|
||||
target_rpy = pin.rpy.matrixToRpy(target)
|
||||
start_rpy = pin.rpy.matrixToRpy(self._robot_start_tcp.rotation)
|
||||
target = pin.rpy.rpyToMatrix(*np.where(axes, target_rpy, start_rpy))
|
||||
return target
|
||||
|
||||
def _filter_rotation(self, target: np.ndarray) -> np.ndarray:
|
||||
assert self._filtered_rotation is not None
|
||||
assert self._last_rotation is not None
|
||||
if (
|
||||
np.linalg.norm(pin.log3(self._last_rotation.T @ target))
|
||||
< self.profile.orientation_deadband_rad
|
||||
):
|
||||
target = self._last_rotation
|
||||
delta = pin.log3(self._filtered_rotation.T @ target)
|
||||
self._filtered_rotation = self._filtered_rotation @ pin.exp3(
|
||||
self.profile.orientation_filter_alpha * delta
|
||||
)
|
||||
return self._filtered_rotation.copy()
|
||||
|
||||
def _limit_rotation_step(
|
||||
self, target: np.ndarray, dt: float
|
||||
) -> tuple[np.ndarray, bool]:
|
||||
assert self._last_rotation is not None
|
||||
delta = pin.log3(self._last_rotation.T @ target)
|
||||
angle = float(np.linalg.norm(delta))
|
||||
maximum = self.profile.max_orientation_speed_rad_s * dt
|
||||
if angle <= maximum or angle <= 1e-12:
|
||||
return target, False
|
||||
return self._last_rotation @ pin.exp3(delta * (maximum / angle)), True
|
||||
|
||||
|
||||
class DualArmQpTeleopController:
|
||||
def __init__(
|
||||
self,
|
||||
robot: RobotBackend,
|
||||
profiles: Mapping[ArmName, ArmTeleopProfile],
|
||||
control_rate_hz: float = 90.0,
|
||||
ik_options: Optional[IkOptions] = None,
|
||||
) -> None:
|
||||
if set(profiles) != {"left", "right"}:
|
||||
raise ValueError("profiles must contain left and right arms")
|
||||
if not np.isfinite(control_rate_hz) or control_rate_hz <= 0.0:
|
||||
raise ValueError("control_rate_hz must be finite and positive")
|
||||
self.robot = robot
|
||||
self.profiles = dict(profiles)
|
||||
self.dt = 1.0 / control_rate_hz
|
||||
self.ik_options = ik_options or IkOptions(
|
||||
max_iterations=120,
|
||||
time_limit_sec=0.008,
|
||||
)
|
||||
self.kinematics = {
|
||||
arm: RM75Kinematics(limits=teleop_joint_limits())
|
||||
for arm in ("left", "right")
|
||||
}
|
||||
self.solvers = {
|
||||
arm: RM75IkSolver(self.kinematics[arm]) for arm in ("left", "right")
|
||||
}
|
||||
self.mappers = {
|
||||
arm: RelativePoseMapper(self.profiles[arm])
|
||||
for arm in ("left", "right")
|
||||
}
|
||||
self._latest: Dict[ArmName, tuple[ControllerSample, float]] = {}
|
||||
self._fault_reason = ""
|
||||
self._closed = False
|
||||
self.robot.connect()
|
||||
|
||||
@property
|
||||
def safety_state(self) -> SafetyState:
|
||||
if self._fault_reason:
|
||||
return SafetyState.FAULT
|
||||
if any(mapper.active for mapper in self.mappers.values()):
|
||||
return SafetyState.ACTIVE
|
||||
return SafetyState.IDLE
|
||||
|
||||
def update_controller(
|
||||
self,
|
||||
message,
|
||||
timestamp_sec: float,
|
||||
expected_arm: Optional[ArmName] = None,
|
||||
) -> None:
|
||||
self.update_sample(
|
||||
ControllerSample.from_message(message, expected_arm), timestamp_sec
|
||||
)
|
||||
|
||||
def update_sample(self, sample: ControllerSample, timestamp_sec: float) -> None:
|
||||
if not np.isfinite(timestamp_sec):
|
||||
raise ValueError("controller timestamp must be finite")
|
||||
self._latest[sample.hand] = (sample, float(timestamp_sec))
|
||||
|
||||
def reject_input(self, reason: str) -> None:
|
||||
self._trip_fault(f"invalid controller input: {reason}")
|
||||
|
||||
def step(self, timestamp_sec: float) -> ControlCycleResult:
|
||||
if self._closed:
|
||||
raise RuntimeError("controller is closed")
|
||||
if not np.isfinite(timestamp_sec):
|
||||
return self._trip_fault("control timestamp is non-finite")
|
||||
if self._fault_reason:
|
||||
if self._can_clear_fault(timestamp_sec):
|
||||
self._fault_reason = ""
|
||||
return ControlCycleResult(SafetyState.IDLE, reason="fault cleared")
|
||||
return ControlCycleResult(SafetyState.FAULT, reason=self._fault_reason)
|
||||
|
||||
try:
|
||||
state = self.robot.read_joint_positions()
|
||||
commands: Dict[ArmName, np.ndarray] = {}
|
||||
targets: Dict[ArmName, pin.SE3] = {}
|
||||
for arm in ("left", "right"):
|
||||
latest = self._latest.get(arm)
|
||||
mapper = self.mappers[arm]
|
||||
if latest is None:
|
||||
continue
|
||||
sample, sample_time = latest
|
||||
age = timestamp_sec - sample_time
|
||||
if age < -1e-6:
|
||||
return self._trip_fault(f"{arm} controller timestamp is in the future")
|
||||
if not sample.grip:
|
||||
if mapper.active:
|
||||
mapper.reset()
|
||||
self.robot.stop((arm,))
|
||||
continue
|
||||
if age > self.profiles[arm].command_timeout_sec:
|
||||
return self._trip_fault(f"{arm} controller input timed out")
|
||||
|
||||
q_current = state.positions_rad[arm]
|
||||
current_tcp = self.kinematics[arm].forward(
|
||||
q_current, self.profiles[arm].tool_from_flange
|
||||
)
|
||||
mapped = mapper.map(sample, current_tcp, self.dt)
|
||||
flange_target = (
|
||||
mapped.target_tcp * self.profiles[arm].tool_from_flange.inverse()
|
||||
)
|
||||
result = self.solvers[arm].solve(
|
||||
flange_target, q_current, self.ik_options
|
||||
)
|
||||
if not result.success or result.q is None:
|
||||
return self._trip_fault(
|
||||
f"{arm} IK failed: {result.status.value}: {result.message}"
|
||||
)
|
||||
max_step = self.profiles[arm].joint_max_speed_rad_s * self.dt
|
||||
q_command = np.clip(result.q, q_current - max_step, q_current + max_step)
|
||||
limits = self.kinematics[arm].limits
|
||||
q_command = np.clip(q_command, limits.lower, limits.upper)
|
||||
commands[arm] = q_command
|
||||
targets[arm] = mapped.target_tcp
|
||||
|
||||
if commands:
|
||||
self.robot.command_joint_positions(commands)
|
||||
for arm, target in targets.items():
|
||||
self.robot.set_target_tcp_pose(arm, target)
|
||||
return ControlCycleResult(
|
||||
self.safety_state,
|
||||
tuple(commands),
|
||||
targets or None,
|
||||
)
|
||||
except Exception as exc:
|
||||
return self._trip_fault(f"control/backend failure: {exc}")
|
||||
|
||||
def _can_clear_fault(self, timestamp_sec: float) -> bool:
|
||||
if set(self._latest) != {"left", "right"}:
|
||||
return False
|
||||
for arm, (sample, sample_time) in self._latest.items():
|
||||
if sample.grip:
|
||||
return False
|
||||
if timestamp_sec - sample_time > self.profiles[arm].command_timeout_sec:
|
||||
return False
|
||||
for mapper in self.mappers.values():
|
||||
mapper.reset()
|
||||
return True
|
||||
|
||||
def _trip_fault(self, reason: str) -> ControlCycleResult:
|
||||
self._fault_reason = str(reason)
|
||||
for mapper in self.mappers.values():
|
||||
mapper.reset()
|
||||
try:
|
||||
self.robot.stop(("left", "right"))
|
||||
except Exception:
|
||||
pass
|
||||
return ControlCycleResult(SafetyState.FAULT, reason=self._fault_reason)
|
||||
|
||||
def stop(self) -> None:
|
||||
for mapper in self.mappers.values():
|
||||
mapper.reset()
|
||||
self.robot.stop(("left", "right"))
|
||||
|
||||
def close(self) -> None:
|
||||
if self._closed:
|
||||
return
|
||||
try:
|
||||
self.stop()
|
||||
finally:
|
||||
self.robot.close()
|
||||
self._closed = True
|
||||
@@ -52,7 +52,7 @@ def physical_joint_limits() -> JointLimits:
|
||||
|
||||
|
||||
def teleop_joint_limits() -> JointLimits:
|
||||
lower = np.deg2rad([-150.0, -30.0, -170.0, -130.0, -175.0, -125.0, -179.0])
|
||||
lower = np.deg2rad([-150.0, -110.0, -170.0, -130.0, -175.0, -125.0, -179.0])
|
||||
upper = np.deg2rad([150.0, 110.0, 170.0, 130.0, 175.0, 125.0, 179.0])
|
||||
return JointLimits("teleop", lower, upper)
|
||||
|
||||
|
||||
@@ -0,0 +1,157 @@
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from rm75_ik import DualArmAssembly, pose_errors
|
||||
from rm75_ik.mujoco_backend import CONTROLLED_HOME_Q_RAD, DualArmMuJoCo
|
||||
from rm75_ik.mujoco_model import build_normalized_dual_mjcf
|
||||
from rm75_ik.mujoco_trajectories import (
|
||||
cartesian_demo_targets,
|
||||
joint_demo_trajectory,
|
||||
se3_target_trajectory,
|
||||
solve_pose_trajectory,
|
||||
)
|
||||
from rm75_ik import RM75IkSolver, RM75Kinematics, teleop_joint_limits
|
||||
|
||||
|
||||
def test_normalized_model_has_standard_dual_arm_structure():
|
||||
xml, assets = build_normalized_dual_mjcf()
|
||||
model = mujoco.MjModel.from_xml_string(xml, assets)
|
||||
|
||||
assert model.nq == model.nv == model.njnt == 14
|
||||
assert model.nu == 0
|
||||
assert model.nmesh == 27
|
||||
assert len([key for key in assets if key.endswith(".obj")]) == 19
|
||||
assert [model.joint(index).name for index in range(14)] == [
|
||||
f"{arm}_joint_{joint}"
|
||||
for arm in ("left", "right")
|
||||
for joint in range(1, 8)
|
||||
]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("arm", ["left", "right"])
|
||||
def test_mujoco_flange_matches_stage1_dual_assembly(arm):
|
||||
scene = DualArmMuJoCo(controlled_arm=arm)
|
||||
assembly = DualArmAssembly.from_source_urdf()
|
||||
q = np.deg2rad([20, -10, 30, 40, -20, 15, 80])
|
||||
|
||||
scene.set_arm_configuration(arm, q)
|
||||
errors = pose_errors(scene.get_flange_pose(arm), assembly.forward(arm, q))
|
||||
|
||||
assert errors[0] < 1e-9
|
||||
assert errors[1] < 1e-9
|
||||
|
||||
|
||||
def test_invalid_configuration_does_not_change_state():
|
||||
scene = DualArmMuJoCo()
|
||||
before = scene.get_arm_configuration("left")
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
scene.set_arm_configuration("left", np.full(7, np.nan))
|
||||
|
||||
np.testing.assert_array_equal(scene.get_arm_configuration("left"), before)
|
||||
|
||||
|
||||
def test_playback_moves_only_controlled_arm():
|
||||
scene = DualArmMuJoCo(controlled_arm="left")
|
||||
inactive_before = scene.get_arm_configuration("right")
|
||||
trajectory = joint_demo_trajectory(CONTROLLED_HOME_Q_RAD, 20)
|
||||
|
||||
result = scene.play_trajectory(trajectory)
|
||||
|
||||
assert result.samples == 20
|
||||
np.testing.assert_array_equal(
|
||||
scene.get_arm_configuration("right"), inactive_before
|
||||
)
|
||||
np.testing.assert_allclose(
|
||||
scene.get_arm_configuration("left"), trajectory[-1], atol=0.0
|
||||
)
|
||||
|
||||
|
||||
def test_se3_target_trajectory_preserves_endpoints():
|
||||
kinematics = RM75Kinematics(limits=teleop_joint_limits())
|
||||
start = kinematics.forward(CONTROLLED_HOME_Q_RAD)
|
||||
target = start.copy()
|
||||
target.translation = target.translation + np.array([0.04, 0.01, 0.0])
|
||||
|
||||
trajectory = se3_target_trajectory(start, target, 25)
|
||||
|
||||
assert pose_errors(trajectory[0], start) == pytest.approx((0.0, 0.0), abs=1e-12)
|
||||
assert pose_errors(trajectory[-1], target) == pytest.approx((0.0, 0.0), abs=1e-12)
|
||||
|
||||
|
||||
def test_manual_drag_dynamics_moves_only_controlled_arm():
|
||||
scene = DualArmMuJoCo(controlled_arm="left")
|
||||
controlled_before = scene.get_arm_configuration("left")
|
||||
inactive_before = scene.get_arm_configuration("right")
|
||||
scene.configure_manual_drag()
|
||||
body_id = mujoco.mj_name2id(
|
||||
scene.model, mujoco.mjtObj.mjOBJ_BODY, "left_link_7"
|
||||
)
|
||||
scene.data.xfrc_applied[body_id, :3] = [0.0, 10.0, 0.0]
|
||||
|
||||
for _ in range(50):
|
||||
scene.step_manual_drag()
|
||||
|
||||
assert np.max(np.abs(scene.get_arm_configuration("left") - controlled_before)) > 1e-5
|
||||
np.testing.assert_array_equal(
|
||||
scene.get_arm_configuration("right"), inactive_before
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("kind", ["line", "arc", "orientation", "combined"])
|
||||
def test_demo_cartesian_trajectories_are_solvable(kind):
|
||||
kinematics = RM75Kinematics(limits=teleop_joint_limits())
|
||||
solver = RM75IkSolver(kinematics)
|
||||
targets = cartesian_demo_targets(
|
||||
kind, kinematics.forward(CONTROLLED_HOME_Q_RAD), 30
|
||||
)
|
||||
|
||||
solutions = solve_pose_trajectory(solver, targets, CONTROLLED_HOME_Q_RAD)
|
||||
|
||||
assert solutions.shape == (30, 7)
|
||||
assert np.all(np.isfinite(solutions))
|
||||
|
||||
|
||||
@pytest.mark.skipif(
|
||||
os.environ.get("MUJOCO_GL") != "egl",
|
||||
reason="EGL rendering test requires MUJOCO_GL=egl",
|
||||
)
|
||||
def test_offscreen_render_contains_both_arms():
|
||||
scene = DualArmMuJoCo()
|
||||
rgb = scene.render(640, 360)
|
||||
segmentation = scene.render(640, 360, segmentation=True)
|
||||
pairs = np.unique(segmentation.reshape(-1, 2), axis=0)
|
||||
names = {
|
||||
mujoco.mj_id2name(scene.model, mujoco.mjtObj.mjOBJ_GEOM, int(object_id))
|
||||
for object_id, object_type in pairs
|
||||
if object_type == mujoco.mjtObj.mjOBJ_GEOM and object_id >= 0
|
||||
}
|
||||
|
||||
assert rgb.std() > 5.0
|
||||
assert any(name.startswith("left_") for name in names if name)
|
||||
assert any(name.startswith("right_") for name in names if name)
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def realman_sdk_root():
|
||||
value = os.environ.get("REALMAN_SDK_ROOT")
|
||||
if not value:
|
||||
pytest.skip("REALMAN_SDK_ROOT is not set")
|
||||
return Path(value)
|
||||
|
||||
|
||||
def test_quick_stage2_validation_passes(realman_sdk_root):
|
||||
from rm75_ik.realman_reference import RealManFkReference
|
||||
from rm75_ik.stage2_validation import Stage2Settings, Stage2Validator
|
||||
|
||||
validator = Stage2Validator(
|
||||
RealManFkReference(realman_sdk_root), settings=Stage2Settings.quick()
|
||||
)
|
||||
summary = validator.run()
|
||||
|
||||
assert summary["passed"] is True
|
||||
assert summary["failure_count"] == 0
|
||||
@@ -0,0 +1,195 @@
|
||||
import os
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
import pytest
|
||||
|
||||
from rm75_ik import pose_errors
|
||||
from rm75_ik.mujoco_robot import MujocoRobot
|
||||
from rm75_ik.teleop_config import load_dual_arm_profiles
|
||||
from rm75_ik.teleop_control import (
|
||||
ControllerSample,
|
||||
DualArmQpTeleopController,
|
||||
RelativePoseMapper,
|
||||
SafetyState,
|
||||
)
|
||||
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[2]
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def profiles():
|
||||
return load_dual_arm_profiles(
|
||||
ROOT / "xr_rm_bringup/config/dual_arm_rm75.yaml",
|
||||
ROOT / "xr_rm_bringup/config/peripherals_rm75.yaml",
|
||||
)
|
||||
|
||||
|
||||
def sample(arm, grip, position=(0.0, 0.0, 0.0), quaternion=(0, 0, 0, 1)):
|
||||
return ControllerSample(
|
||||
hand=arm,
|
||||
grip=grip,
|
||||
trigger=0.0,
|
||||
position_m=np.asarray(position, dtype=float),
|
||||
quaternion_xyzw=np.asarray(quaternion, dtype=float),
|
||||
)
|
||||
|
||||
|
||||
def test_profiles_use_expected_tools_and_mapping(profiles):
|
||||
assert profiles["left"].active_tool_name == "minisci"
|
||||
assert profiles["right"].active_tool_name == "omnipic"
|
||||
np.testing.assert_array_equal(
|
||||
profiles["left"].xr_to_robot,
|
||||
[[0, -1, 0], [0, 0, 1], [-1, 0, 0]],
|
||||
)
|
||||
np.testing.assert_array_equal(
|
||||
profiles["right"].xr_to_robot,
|
||||
[[0, 1, 0], [0, 0, 1], [1, 0, 0]],
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("arm", "expected_delta"),
|
||||
[("left", [-0.01, 0.0, 0.0]), ("right", [0.01, 0.0, 0.0])],
|
||||
)
|
||||
def test_relative_position_mapping_matches_ros_configuration(
|
||||
profiles, arm, expected_delta
|
||||
):
|
||||
profile = replace(
|
||||
profiles[arm],
|
||||
scale=1.0,
|
||||
deadband_m=0.0,
|
||||
target_filter_alpha=1.0,
|
||||
target_filter_alpha_fast=1.0,
|
||||
max_linear_speed_m_s=10.0,
|
||||
orientation_filter_alpha=1.0,
|
||||
max_orientation_speed_rad_s=10.0,
|
||||
)
|
||||
mapper = RelativePoseMapper(profile)
|
||||
start = profile.initial_tcp
|
||||
mapper.map(sample(arm, True), start, 0.1)
|
||||
|
||||
mapped = mapper.map(sample(arm, True, [0.0, 0.01, 0.0]), start, 0.1)
|
||||
|
||||
np.testing.assert_allclose(
|
||||
mapped.target_tcp.translation - start.translation,
|
||||
expected_delta,
|
||||
atol=1e-12,
|
||||
)
|
||||
|
||||
|
||||
def test_mujoco_initializes_both_configured_tool_tcp_poses(profiles):
|
||||
robot = MujocoRobot(profiles)
|
||||
try:
|
||||
for arm in ("left", "right"):
|
||||
position_error, orientation_error = pose_errors(
|
||||
robot.get_tcp_pose(arm), profiles[arm].initial_tcp
|
||||
)
|
||||
assert position_error < 1e-3
|
||||
assert orientation_error < np.deg2rad(0.1)
|
||||
diagnostic = robot.initial_pose_diagnostics[arm]
|
||||
assert diagnostic.configured_joint_position_error_m > 0.05
|
||||
finally:
|
||||
robot.close()
|
||||
|
||||
|
||||
def test_mujoco_dual_command_is_atomic(profiles):
|
||||
robot = MujocoRobot(profiles)
|
||||
try:
|
||||
before = robot.read_joint_positions().positions_rad
|
||||
with pytest.raises(ValueError):
|
||||
robot.command_joint_positions(
|
||||
{
|
||||
"left": before["left"] + 0.01,
|
||||
"right": np.full(7, np.nan),
|
||||
}
|
||||
)
|
||||
after = robot.read_joint_positions().positions_rad
|
||||
np.testing.assert_array_equal(after["left"], before["left"])
|
||||
np.testing.assert_array_equal(after["right"], before["right"])
|
||||
finally:
|
||||
robot.close()
|
||||
|
||||
|
||||
def test_dual_controller_has_no_grip_jump_and_moves_both_arms(profiles):
|
||||
robot = MujocoRobot(profiles)
|
||||
controller = DualArmQpTeleopController(robot, profiles)
|
||||
try:
|
||||
initial = robot.read_joint_positions().positions_rad
|
||||
for arm in ("left", "right"):
|
||||
controller.update_sample(sample(arm, True), 0.0)
|
||||
engaged = controller.step(0.0)
|
||||
after_engage = robot.read_joint_positions().positions_rad
|
||||
assert engaged.state is SafetyState.ACTIVE
|
||||
for arm in ("left", "right"):
|
||||
np.testing.assert_allclose(after_engage[arm], initial[arm], atol=1e-10)
|
||||
|
||||
for arm in ("left", "right"):
|
||||
controller.update_sample(sample(arm, True, [0, 0.005, 0]), 0.01)
|
||||
moved_result = controller.step(0.01)
|
||||
moved = robot.read_joint_positions().positions_rad
|
||||
assert moved_result.state is SafetyState.ACTIVE
|
||||
for arm in ("left", "right"):
|
||||
assert np.max(np.abs(moved[arm] - initial[arm])) > 1e-5
|
||||
finally:
|
||||
controller.close()
|
||||
|
||||
|
||||
def test_active_arm_timeout_faults_both_and_requires_release(profiles):
|
||||
robot = MujocoRobot(profiles)
|
||||
controller = DualArmQpTeleopController(robot, profiles)
|
||||
try:
|
||||
for arm in ("left", "right"):
|
||||
controller.update_sample(sample(arm, True), 0.0)
|
||||
assert controller.step(0.0).state is SafetyState.ACTIVE
|
||||
|
||||
fault = controller.step(1.0)
|
||||
assert fault.state is SafetyState.FAULT
|
||||
assert "timed out" in fault.reason
|
||||
|
||||
for arm in ("left", "right"):
|
||||
controller.update_sample(sample(arm, False), 1.01)
|
||||
cleared = controller.step(1.01)
|
||||
assert cleared.state is SafetyState.IDLE
|
||||
finally:
|
||||
controller.close()
|
||||
|
||||
|
||||
def test_message_compatible_adapter_rejects_wrong_hand():
|
||||
message = SimpleNamespace(
|
||||
hand="right",
|
||||
grip=True,
|
||||
trigger=0.0,
|
||||
pose=SimpleNamespace(
|
||||
position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
|
||||
orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
|
||||
),
|
||||
)
|
||||
|
||||
with pytest.raises(ValueError, match="does not match"):
|
||||
ControllerSample.from_message(message, expected_arm="left")
|
||||
|
||||
|
||||
@pytest.mark.skipif(
|
||||
not os.environ.get("REALMAN_SDK_ROOT"),
|
||||
reason="REALMAN_SDK_ROOT is not set",
|
||||
)
|
||||
def test_quick_stage3_validation_passes():
|
||||
from rm75_ik.realman_reference import RealManFkReference
|
||||
from rm75_ik.stage3_validation import Stage3Settings, Stage3Validator
|
||||
|
||||
validator = Stage3Validator(
|
||||
RealManFkReference(Path(os.environ["REALMAN_SDK_ROOT"])),
|
||||
ROOT / "xr_rm_bringup/config/dual_arm_rm75.yaml",
|
||||
ROOT / "xr_rm_bringup/config/peripherals_rm75.yaml",
|
||||
Stage3Settings.quick(),
|
||||
)
|
||||
|
||||
summary = validator.run()
|
||||
|
||||
assert summary["passed"] is True
|
||||
assert summary["failure_count"] == 0
|
||||
Reference in New Issue
Block a user