Implement dual-arm teleoperation with RM75 QP controller and MuJoCo backend
This commit is contained in:
23
ik_qp/CMakeLists.txt
Normal file
23
ik_qp/CMakeLists.txt
Normal file
@ -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()
|
||||
144
ik_qp/README.md
144
ik_qp/README.md
@ -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()` 实现确定性的运动学播放。
|
||||
|
||||
@ -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，以及固定的工具变换或
|
||||
安装变换。它不验证动力学、自碰撞、环境碰撞、力矩限制、通信延迟或硬件安全性。
|
||||
|
||||
75
ik_qp/STAGE2_VALIDATION.md
Normal file
75
ik_qp/STAGE2_VALIDATION.md
Normal file
@ -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 不参与碰撞，因此本阶段不验证动力学、重力补偿、位置伺服、碰撞检测、通信延迟
|
||||
或真实硬件安全。
|
||||
79
ik_qp/STAGE3_VALIDATION.md
Normal file
79
ik_qp/STAGE3_VALIDATION.md
Normal file
@ -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
|
||||
|
||||
@ -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())
|
||||
|
||||
16886
ik_qp/models/dual_arm_obj/dual_arm_base_link_left_vis_1.obj
Normal file
16886
ik_qp/models/dual_arm_obj/dual_arm_base_link_left_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
16886
ik_qp/models/dual_arm_obj/dual_arm_base_link_right_vis_1.obj
Normal file
16886
ik_qp/models/dual_arm_obj/dual_arm_base_link_right_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
65916
ik_qp/models/dual_arm_obj/dual_arm_gripper1_vis_1.obj
Normal file
65916
ik_qp/models/dual_arm_obj/dual_arm_gripper1_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
65916
ik_qp/models/dual_arm_obj/dual_arm_gripper2_vis_1.obj
Normal file
65916
ik_qp/models/dual_arm_obj/dual_arm_gripper2_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
8151
ik_qp/models/dual_arm_obj/dual_arm_link10_vis_1.obj
Normal file
8151
ik_qp/models/dual_arm_obj/dual_arm_link10_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
9346
ik_qp/models/dual_arm_obj/dual_arm_link11_vis_1.obj
Normal file
9346
ik_qp/models/dual_arm_obj/dual_arm_link11_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
8648
ik_qp/models/dual_arm_obj/dual_arm_link12_vis_1.obj
Normal file
8648
ik_qp/models/dual_arm_obj/dual_arm_link12_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
10687
ik_qp/models/dual_arm_obj/dual_arm_link13_vis_1.obj
Normal file
10687
ik_qp/models/dual_arm_obj/dual_arm_link13_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
8728
ik_qp/models/dual_arm_obj/dual_arm_link14_vis_1.obj
Normal file
8728
ik_qp/models/dual_arm_obj/dual_arm_link14_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
8926
ik_qp/models/dual_arm_obj/dual_arm_link1_vis_1.obj
Normal file
8926
ik_qp/models/dual_arm_obj/dual_arm_link1_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
9943
ik_qp/models/dual_arm_obj/dual_arm_link2_vis_1.obj
Normal file
9943
ik_qp/models/dual_arm_obj/dual_arm_link2_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
8151
ik_qp/models/dual_arm_obj/dual_arm_link3_vis_1.obj
Normal file
8151
ik_qp/models/dual_arm_obj/dual_arm_link3_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
9346
ik_qp/models/dual_arm_obj/dual_arm_link4_vis_1.obj
Normal file
9346
ik_qp/models/dual_arm_obj/dual_arm_link4_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
8648
ik_qp/models/dual_arm_obj/dual_arm_link5_vis_1.obj
Normal file
8648
ik_qp/models/dual_arm_obj/dual_arm_link5_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
10687
ik_qp/models/dual_arm_obj/dual_arm_link6_vis_1.obj
Normal file
10687
ik_qp/models/dual_arm_obj/dual_arm_link6_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
8728
ik_qp/models/dual_arm_obj/dual_arm_link7_vis_1.obj
Normal file
8728
ik_qp/models/dual_arm_obj/dual_arm_link7_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
8926
ik_qp/models/dual_arm_obj/dual_arm_link8_vis_1.obj
Normal file
8926
ik_qp/models/dual_arm_obj/dual_arm_link8_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
9943
ik_qp/models/dual_arm_obj/dual_arm_link9_vis_1.obj
Normal file
9943
ik_qp/models/dual_arm_obj/dual_arm_link9_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
313574
ik_qp/models/dual_arm_obj/dual_arm_robot_base_vis_1.obj
Normal file
313574
ik_qp/models/dual_arm_obj/dual_arm_robot_base_vis_1.obj
Normal file
File diff suppressed because it is too large
Load Diff
20
ik_qp/package.xml
Normal file
20
ik_qp/package.xml
Normal file
@ -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()
|
||||
|
||||
|
||||
247
ik_qp/src/rm75_ik/mujoco_backend.py
Normal file
247
ik_qp/src/rm75_ik/mujoco_backend.py
Normal file
@ -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()
|
||||
413
ik_qp/src/rm75_ik/mujoco_model.py
Normal file
413
ik_qp/src/rm75_ik/mujoco_model.py
Normal file
@ -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
|
||||
185
ik_qp/src/rm75_ik/mujoco_robot.py
Normal file
185
ik_qp/src/rm75_ik/mujoco_robot.py
Normal file
@ -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")
|
||||
109
ik_qp/src/rm75_ik/mujoco_trajectories.py
Normal file
109
ik_qp/src/rm75_ik/mujoco_trajectories.py
Normal file
@ -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)
|
||||
43
ik_qp/src/rm75_ik/robot_backend.py
Normal file
43
ik_qp/src/rm75_ik/robot_backend.py
Normal file
@ -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: ...
|
||||
80
ik_qp/src/rm75_ik/stage2_cli.py
Normal file
80
ik_qp/src/rm75_ik/stage2_cli.py
Normal file
@ -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())
|
||||
|
||||
232
ik_qp/src/rm75_ik/stage2_demo.py
Normal file
232
ik_qp/src/rm75_ik/stage2_demo.py
Normal file
@ -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())
|
||||
595
ik_qp/src/rm75_ik/stage2_validation.py
Normal file
595
ik_qp/src/rm75_ik/stage2_validation.py
Normal file
@ -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
|
||||
|
||||
78
ik_qp/src/rm75_ik/stage3_cli.py
Normal file
78
ik_qp/src/rm75_ik/stage3_cli.py
Normal file
@ -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())
|
||||
311
ik_qp/src/rm75_ik/stage3_validation.py
Normal file
311
ik_qp/src/rm75_ik/stage3_validation.py
Normal file
@ -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
|
||||
197
ik_qp/src/rm75_ik/teleop_config.py
Normal file
197
ik_qp/src/rm75_ik/teleop_config.py
Normal file
@ -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
|
||||
429
ik_qp/src/rm75_ik/teleop_control.py
Normal file
429
ik_qp/src/rm75_ik/teleop_control.py
Normal file
@ -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)
|
||||
|
||||
|
||||
157
ik_qp/tests/test_mujoco_stage2.py
Normal file
157
ik_qp/tests/test_mujoco_stage2.py
Normal file
@ -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
|
||||
195
ik_qp/tests/test_stage3_control.py
Normal file
195
ik_qp/tests/test_stage3_control.py
Normal file
@ -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