Compare commits
5 Commits
96b9014b7f
...
xr_mujoco_
| Author | SHA1 | Date | |
|---|---|---|---|
| fcb0fec1ef | |||
| 0d7e30cb7b | |||
| 5678cc8e69 | |||
| fdf505eac5 | |||
| 7255daa69c |
1
.gitignore
vendored
1
.gitignore
vendored
@ -21,6 +21,7 @@ __pycache__/
|
|||||||
.pytest_cache/
|
.pytest_cache/
|
||||||
.mypy_cache/
|
.mypy_cache/
|
||||||
*.egg-info/
|
*.egg-info/
|
||||||
|
ik_qp/artifacts/
|
||||||
*.log
|
*.log
|
||||||
qtcreator-*
|
qtcreator-*
|
||||||
*.user
|
*.user
|
||||||
|
|||||||
110
AGENTS.md
Normal file
110
AGENTS.md
Normal file
@ -0,0 +1,110 @@
|
|||||||
|
# AGENTS.md
|
||||||
|
|
||||||
|
本文件为 Codex、Claude Code 及其他编码智能体提供仓库级工作指引。若与更高优先级的系统或开发者指令冲突，以后者为准。
|
||||||
|
|
||||||
|
## 项目概览
|
||||||
|
|
||||||
|
本仓库是 ROS2 Humble 工作空间的 `src/` 目录，用于通过 PICO/XR 控制器遥操作左右两台 RealMan RM75 机械臂。
|
||||||
|
|
||||||
|
控制器映射、运动控制链路和 IK 实现后续可能调整。修改前应阅读当前源码、启动文件、配置和 `README.md`，不要将现有 topic 链路、适配器或 IK 算法视为固定架构。
|
||||||
|
|
||||||
|
## 仓库结构
|
||||||
|
|
||||||
|
- `xr_rm_interfaces`：ROS 消息定义。
|
||||||
|
- `xr_rm_input`：XR 控制器数据输入与测试发送工具。
|
||||||
|
- `xr_rm_teleop`：控制器映射、机械臂控制及相关算法。
|
||||||
|
- `xr_rm_bringup`：启动文件、机械臂配置及启动工具。
|
||||||
|
- `unity/XR_RM_PICO_UDP_Sender`：Unity/PICO 数据发送项目。
|
||||||
|
|
||||||
|
工作空间根目录：`/home/robot/WS_xr`
|
||||||
|
|
||||||
|
源码根目录：`/home/robot/WS_xr/src`
|
||||||
|
|
||||||
|
除非任务明确涉及，否则将 PICO SDK、XRoboToolkit、机械臂 SDK 及其他引入的依赖视为第三方代码。
|
||||||
|
|
||||||
|
## 构建与运行
|
||||||
|
|
||||||
|
在工作空间根目录执行 ROS2 命令：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
colcon build --symlink-install
|
||||||
|
source install/setup.bash
|
||||||
|
```
|
||||||
|
|
||||||
|
Mock 模式启动：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
真实机械臂支持 `arm:=left|right|both` 和 `use_mock:=false`。只有在用户明确要求且测试环境安全时，才可执行真实机械臂测试。
|
||||||
|
|
||||||
|
## 开发流程
|
||||||
|
|
||||||
|
- 修改前阅读相关代码、配置和文档。
|
||||||
|
- 先执行 `git status --short`，保留用户未提交及与当前任务无关的修改。
|
||||||
|
- 修改应尽量小，并严格限定在当前任务范围内。
|
||||||
|
- 不得自动执行 `git add`、提交、推送、合并、强制推送或删除分支。
|
||||||
|
- 完成后说明修改的文件、已执行的验证及剩余风险。
|
||||||
|
|
||||||
|
### 文件修改确认门槛
|
||||||
|
|
||||||
|
- 任何创建、编辑、删除、重命名、格式化文件或生成会写入磁盘的产物，都属于文件修改。
|
||||||
|
- 修改前必须说明目标、涉及文件、实施方案、风险和验证方式，然后等待用户明确授权。
|
||||||
|
- 只有当前消息中包含明确执行意图，如“直接修改”“执行”“应用这些修改”“写入文件”“创建”或“删除”，才视为授权。
|
||||||
|
- “可以……吗”“能否……”“是否可以”“怎么修改”“这样行吗”等疑问或讨论，只表示咨询，不构成文件修改授权。此时只能回答问题或提出方案，不得写入文件。
|
||||||
|
- 用户此前允许过其他修改，不代表自动授权新的修改；每个新增范围都应重新确认。
|
||||||
|
- 授权仅覆盖用户明确提出的范围。发现需要修改额外文件时，应先说明原因并再次取得授权。
|
||||||
|
- 执行任何可能写入文件的工具前，应再次核对用户当前消息是否已经明确授权；无法确定时，必须先询问，不得自行推断。
|
||||||
|
|
||||||
|
## IK 与控制修改
|
||||||
|
|
||||||
|
- 将当前 IK 和控制策略视为可替换实现；先检查实际代码，不要预设使用笛卡尔流式控制、速度控制或某种特定求解器。
|
||||||
|
- 除非任务明确要求修改，否则保留现有 ROS 接口和启动参数；有意进行的兼容性变更必须写入文档。
|
||||||
|
- 明确坐标系、四元数顺序、单位、关节顺序和时间戳约定。
|
||||||
|
- 根据算法需要处理非有限数值、关节与工作空间限制、奇异点、指令频率限制和数据超时。
|
||||||
|
- 修改映射、滤波、IK、轨迹生成或适配器时，不得削弱停止机制。
|
||||||
|
- 算法修改应先通过静态检查、确定性输入测试或 Mock 测试，再进行真实机械臂测试。
|
||||||
|
|
||||||
|
## 安全规则
|
||||||
|
|
||||||
|
本项目能够控制真实机械臂，涉及硬件的修改必须保守进行。
|
||||||
|
|
||||||
|
不得随意修改机械臂 IP、端口 `8080`、工作空间或圆柱限制、坐标变换、初始位姿、速度/加速度限制及末端执行器配置。
|
||||||
|
|
||||||
|
当输入无效或被释放、数据过期、通信失败、适配器抛出异常、节点或应用退出时，机械臂必须安全停止。除非用户明确要求，否则 `move_to_initial_pose_on_connect` 默认保持为 `false`。
|
||||||
|
|
||||||
|
涉及安全的修改按以下顺序验证：
|
||||||
|
|
||||||
|
1. 静态检查和语法检查。
|
||||||
|
2. 针对性的单元测试或算法测试。
|
||||||
|
3. Mock 模式。
|
||||||
|
4. 单台真实机械臂低速测试。
|
||||||
|
5. 双臂真实测试。
|
||||||
|
|
||||||
|
除非相关能力已经实现并经过验证，否则不得声称项目具备碰撞规避、IK 安全保证或其他保护能力。
|
||||||
|
|
||||||
|
## 文档与生成文件
|
||||||
|
|
||||||
|
- `README.md` 是面向用户的主项目文档；`AGENTS.md` 用于记录智能体工作流程和安全规则。
|
||||||
|
- 项目自有的 Markdown 文档应以简体中文为主要叙述语言；新建或更新文档时，标题、正文、表格说明和验证结论默认使用中文。
|
||||||
|
- 代码、命令、路径、API、类名、函数名、变量名、配置键、topic、坐标系名称、协议字段、公式、单位、版本号、原始日志、英文论文标题及参考文献可按原文保留，避免翻译破坏可执行性、可检索性或技术含义。
|
||||||
|
- 编码智能体生成的 Markdown 报告也应遵循中文为主的要求；若固定字段需供程序解析，应保持字段兼容，并为面向读者的说明提供中文。
|
||||||
|
- 不得仅为统一语言而批量翻译第三方代码、SDK、依赖包或其随附文档。
|
||||||
|
- 与具体实现相关的协议、topic 和配置说明应放入对应 README 或专项文档，不在此处重复维护。
|
||||||
|
- 不得将尚未实现的功能描述为已完成。
|
||||||
|
- 将 Unity 的 `Library/`、`Builds/`、`Logs/`、`UserSettings/`、APK、生成日志及构建输出视为生成文件。
|
||||||
|
- 安全敏感修复期间避免大范围重构，代码应保持便于现场调试。
|
||||||
|
|
||||||
|
## 验证
|
||||||
|
|
||||||
|
根据修改内容选择合适的检查方式，至少执行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr/src
|
||||||
|
git diff --check
|
||||||
|
```
|
||||||
|
|
||||||
|
修改 ROS 代码时，优先构建目标软件包，再进行完整工作空间构建。自动化测试无法单独证明真实硬件行为安全。
|
||||||
250
CODEX.md
250
CODEX.md
@ -1,250 +0,0 @@
|
|||||||
# CODEX.md
|
|
||||||
|
|
||||||
This file provides guidance to Codex, Claude Code, and other coding agents when working with this repository. Higher-priority system or developer instructions override this file.
|
|
||||||
|
|
||||||
## Project Overview
|
|
||||||
|
|
||||||
This repository is the `src/` layer of a ROS2 Humble workspace for PICO/XR controller teleoperation of left and right RealMan RM75 arms.
|
|
||||||
|
|
||||||
The core behavior is relative Cartesian pose streaming:
|
|
||||||
|
|
||||||
```text
|
|
||||||
PICO/XR UDP JSON
|
|
||||||
-> xr_rm_input/udp_controller_receiver
|
|
||||||
-> /xr/left_controller and /xr/right_controller
|
|
||||||
-> xr_rm_teleop/single_arm_velocity_teleop
|
|
||||||
-> MockRealManAdapter or RealManAdapter
|
|
||||||
-> /xr_rm/<arm_name>/current_pose
|
|
||||||
-> /xr_rm/<arm_name>/raw_target_pose
|
|
||||||
-> /xr_rm/<arm_name>/target_pose
|
|
||||||
-> /xr_rm/<arm_name>/cmd_vel
|
|
||||||
-> /xr_rm/<arm_name>/target_clamped
|
|
||||||
```
|
|
||||||
|
|
||||||
When `grip=true`, the first valid frame locks both the XR controller origin and the robot TCP origin. Later controller translation and rotation deltas are converted into target TCP poses and sent through `rm_movep_canfd`. When `grip=false`, `pose_valid=false`, UDP data times out, an adapter exception occurs, or the node shuts down, motion must stop.
|
|
||||||
|
|
||||||
## Architecture
|
|
||||||
|
|
||||||
The project consists of:
|
|
||||||
|
|
||||||
- **Interface package** (`xr_rm_interfaces`): defines the `XrController` message.
|
|
||||||
- **Input package** (`xr_rm_input`): receives UDP controller JSON, normalizes controller payloads, publishes left/right XR controller topics, and provides `sample_udp_sender` for mock/debug input.
|
|
||||||
- **Teleop package** (`xr_rm_teleop`): maps relative XR controller motion to RM75 Cartesian target poses and dispatches commands through mock or real adapters.
|
|
||||||
- **Bringup package** (`xr_rm_bringup`): owns launch files, arm YAML configuration, and the local launcher UI.
|
|
||||||
- **Unity/PICO sender** (`unity/XR_RM_PICO_UDP_Sender`): PICO 4 Ultra Unity project that sends controller pose, validity, source, tracking status, sequence, and timestamp fields over UDP.
|
|
||||||
|
|
||||||
Key control facts:
|
|
||||||
|
|
||||||
- Main launch file: `xr_rm_bringup/launch/arm_debug.launch.py`
|
|
||||||
- Supported launch arm modes: `arm:=left|right|both`
|
|
||||||
- Supported adapter modes: `use_mock:=true|false`
|
|
||||||
- ROS2 workspace root: `/home/robot/WS_xr`
|
|
||||||
- Repository/source root: `/home/robot/WS_xr/src`
|
|
||||||
- `cmd_vel` is a debug estimate of target-pose change rate, not the real robot command topic.
|
|
||||||
|
|
||||||
## Build Commands
|
|
||||||
|
|
||||||
Run ROS2 commands from the workspace root:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd /home/robot/WS_xr
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
rosdep install --from-paths src -y --ignore-src
|
|
||||||
colcon build --symlink-install
|
|
||||||
source install/setup.bash
|
|
||||||
```
|
|
||||||
|
|
||||||
Git inspection commands:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd /home/robot/WS_xr/src
|
|
||||||
git status --short
|
|
||||||
git diff
|
|
||||||
git diff --check
|
|
||||||
```
|
|
||||||
|
|
||||||
## Run Commands
|
|
||||||
|
|
||||||
### Mock Dual-Arm Debug
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd /home/robot/WS_xr
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
source install/setup.bash
|
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
|
||||||
```
|
|
||||||
|
|
||||||
In another terminal:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd /home/robot/WS_xr
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
source install/setup.bash
|
|
||||||
ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 \
|
|
||||||
--pattern axis_sweep --seconds 60 --both-mode staggered
|
|
||||||
```
|
|
||||||
|
|
||||||
Use `--rotation-pattern rpy_steps` when checking orientation mapping.
|
|
||||||
|
|
||||||
### Real Arm Debug
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
|
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
|
|
||||||
```
|
|
||||||
|
|
||||||
Dual-arm real hardware:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
|
|
||||||
left_robot_ip:=192.168.192.18 \
|
|
||||||
right_robot_ip:=192.168.192.19
|
|
||||||
```
|
|
||||||
|
|
||||||
Launcher UI:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
python3 src/xr_rm_bringup/tools/launcher_ui.py
|
|
||||||
```
|
|
||||||
|
|
||||||
## Debug Topics
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 topic echo /xr/left_controller
|
|
||||||
ros2 topic echo /xr/right_controller
|
|
||||||
ros2 topic echo /xr_rm/left_rm75/target_pose
|
|
||||||
ros2 topic echo /xr_rm/right_rm75/target_pose
|
|
||||||
ros2 topic echo /xr_rm/left_rm75/cmd_vel
|
|
||||||
ros2 topic echo /xr_rm/right_rm75/cmd_vel
|
|
||||||
```
|
|
||||||
|
|
||||||
## UDP Protocol
|
|
||||||
|
|
||||||
Preferred Unity packets contain top-level `controllers.left` and `controllers.right` objects plus `t`, `source_time`, `seq`, and `frame_id`.
|
|
||||||
|
|
||||||
Each controller payload should include:
|
|
||||||
|
|
||||||
- `grip`
|
|
||||||
- `trigger`
|
|
||||||
- `pos[3]`
|
|
||||||
- `quat[4]`
|
|
||||||
- `pose_valid`
|
|
||||||
- `pose_source`
|
|
||||||
- `tracking_state`
|
|
||||||
- `controller_status`
|
|
||||||
- `grip_value`
|
|
||||||
- `axis[2]`
|
|
||||||
- `buttons`
|
|
||||||
|
|
||||||
The receiver also supports single-controller debug packets with `hand`, `pos`, and `quat`, plus aliases such as `position`, `p`, `pose.position`, `orientation`, and `q`. The default quaternion order is `xyzw`; use `quat_order:=wxyz` only when the sender really emits `wxyz`.
|
|
||||||
|
|
||||||
When the PICO HUD shows `invalid none`, expect ROS-side `grip` to be forced false. Debug in this order: Unity pose source, `udp_controller_receiver` warnings, then teleop timeout or clamping topics.
|
|
||||||
|
|
||||||
## Coordinate Notes
|
|
||||||
|
|
||||||
PICO/OpenXR project coordinates are:
|
|
||||||
|
|
||||||
- `+X`: right
|
|
||||||
- `+Y`: up
|
|
||||||
- `+Z`: back
|
|
||||||
|
|
||||||
Current Unity `Project (+Z back)` output must remain in that project coordinate convention. PXR `pxr_predict` native values are converted to:
|
|
||||||
|
|
||||||
```text
|
|
||||||
project.x = native.z
|
|
||||||
project.y = native.y
|
|
||||||
project.z = -native.x
|
|
||||||
```
|
|
||||||
|
|
||||||
`Source raw` is only for field comparison and diagnostics. If `/xr/*_controller.pose.position` already matches the expected PICO/OpenXR coordinates but one arm moves in the wrong robot direction, prefer changing only that arm's YAML `xr_to_robot_matrix`.
|
|
||||||
|
|
||||||
## Development Workflow
|
|
||||||
|
|
||||||
Use the repository rule file as the project-level source of truth for coding-agent behavior:
|
|
||||||
|
|
||||||
- Read relevant code and docs before editing.
|
|
||||||
- Check `git status --short` before edits.
|
|
||||||
- Treat existing uncommitted changes as user work; do not revert them unless explicitly requested.
|
|
||||||
- Unless the user explicitly asks for direct code or file changes, first explain the proposed approach and wait for the user to decide whether to execute, continue, revise, or stop.
|
|
||||||
- Before writing files, explain the goal understanding, files to touch, implementation plan, risks, and validation path, then wait for user confirmation.
|
|
||||||
- Skip the confirmation gate only when the user's current request clearly authorizes direct modification, such as "可以直接修改", "无需确认", "直接执行", or equivalent wording.
|
|
||||||
- Keep edits small and task-scoped.
|
|
||||||
- Do not automatically run `git add`, commit, push, force push, delete branches, or merge branches.
|
|
||||||
- After changes, summarize touched files, behavior changes, validation performed, and remaining risks.
|
|
||||||
|
|
||||||
## Safety Rules
|
|
||||||
|
|
||||||
This is a real robot teleoperation project. Keep all hardware-related changes conservative.
|
|
||||||
|
|
||||||
Do not casually modify:
|
|
||||||
|
|
||||||
- Left arm IP: `192.168.192.18`
|
|
||||||
- Right arm IP: `192.168.192.19`
|
|
||||||
- RM75 TCP port: `8080`
|
|
||||||
- Workspace limits
|
|
||||||
- Cylinder limits
|
|
||||||
- `xr_to_robot_matrix`
|
|
||||||
- Initial joint or TCP poses
|
|
||||||
- Speed and acceleration limits
|
|
||||||
- End-effector peripheral configuration
|
|
||||||
|
|
||||||
Preserve stop behavior for:
|
|
||||||
|
|
||||||
- `grip=false`
|
|
||||||
- `pose_valid=false`
|
|
||||||
- UDP timeout or stale controller data
|
|
||||||
- Adapter exceptions
|
|
||||||
- Node shutdown
|
|
||||||
- PICO app pause, exit, or disabled sending
|
|
||||||
|
|
||||||
Additional safety constraints:
|
|
||||||
|
|
||||||
- Keep `move_to_initial_pose_on_connect` defaulting to `false`.
|
|
||||||
- Do not bypass receiver behavior that forces `grip=false` when `pose_valid=false`.
|
|
||||||
- Validate hardware-related behavior in this order: static checks, mock mode, single real arm, dual real arms.
|
|
||||||
- Do not claim dual-arm collision detection exists unless code implements it.
|
|
||||||
|
|
||||||
## Unity / PICO Notes
|
|
||||||
|
|
||||||
- Current Unity project: `unity/XR_RM_PICO_UDP_Sender`
|
|
||||||
- Current PICO SDK: `unity/PICO-Unity-Integration-SDK-release_3.4.0`
|
|
||||||
- Treat the PICO SDK as third-party code unless the user explicitly targets it.
|
|
||||||
- Treat Unity `Library/`, `Builds/`, `Logs/`, `UserSettings/`, APKs, and generated logs as local/generated artifacts.
|
|
||||||
- The Unity package depends on TextMeshPro.
|
|
||||||
- PICO panel fonts come from pregenerated `Assets/Resources/Fonts/Roboto-Regular SDF.asset` and `Roboto-Bold SDF.asset`.
|
|
||||||
- Do not create TMP font assets dynamically at APK runtime.
|
|
||||||
- The PICO UDP target IP must be the Ubuntu ROS host IPv4 address on the same LAN.
|
|
||||||
- PICO app pause, exit, or disabled sending must send `grip=false`.
|
|
||||||
- Invalid pose data must send `pose_valid=false` and allow the ROS receiver to force stop.
|
|
||||||
|
|
||||||
## Documentation Rules
|
|
||||||
|
|
||||||
- `README.md` is the main project document for humans.
|
|
||||||
- `CODEX.md` is the coding-agent workflow and safety document.
|
|
||||||
- Keep `docs/` for focused setup guides and report materials.
|
|
||||||
- Do not add scattered Markdown files unless the user asks.
|
|
||||||
- Do not document unimplemented features as complete.
|
|
||||||
- Keep mock, real-arm, and Unity/PICO paths explicit.
|
|
||||||
|
|
||||||
## Testing and Validation
|
|
||||||
|
|
||||||
There is no single full-system automated test that proves real-hardware safety. Prefer layered validation:
|
|
||||||
|
|
||||||
- Static checks: syntax, imports, formatting-sensitive checks, `git diff --check`.
|
|
||||||
- ROS build: `colcon build --symlink-install`.
|
|
||||||
- Mock validation: launch `arm_debug.launch.py` with `use_mock:=true`.
|
|
||||||
- UDP validation: run `sample_udp_sender` with `axis_sweep` and, when needed, `rpy_steps`.
|
|
||||||
- Real hardware validation: single arm first, then dual arm.
|
|
||||||
|
|
||||||
## Important Notes
|
|
||||||
|
|
||||||
- Do not implement or claim the following unless the user explicitly requests and code actually supports it:
|
|
||||||
- XRoboToolkit PC-Service bridge as a required runtime path
|
|
||||||
- D405/D435 video streaming or data recording
|
|
||||||
- Dual-arm collision detection
|
|
||||||
- Autonomous picking state machine
|
|
||||||
- QP IK, dexterous-hand retargeting, or whole-body tracker support
|
|
||||||
- Full time synchronization
|
|
||||||
- Robot state feedback to PICO
|
|
||||||
- If motion direction is wrong, inspect `/xr/*_controller.pose.position` first before changing robot-side YAML.
|
|
||||||
- The project should stay understandable enough for field debugging; avoid broad refactors during safety-sensitive fixes.
|
|
||||||
14
README.md
14
README.md
@ -35,7 +35,7 @@ PICO/XR 双手柄 UDP JSON
|
|||||||
```text
|
```text
|
||||||
src/
|
src/
|
||||||
├── README.md # 项目主文档
|
├── README.md # 项目主文档
|
||||||
├── CODEX.md # Codex/Claude Code 项目工作流和安全规则
|
├── AGENTS.md # Codex/Claude Code 项目工作流和安全规则
|
||||||
├── docs/
|
├── docs/
|
||||||
│ └── pico_udp_sender_ubuntu22_setup.md # Ubuntu 22.04 下 PICO UDP Sender 配置教程
|
│ └── pico_udp_sender_ubuntu22_setup.md # Ubuntu 22.04 下 PICO UDP Sender 配置教程
|
||||||
├── unity/
|
├── unity/
|
||||||
@ -199,7 +199,17 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
|
|||||||
|
|
||||||
## Launch 入口说明
|
## Launch 入口说明
|
||||||
|
|
||||||
`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 mock、单臂真机和双臂真机按钮都调用它。
|
`arm_debug.launch.py` 是现有笛卡尔透传及真机调试主入口，`launcher_ui.py` 中的 mock、
|
||||||
|
单臂真机和双臂真机按钮都调用它。独立 QP IK 的双臂 MuJoCo 验证入口为：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
conda activate qp
|
||||||
|
ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py
|
||||||
|
```
|
||||||
|
|
||||||
|
该入口订阅同一组左右 `XrController` 话题，但仅创建 `MujocoRobot`，不会连接 RM75。
|
||||||
|
关闭 viewer 或按 `Ctrl+C` 会停止控制器并退出。无桌面环境可添加
|
||||||
|
`show_viewer:=false mujoco_gl:=egl`。
|
||||||
|
|
||||||
常用参数：
|
常用参数：
|
||||||
|
|
||||||
|
|||||||
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()
|
||||||
246
ik_qp/README.md
Normal file
246
ik_qp/README.md
Normal file
@ -0,0 +1,246 @@
|
|||||||
|
# RM75-B 运动学、QP IK 与 MuJoCo 验证
|
||||||
|
|
||||||
|
本目录是独立 Python 算法包，用于验证 RM75-B 的运动学、逆运动学和后端无关遥操作
|
||||||
|
控制。核心包不依赖 ROS 2，也不会建立真实机器人连接；第三阶段由 `xr_rm_teleop` 提供
|
||||||
|
薄 ROS 消息适配层并使用 MuJoCo 后端。
|
||||||
|
|
||||||
|
第一阶段包含：
|
||||||
|
|
||||||
|
- 由 Pinocchio 加载的标准单臂 RM75-B URDF。
|
||||||
|
- 基于 SE(3) 的正运动学、局部坐标雅可比矩阵和位姿残差。
|
||||||
|
- 支持热启动的 OSQP 微分逆运动学。
|
||||||
|
- 作为独立参考的 RealMan API2 Algo FK。
|
||||||
|
- 物理关节限位配置和项目专用的遥操作关节限位配置。
|
||||||
|
- 由两份标准单臂模型组成的双臂装配模型。
|
||||||
|
- 可生成 JSON、CSV 和 Markdown 报告的确定性验证流程。
|
||||||
|
|
||||||
|
第二阶段在完整双臂场景中使用第一阶段求解器驱动一侧机械臂，包含：
|
||||||
|
|
||||||
|
- 由标准单臂 URDF 复制两条运动链生成的规范化 14 轴 MJCF。
|
||||||
|
- 来自上传双臂模型的安装变换、平台、挂架、夹爪和 19 个 OBJ 资源。
|
||||||
|
- 默认控制左臂、固定右臂的纯运动学 MuJoCo 播放。
|
||||||
|
- headless EGL 自动验证和 GLFW 实时 viewer 演示。
|
||||||
|
- MuJoCo、Pinocchio 与 RealMan Algo FK 的三方对照。
|
||||||
|
|
||||||
|
第三阶段包含：
|
||||||
|
|
||||||
|
- 右臂 FK、IK 和连续轨迹快速验收。
|
||||||
|
- 按左右工具 TCP 求解的双臂仿真初始姿态。
|
||||||
|
- 与 `XrController.msg` 字段兼容、但不导入 ROS 的 grip 相对位姿控制核心。
|
||||||
|
- 双臂 SE(3) 目标、QP IK、关节限位、速度限制和故障联停状态机。
|
||||||
|
- 可由未来 `RealmanRobot` 实现复用的 `RobotBackend` 协议。
|
||||||
|
|
||||||
|
碰撞规避、动力学伺服和真实机器人控制仍不在当前范围内。
|
||||||
|
|
||||||
|
## 环境
|
||||||
|
|
||||||
|
经过验证的环境定义在 `environment.yml` 中：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr/src/ik_qp
|
||||||
|
conda env update -f environment.yml
|
||||||
|
conda run -n qp python -m pip install -e . --no-deps
|
||||||
|
```
|
||||||
|
|
||||||
|
环境文件会从 PyPI 安装 `Robotic_Arm 1.1.5`，通常无需额外设置。若需要强制使用
|
||||||
|
本地 API2 SDK，可为验证程序指定包含 `Robotic_Arm/` 的目录：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
export REALMAN_SDK_ROOT=/path/to/RM_API2/Python
|
||||||
|
```
|
||||||
|
|
||||||
|
## 公共 API
|
||||||
|
|
||||||
|
```python
|
||||||
|
from rm75_ik import (
|
||||||
|
DualArmAssembly,
|
||||||
|
RM75IkSolver,
|
||||||
|
RM75Kinematics,
|
||||||
|
RealManFkReference,
|
||||||
|
teleop_joint_limits,
|
||||||
|
)
|
||||||
|
|
||||||
|
kinematics = RM75Kinematics(limits=teleop_joint_limits())
|
||||||
|
solver = RM75IkSolver(kinematics)
|
||||||
|
target = kinematics.forward(target_q_rad)
|
||||||
|
result = solver.solve(target, current_q_rad)
|
||||||
|
|
||||||
|
if result.success:
|
||||||
|
solution_q_rad = result.q
|
||||||
|
```
|
||||||
|
|
||||||
|
双臂 MuJoCo 场景：
|
||||||
|
|
||||||
|
```python
|
||||||
|
from rm75_ik import DualArmMuJoCo
|
||||||
|
|
||||||
|
scene = DualArmMuJoCo(controlled_arm="left")
|
||||||
|
scene.set_arm_configuration("left", solution_q_rad)
|
||||||
|
world_flange_pose = scene.get_flange_pose("left")
|
||||||
|
image = scene.render()
|
||||||
|
```
|
||||||
|
|
||||||
|
后端无关双臂控制：
|
||||||
|
|
||||||
|
```python
|
||||||
|
from rm75_ik import DualArmQpTeleopController, load_dual_arm_profiles
|
||||||
|
from rm75_ik.mujoco_robot import MujocoRobot
|
||||||
|
|
||||||
|
profiles = load_dual_arm_profiles(teleop_yaml, peripheral_yaml)
|
||||||
|
robot = MujocoRobot(profiles)
|
||||||
|
controller = DualArmQpTeleopController(robot, profiles)
|
||||||
|
```
|
||||||
|
|
||||||
|
对于任何失败状态，`IkResult.q` 均为 `None`。不得将失败或未经验证的结果发送给
|
||||||
|
机器人。
|
||||||
|
|
||||||
|
每一对 `RM75Kinematics`/`RM75IkSolver` 都持有可变的 Pinocchio 和 OSQP 状态，因此
|
||||||
|
只能由一个控制线程使用。未来的双臂控制器应为每条机械臂分别持有一对实例。
|
||||||
|
|
||||||
|
## 验证
|
||||||
|
|
||||||
|
运行快速单元测试：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
|
||||||
|
conda run -n qp python -m pytest -q
|
||||||
|
```
|
||||||
|
|
||||||
|
运行完整、严格的第一阶段基准测试：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
|
||||||
|
conda run -n qp rm75-stage1-validate
|
||||||
|
```
|
||||||
|
|
||||||
|
如需进行小规模冒烟测试，请添加 `--quick`。报告将写入 `artifacts/stage1/`，并且
|
||||||
|
该目录有意设置为由 Git 忽略。
|
||||||
|
|
||||||
|
验收标准和最近一次完整结果请参见
|
||||||
|
[STAGE1_VALIDATION.md](STAGE1_VALIDATION.md)。
|
||||||
|
|
||||||
|
运行第二阶段完整 headless 验证：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
|
||||||
|
conda run -n qp rm75-stage2-validate --arm left
|
||||||
|
```
|
||||||
|
|
||||||
|
启动双臂实时可视化，只驱动左臂：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
conda run -n qp rm75-stage2-demo --arm left --trajectory combined
|
||||||
|
```
|
||||||
|
|
||||||
|
移动到指定目标点。目标位置位于受控机械臂基座坐标系，单位为米；未提供 RPY 时保持
|
||||||
|
起始姿态：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
conda run -n qp rm75-stage2-demo \
|
||||||
|
--arm left \
|
||||||
|
--target-position 0.45 0.0 0.3375 \
|
||||||
|
--duration 8 \
|
||||||
|
--wait-before 2 \
|
||||||
|
--hold-after 3
|
||||||
|
```
|
||||||
|
|
||||||
|
也可用弧度指定目标姿态：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
conda run -n qp rm75-stage2-demo \
|
||||||
|
--arm left \
|
||||||
|
--target-position 0.45 0.0 0.3375 \
|
||||||
|
--target-rpy -3.14159 0.52360 -3.14159 \
|
||||||
|
--duration 8
|
||||||
|
```
|
||||||
|
|
||||||
|
手动拖动受控机械臂：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
conda run -n qp rm75-stage2-demo --arm left --manual-drag
|
||||||
|
```
|
||||||
|
|
||||||
|
在 Viewer 中双击选中连杆，然后按住 `Ctrl` 并使用鼠标右键拖动。该模式使用零重力、
|
||||||
|
有阻尼的 MuJoCo 动力学，只用于检查关节活动与模型装配，不属于 IK 验收结果。
|
||||||
|
|
||||||
|
支持的演示轨迹为 `joint`、`line`、`arc`、`orientation` 和 `combined`。无桌面环境时
|
||||||
|
可添加 `--headless --output stage2_demo.png`。阶段二报告和截图写入
|
||||||
|
`artifacts/stage2/`，验收结果见 [STAGE2_VALIDATION.md](STAGE2_VALIDATION.md)。
|
||||||
|
|
||||||
|
运行第三阶段完整 headless 验证：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
conda run -n qp rm75-stage3-validate \
|
||||||
|
--sdk-root /path/to/RM_API2/Python \
|
||||||
|
--teleop-config ../xr_rm_bringup/config/dual_arm_rm75.yaml \
|
||||||
|
--peripheral-config ../xr_rm_bringup/config/peripherals_rm75.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
在 ROS 工作空间中启动 PICO/UDP、双臂 QP 和共享 MuJoCo viewer：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
conda activate qp
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py
|
||||||
|
```
|
||||||
|
|
||||||
|
该 launch 将 `robot_backend` 固定为 `mujoco`，不会连接真实机械臂。headless 运行可使用：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py \
|
||||||
|
show_viewer:=false mujoco_gl:=egl
|
||||||
|
```
|
||||||
|
|
||||||
|
常用 launch 参数：
|
||||||
|
|
||||||
|
| 参数 | 默认值 | 说明 |
|
||||||
|
|---|---:|---|
|
||||||
|
| `udp_host` | `0.0.0.0` | XR UDP 监听地址 |
|
||||||
|
| `udp_port` | `15000` | XR UDP 监听端口 |
|
||||||
|
| `udp_timer_hz` | `200.0` | UDP receiver 轮询频率 |
|
||||||
|
| `control_rate_hz` | `90.0` | 双臂 QP 控制周期频率 |
|
||||||
|
| `show_viewer` | `true` | 是否显示共享 MuJoCo viewer |
|
||||||
|
| `mujoco_gl` | `glfw` | MuJoCo 渲染后端；无桌面环境建议使用 `egl` |
|
||||||
|
|
||||||
|
启动成功后，日志应先显示 UDP receiver 的监听地址，再显示左右臂从
|
||||||
|
`initial_tcp_pose` 初始化的残差，最后出现 `dual-arm QP teleop ready`。第三阶段结果见
|
||||||
|
[STAGE3_VALIDATION.md](STAGE3_VALIDATION.md)。
|
||||||
|
|
||||||
|
PICO 在线控制使用 `src/rm75_ik/solver.py` 中的正式 QP 求解器。每个机械臂可在
|
||||||
|
`dual_arm_rm75.yaml` 中独立配置以下参数：
|
||||||
|
|
||||||
|
| 参数 | 当前值 | 作用 |
|
||||||
|
|---|---:|---|
|
||||||
|
| `qp_w_limit_mid` | `0.00002` | 按关节范围归一化，将关节从软限位附近拉向范围中心 |
|
||||||
|
| `qp_joint_motion_weights` | `[1,1,1,1,0.3,0.3,0.2]` | QP 阻尼权重；越小越积极参与冗余运动 |
|
||||||
|
| `qp_joint_step_limits_rad` | `[0.05,0.05,0.05,0.05,0.08,0.08,0.10]` | 每次 QP 内部迭代的关节步长上限，单位为 rad |
|
||||||
|
|
||||||
|
中心惩罚使用 `diag(1 / (q_upper - q_lower)^2)` 消除不同关节范围造成的量级差异，并与
|
||||||
|
原有硬关节限位同时生效。`qp_joint_step_limits_rad` 只影响 IK 内部收敛；最终发送给
|
||||||
|
MuJoCo 的每周期关节变化仍受 `joint_max_speed / control_rate_hz` 限制。上述参数不会为
|
||||||
|
初始化 IK 自动启用，避免改变启动逆解支路，也不构成碰撞规避保证。
|
||||||
|
|
||||||
|
运行过程中可在 MuJoCo viewer 中按 `R` 或 `Home`，将双臂恢复到本次启动时求得的
|
||||||
|
初始关节状态；也可调用：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 service call /xr_rm/qp/reset_to_initial std_srvs/srv/Trigger "{}"
|
||||||
|
```
|
||||||
|
|
||||||
|
Reset 会清除旧的 grip 相对位姿和控制故障。为避免旧输入在复位后立即重新驱动机械臂，
|
||||||
|
控制器必须收到左右手柄新的 `grip=false` 消息后才恢复为 `idle`。MuJoCo passive viewer
|
||||||
|
自带的 `Reload` 按钮仍为灰色；它用于重新加载模型，不用于控制本项目的运行时状态。
|
||||||
|
|
||||||
|
## 模型说明
|
||||||
|
|
||||||
|
单臂 URDF 是 RM75-B 运动链几何参数的唯一来源。导入的双臂 URDF 仅用于提供左右
|
||||||
|
安装变换；求解器不使用其中的镜像关节限位和固化的关节零位偏移。
|
||||||
|
|
||||||
|
导入的双臂 URDF 中，右侧基座的视觉原点与运动学原点相差约 1 mm。第一阶段采用
|
||||||
|
第一关节的运动学原点，并叠加文档规定的 240.5 mm 基座至第一关节偏移。
|
||||||
|
|
||||||
|
第二阶段不会把原始双臂 URDF 的镜像关节角直接交给 MuJoCo。MJCF 的左右关节均采用
|
||||||
|
标准 RealMan 弧度定义；原始双臂模型只作为安装和视觉来源。当前场景没有 actuator，
|
||||||
|
通过设置 `qpos` 并调用 `mj_forward()` 实现确定性的运动学播放。
|
||||||
58
ik_qp/STAGE1_VALIDATION.md
Normal file
58
ik_qp/STAGE1_VALIDATION.md
Normal file
@ -0,0 +1,58 @@
|
|||||||
|
# RM75-B 第一阶段验证记录
|
||||||
|
|
||||||
|
- 日期：2026-06-29
|
||||||
|
- 随机种子：`20260629`
|
||||||
|
- RealMan API2 C API 版本：`v1.1.5`
|
||||||
|
|
||||||
|
## 验收结果
|
||||||
|
|
||||||
|
完整的严格基准测试通过了全部必需检查，未记录到失败样本。
|
||||||
|
|
||||||
|
| 检查项 | 样本数 | 结果 |
|
||||||
|
|---|---:|---:|
|
||||||
|
| 物理限位 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 | 通过 |
|
||||||
|
|
||||||
|
关键测量结果：
|
||||||
|
|
||||||
|
- 物理限位 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`。
|
||||||
|
|
||||||
|
## 误差定义
|
||||||
|
|
||||||
|
位置误差：
|
||||||
|
|
||||||
|
```text
|
||||||
|
||p_result - p_target||
|
||||||
|
```
|
||||||
|
|
||||||
|
姿态误差：
|
||||||
|
|
||||||
|
```text
|
||||||
|
||log(R_result^T R_target)||
|
||||||
|
```
|
||||||
|
|
||||||
|
仅当对返回的关节配置应用 RealMan Algo FK 并通过检查后，才接受该 IK 结果为成功。
|
||||||
|
Pinocchio 不用于验证其自身产生的 IK 结果。
|
||||||
|
|
||||||
|
验证程序要求数值求解器收敛至 `0.9 mm / 0.09 deg`，随后应用独立的验收限值
|
||||||
|
`1 mm / 0.1 deg`。该保护带用于避免测得的微小模型差异在边界处造成假阳性。
|
||||||
|
|
||||||
|
## 验证边界
|
||||||
|
|
||||||
|
本结果验证了几何模型、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` 协议约束，尚未实现，
|
||||||
|
也未进行真实机械臂测试。本阶段不验证碰撞规避、动力学跟踪或硬件安全。
|
||||||
18
ik_qp/environment.yml
Normal file
18
ik_qp/environment.yml
Normal file
@ -0,0 +1,18 @@
|
|||||||
|
name: qp
|
||||||
|
channels:
|
||||||
|
- conda-forge
|
||||||
|
dependencies:
|
||||||
|
- python=3.10.20
|
||||||
|
- 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
|
||||||
6
ik_qp/kine_ctrl/fix_robotics_env.sh
Executable file
6
ik_qp/kine_ctrl/fix_robotics_env.sh
Executable file
@ -0,0 +1,6 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
echo "Fixing robotics environment..."
|
||||||
|
conda activate coppeliasim
|
||||||
|
export PYTHONPATH="/home/zl/miniforge3/envs/coppeliasim/lib/python3.10/site-packages"
|
||||||
|
pip install osqp==0.6.2.post8 --force-reinstall
|
||||||
|
python -c "import osqp; print(f'OSQP version: {osqp.__version__}')"
|
||||||
8
ik_qp/kine_ctrl/main.py
Normal file
8
ik_qp/kine_ctrl/main.py
Normal file
@ -0,0 +1,8 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Compatibility entry point for the stage-1 validation command."""
|
||||||
|
|
||||||
|
from rm75_ik.cli import main
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
824
ik_qp/kine_ctrl/rm75_kine_qp.py
Normal file
824
ik_qp/kine_ctrl/rm75_kine_qp.py
Normal file
@ -0,0 +1,824 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
import sys
|
||||||
|
import os
|
||||||
|
|
||||||
|
import pinocchio as pin
|
||||||
|
import numpy as np
|
||||||
|
import osqp
|
||||||
|
from scipy import sparse
|
||||||
|
from math import radians, degrees, pi, cos, sin
|
||||||
|
import time
|
||||||
|
import threading
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class KinematicsSolver():
|
||||||
|
def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
|
||||||
|
"""
|
||||||
|
for realman 75b
|
||||||
|
Initialize robotic arm kinematics using Pinocchio (ROS2 version).
|
||||||
|
unit: m, rad
|
||||||
|
"""
|
||||||
|
print(f' ------------ the qp based kinematic initialising -----------')
|
||||||
|
self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
self.cfg_j_limit()
|
||||||
|
|
||||||
|
q_range = (
|
||||||
|
self.model.upperPositionLimit[:7] -
|
||||||
|
self.model.lowerPositionLimit[:7]
|
||||||
|
)
|
||||||
|
|
||||||
|
self.w_q_limit = np.diag(1.0 / (q_range ** 2))
|
||||||
|
|
||||||
|
self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7])
|
||||||
|
|
||||||
|
# ---------- for reused qp_solver ------------------
|
||||||
|
self.nv = 7
|
||||||
|
|
||||||
|
# Full dense symmetric matrix structure
|
||||||
|
# P_template = np.triu(np.ones((7, 7)))
|
||||||
|
self.P_pattern = sparse.triu(np.ones((7,7))).tocsc()
|
||||||
|
|
||||||
|
P_sparse = sparse.csc_matrix(self.P_pattern)
|
||||||
|
|
||||||
|
A_sparse = sparse.eye(7, format='csc')
|
||||||
|
|
||||||
|
self.osqp_solver = osqp.OSQP()
|
||||||
|
|
||||||
|
self.osqp_solver.setup(
|
||||||
|
P=P_sparse,
|
||||||
|
q=np.zeros(7),
|
||||||
|
A=A_sparse,
|
||||||
|
l=-np.ones(7),
|
||||||
|
u=np.ones(7),
|
||||||
|
verbose=False,
|
||||||
|
warm_start=True,
|
||||||
|
polish=False
|
||||||
|
)
|
||||||
|
|
||||||
|
self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
|
||||||
|
# Smaller value => joint moves more actively
|
||||||
|
# Larger value => joint moves less / more lazy
|
||||||
|
self.joint_motion_weight = np.diag([
|
||||||
|
1.0, 1.0, 1.0, 1.0,
|
||||||
|
0.3, 0.3, 0.2
|
||||||
|
])
|
||||||
|
|
||||||
|
def add_frame(self,frame_name, position, rotationXYZ):
|
||||||
|
'''
|
||||||
|
:param frame_name: str
|
||||||
|
:param position: [x, y, z] target position (meters)
|
||||||
|
:param rotationXYZ: [x, y, z] target rotation (rad)
|
||||||
|
'''
|
||||||
|
camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0], rotationXYZ[1], rotationXYZ[2] )
|
||||||
|
camera_offset = pin.SE3(
|
||||||
|
camera_rotation,
|
||||||
|
np.array(position)
|
||||||
|
)
|
||||||
|
self.model.addFrame( pin.Frame( frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) )
|
||||||
|
|
||||||
|
def add_tool_frames(self,dict_frames):
|
||||||
|
self.tool_frames ={}
|
||||||
|
for tool_name in dict_frames:
|
||||||
|
tool_attr = dict_frames[tool_name]
|
||||||
|
position = tool_attr[0][0:3]
|
||||||
|
rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
|
||||||
|
self.add_frame(tool_name, position, rotationXYZ)
|
||||||
|
self.tool_frames.update({tool_name: self.model.getFrameId(tool_name)})
|
||||||
|
self.data = self.model.createData()
|
||||||
|
|
||||||
|
|
||||||
|
def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
|
||||||
|
if min_j is None:
|
||||||
|
min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159]
|
||||||
|
if max_j is None:
|
||||||
|
max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
|
||||||
|
if rad_flag:
|
||||||
|
for i in range(7):
|
||||||
|
self.model.lowerPositionLimit[i] = min_j[i]
|
||||||
|
self.model.upperPositionLimit[i] = max_j[i]
|
||||||
|
else:
|
||||||
|
for i in range(7):
|
||||||
|
self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi
|
||||||
|
self.model.upperPositionLimit[i] = max_j[i] / 180 * pi
|
||||||
|
|
||||||
|
def forward_kinematics(self, joint_angles, tool="omnipic"):
|
||||||
|
"""
|
||||||
|
Compute forward kinematics.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
joint_angles: List or array of 7 joint angles (radians)
|
||||||
|
tool: Name of frame to compute
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
dict: Position, rotation, rpy, quaternion
|
||||||
|
unit: position: m
|
||||||
|
rpy: rad
|
||||||
|
"""
|
||||||
|
if len(joint_angles) != 7:
|
||||||
|
raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
|
||||||
|
|
||||||
|
# Create configuration vector
|
||||||
|
q = pin.neutral(self.model)
|
||||||
|
for i, angle in enumerate(joint_angles):
|
||||||
|
q[i] = angle
|
||||||
|
|
||||||
|
# Compute forward kinematics
|
||||||
|
pin.forwardKinematics(self.model, self.data, q)
|
||||||
|
pin.updateFramePlacements(self.model, self.data)
|
||||||
|
|
||||||
|
# Get frame transform
|
||||||
|
frame_id = self.tool_frames[tool]
|
||||||
|
frame_transform = self.data.oMf[frame_id]
|
||||||
|
|
||||||
|
# Extract results
|
||||||
|
position = frame_transform.translation.copy()
|
||||||
|
rotation = frame_transform.rotation.copy()
|
||||||
|
|
||||||
|
# Compute RPY
|
||||||
|
rpy = pin.rpy.matrixToRpy(rotation)
|
||||||
|
|
||||||
|
# Compute quaternion
|
||||||
|
# quat = pin.Quaternion(rotation)
|
||||||
|
pose = np.concatenate([position, rpy], axis=0)
|
||||||
|
return pose
|
||||||
|
# return {
|
||||||
|
# 'position': position,
|
||||||
|
# # 'rotation': rotation,
|
||||||
|
# 'rpy': rpy,
|
||||||
|
# 'quaternion': [quat.x, quat.y, quat.z, quat.w],
|
||||||
|
# # 'transform': frame_transform
|
||||||
|
# }
|
||||||
|
|
||||||
|
def inverse_kinematics(self, target_position, target_rpy=None,
|
||||||
|
target_quat=None, initial_guess=None,
|
||||||
|
max_iter=500, tolerance=5e-3, debug=False, tool="ee"):
|
||||||
|
"""
|
||||||
|
Compute inverse kinematics using differential IK with multiple strategies.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
target_position: [x, y, z] target position (meters)
|
||||||
|
target_rpy: [roll, pitch, yaw] target orientation (radians)
|
||||||
|
target_quat: [x, y, z, w] target orientation as quaternion
|
||||||
|
initial_guess: Initial joint angles (radians)
|
||||||
|
max_iter: Maximum iterations
|
||||||
|
tolerance: Error tolerance
|
||||||
|
debug: Print debug information
|
||||||
|
tool: the frame name ('scissor', 'camera', 'ee')
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
tuple: (joint_angles, success, error)
|
||||||
|
"""
|
||||||
|
# Build target SE3 placement
|
||||||
|
if target_quat is not None:
|
||||||
|
quat = pin.Quaternion(target_quat[3], target_quat[0], target_quat[1], target_quat[2])
|
||||||
|
target_rotation = quat.matrix()
|
||||||
|
elif target_rpy is not None:
|
||||||
|
target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
|
||||||
|
target_rpy[1],
|
||||||
|
target_rpy[2])
|
||||||
|
else:
|
||||||
|
target_rotation = np.eye(3)
|
||||||
|
|
||||||
|
target_placement = pin.SE3(target_rotation, np.array(target_position))
|
||||||
|
|
||||||
|
# Try multiple initial guesses
|
||||||
|
initial_guesses = []
|
||||||
|
|
||||||
|
if initial_guess is not None:
|
||||||
|
initial_guesses.append(initial_guess)
|
||||||
|
else:
|
||||||
|
# Try different initial configurations
|
||||||
|
initial_guesses.append([0.1] * 7) # Zero config
|
||||||
|
|
||||||
|
|
||||||
|
best_solution = None
|
||||||
|
best_error = float('inf')
|
||||||
|
|
||||||
|
for guess_idx, guess in enumerate(initial_guesses):
|
||||||
|
q = pin.neutral(self.model)
|
||||||
|
for i, angle in enumerate(guess):
|
||||||
|
if i < len(q):
|
||||||
|
q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
|
||||||
|
self.model.upperPositionLimit[i])
|
||||||
|
q_ref = q.copy()
|
||||||
|
|
||||||
|
# Differential IK with adaptive damping
|
||||||
|
damping = 0.1
|
||||||
|
damping_reduction = 0.95
|
||||||
|
iter_count = 0
|
||||||
|
prev_error = float('inf')
|
||||||
|
|
||||||
|
ee_frame_id = self.tool_frames[tool]
|
||||||
|
|
||||||
|
J = pin.computeFrameJacobian(
|
||||||
|
self.model,
|
||||||
|
self.data,
|
||||||
|
q,
|
||||||
|
ee_frame_id,
|
||||||
|
pin.ReferenceFrame.LOCAL
|
||||||
|
)
|
||||||
|
|
||||||
|
pin.forwardKinematics(self.model, self.data, q)
|
||||||
|
pin.updateFramePlacements(self.model, self.data)
|
||||||
|
|
||||||
|
current_placement = self.data.oMf[ee_frame_id]
|
||||||
|
|
||||||
|
error_SE3 = current_placement.actInv(target_placement)
|
||||||
|
error_vec = pin.log(error_SE3).vector
|
||||||
|
|
||||||
|
# print("\n initial error =", np.linalg.norm(error_vec))
|
||||||
|
# print(error_vec)
|
||||||
|
|
||||||
|
while iter_count < max_iter:
|
||||||
|
# Compute forward kinematics
|
||||||
|
|
||||||
|
pin.computeJointJacobians(self.model, self.data, q)
|
||||||
|
pin.framesForwardKinematics(self.model, self.data, q)
|
||||||
|
|
||||||
|
# Get current end-effector placement
|
||||||
|
current_placement = self.data.oMf[ee_frame_id]
|
||||||
|
|
||||||
|
# Compute error
|
||||||
|
error_SE3 = current_placement.actInv(target_placement)
|
||||||
|
error_vec = pin.log(error_SE3).vector
|
||||||
|
error_norm = np.linalg.norm(error_vec)
|
||||||
|
|
||||||
|
if error_norm < tolerance:
|
||||||
|
if error_norm < best_error:
|
||||||
|
best_error = error_norm
|
||||||
|
best_solution = q[:7].copy()
|
||||||
|
break
|
||||||
|
|
||||||
|
# Check if error is increasing (diverging)
|
||||||
|
if error_norm > prev_error * 1.1 and iter_count > 10:
|
||||||
|
damping = min(1.0, damping * 1.5)
|
||||||
|
else:
|
||||||
|
damping = max(0.01, damping * damping_reduction)
|
||||||
|
|
||||||
|
|
||||||
|
J = pin.getFrameJacobian(
|
||||||
|
self.model,
|
||||||
|
self.data,
|
||||||
|
ee_frame_id,
|
||||||
|
pin.ReferenceFrame.LOCAL
|
||||||
|
)
|
||||||
|
|
||||||
|
# =========================
|
||||||
|
# QP-based IK
|
||||||
|
# =========================
|
||||||
|
w_ref = 0.0001
|
||||||
|
w_limit_mid = 0.00002
|
||||||
|
|
||||||
|
J_eff = pin.Jlog6(error_SE3) @ J #J #
|
||||||
|
|
||||||
|
H = J_eff.T @ self.W @ J_eff
|
||||||
|
|
||||||
|
|
||||||
|
H += damping * damping * self.joint_motion_weight
|
||||||
|
H += w_ref * np.eye(7)
|
||||||
|
H += w_limit_mid * self.w_q_limit
|
||||||
|
|
||||||
|
H_triu = sparse.triu(H).tocsc()
|
||||||
|
|
||||||
|
g = -J_eff.T @ self.W @ error_vec
|
||||||
|
g += w_ref * (q[:7] - q_ref[:7])
|
||||||
|
g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid)
|
||||||
|
|
||||||
|
# -------------------------
|
||||||
|
# Joint velocity constraints
|
||||||
|
# -------------------------
|
||||||
|
dq_limit = np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ]) # rad per iteration
|
||||||
|
|
||||||
|
lb = -dq_limit * np.ones(7)
|
||||||
|
ub = dq_limit * np.ones(7)
|
||||||
|
|
||||||
|
# -------------------------
|
||||||
|
# Joint position constraints
|
||||||
|
# -------------------------
|
||||||
|
|
||||||
|
q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
|
||||||
|
q_max_step = self.model.upperPositionLimit[:7] - q[:7]
|
||||||
|
|
||||||
|
lb = np.maximum(lb, q_min_step)
|
||||||
|
ub = np.minimum(ub, q_max_step)
|
||||||
|
|
||||||
|
# -------------------------
|
||||||
|
# Solve QP
|
||||||
|
# ------------------------
|
||||||
|
# Update solver
|
||||||
|
self.osqp_solver.update(
|
||||||
|
Px= H_triu.data, #H[np.triu_indices(7)], #
|
||||||
|
q=g,
|
||||||
|
l=lb,
|
||||||
|
u=ub
|
||||||
|
)
|
||||||
|
|
||||||
|
# Solve
|
||||||
|
result = self.osqp_solver.solve()
|
||||||
|
if result.info.status != 'solved':
|
||||||
|
break
|
||||||
|
|
||||||
|
dq = result.x
|
||||||
|
|
||||||
|
if dq is None:
|
||||||
|
break
|
||||||
|
|
||||||
|
# Apply joint limits with scaling
|
||||||
|
alpha = 1.0
|
||||||
|
q = pin.integrate(self.model, q, alpha * dq)
|
||||||
|
|
||||||
|
prev_error = error_norm
|
||||||
|
iter_count += 1
|
||||||
|
|
||||||
|
if best_solution is not None:
|
||||||
|
# return best_solution, True, best_error, iter_count
|
||||||
|
return 0, best_solution.tolist()
|
||||||
|
else:
|
||||||
|
# return q[:7].copy(), False, error_norm, iter_count
|
||||||
|
return -1, q[:7].copy().tolist()
|
||||||
|
|
||||||
|
def quaternion_to_euler(self, q):
|
||||||
|
"""
|
||||||
|
Convert quaternion to Euler angles (roll, pitch, yaw)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
qx, qy, qz, qw: quaternion components
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
tuple: (roll, pitch, yaw) in radians
|
||||||
|
"""
|
||||||
|
# Roll (x-axis rotation)
|
||||||
|
sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
|
||||||
|
cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
|
||||||
|
roll = np.arctan2(sinr_cosp, cosr_cosp)
|
||||||
|
|
||||||
|
# Pitch (y-axis rotation)
|
||||||
|
sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
|
||||||
|
if abs(sinp) >= 1:
|
||||||
|
pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
|
||||||
|
else:
|
||||||
|
pitch = np.arcsin(sinp)
|
||||||
|
|
||||||
|
# Yaw (z-axis rotation)
|
||||||
|
siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
|
||||||
|
cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
|
||||||
|
yaw = np.arctan2(siny_cosp, cosy_cosp)
|
||||||
|
|
||||||
|
return [roll, pitch, yaw]
|
||||||
|
|
||||||
|
# def invese_kinematics_velocity(self, target_position, target_rpy=None,
|
||||||
|
# target_quat=None, initial_guess=None, tool="ee"):
|
||||||
|
# """
|
||||||
|
# Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
|
||||||
|
#
|
||||||
|
# Args:
|
||||||
|
# target_position: [x, y, z] target position (meters)
|
||||||
|
# target_rpy: [roll, pitch, yaw] target orientation (radians)
|
||||||
|
# target_quat: [x, y, z, w] target orientation as quaternion
|
||||||
|
# initial_guess: Initial joint angles (radians)
|
||||||
|
# tool: the frame name ('scissor', 'camera', 'ee')
|
||||||
|
#
|
||||||
|
# Returns:
|
||||||
|
# joint_velocity: np.array()
|
||||||
|
# """
|
||||||
|
# # Build target SE3 placement
|
||||||
|
# if target_quat is not None:
|
||||||
|
# quat = pin.Quaternion(target_quat[3], target_quat[0],
|
||||||
|
# target_quat[1], target_quat[2])
|
||||||
|
# target_rotation = quat.matrix()
|
||||||
|
# elif target_rpy is not None:
|
||||||
|
# target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
|
||||||
|
# target_rpy[1],
|
||||||
|
# target_rpy[2])
|
||||||
|
# else:
|
||||||
|
# target_rotation = np.eye(3)
|
||||||
|
#
|
||||||
|
# target_placement = pin.SE3(target_rotation, np.array(target_position))
|
||||||
|
#
|
||||||
|
# # Try multiple initial guesses
|
||||||
|
# initial_guesses = []
|
||||||
|
#
|
||||||
|
# if initial_guess is not None:
|
||||||
|
# initial_guesses.append(initial_guess)
|
||||||
|
# else:
|
||||||
|
# # Try different initial configurations
|
||||||
|
# initial_guesses.append([0.1] * 7) # Zero config
|
||||||
|
# initial_guesses.append([radians(30), radians(45), radians(30),
|
||||||
|
# radians(-45), radians(30), radians(-30), 0])
|
||||||
|
# initial_guesses.append([radians(-30), radians(45), radians(-30),
|
||||||
|
# radians(45), radians(30), radians(30), 0])
|
||||||
|
#
|
||||||
|
# best_solution = None
|
||||||
|
# best_error = float('inf')
|
||||||
|
#
|
||||||
|
# for guess_idx, guess in enumerate(initial_guesses):
|
||||||
|
# q = pin.neutral(self.model)
|
||||||
|
# for i, angle in enumerate(guess):
|
||||||
|
# if i < len(q):
|
||||||
|
# q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
|
||||||
|
# self.model.upperPositionLimit[i])
|
||||||
|
#
|
||||||
|
# # Differential IK with adaptive damping
|
||||||
|
# damping = 0.01
|
||||||
|
# damping_reduction = 0.95
|
||||||
|
# iter_count = 0
|
||||||
|
# prev_error = float('inf')
|
||||||
|
#
|
||||||
|
# ee_frame_id = self.tool_frames[tool]
|
||||||
|
#
|
||||||
|
# J = pin.computeFrameJacobian(
|
||||||
|
# self.model,
|
||||||
|
# self.data,
|
||||||
|
# q,
|
||||||
|
# ee_frame_id,
|
||||||
|
# pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||||
|
# )
|
||||||
|
#
|
||||||
|
# while iter_count < max_iter:
|
||||||
|
# # Compute forward kinematics
|
||||||
|
#
|
||||||
|
# pin.computeJointJacobians(self.model, self.data, q)
|
||||||
|
# pin.framesForwardKinematics(self.model, self.data, q)
|
||||||
|
#
|
||||||
|
# # Get current end-effector placement
|
||||||
|
#
|
||||||
|
# current_placement = self.data.oMf[ee_frame_id]
|
||||||
|
#
|
||||||
|
# # Compute error
|
||||||
|
# error_SE3 = current_placement.actInv(target_placement)
|
||||||
|
# error_vec = pin.log(error_SE3).vector
|
||||||
|
# error_norm = np.linalg.norm(error_vec)
|
||||||
|
#
|
||||||
|
# if error_norm < tolerance:
|
||||||
|
# joint_angles = q[:7].copy()
|
||||||
|
# fk_result = self.forward_kinematics(joint_angles, tool=tool)
|
||||||
|
# position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
|
||||||
|
#
|
||||||
|
# if position_error < best_error:
|
||||||
|
# best_error = position_error
|
||||||
|
# best_solution = joint_angles
|
||||||
|
# break
|
||||||
|
#
|
||||||
|
# # Check if error is increasing (diverging)
|
||||||
|
# if error_norm > prev_error * 1.1 and iter_count > 10:
|
||||||
|
# damping = min(1.0, damping * 1.5)
|
||||||
|
# else:
|
||||||
|
# damping = max(0.01, damping * damping_reduction)
|
||||||
|
#
|
||||||
|
# J = pin.getFrameJacobian(
|
||||||
|
# self.model,
|
||||||
|
# self.data,
|
||||||
|
# ee_frame_id,
|
||||||
|
# pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||||
|
# )
|
||||||
|
#
|
||||||
|
# # =========================
|
||||||
|
# # QP-based IK
|
||||||
|
# # =========================
|
||||||
|
#
|
||||||
|
# H = J.T @ self.W @ J
|
||||||
|
# H += damping * damping * np.eye(7)
|
||||||
|
#
|
||||||
|
# H_triu = sparse.triu(H).tocsc()
|
||||||
|
#
|
||||||
|
# g = -J.T @ self.W @ error_vec
|
||||||
|
#
|
||||||
|
# # -------------------------
|
||||||
|
# # Joint velocity constraints
|
||||||
|
# # -------------------------
|
||||||
|
#
|
||||||
|
# dq_limit = 0.05 # rad per iteration
|
||||||
|
#
|
||||||
|
# lb = -dq_limit * np.ones(7)
|
||||||
|
# ub = dq_limit * np.ones(7)
|
||||||
|
#
|
||||||
|
# # -------------------------
|
||||||
|
# # Joint position constraints
|
||||||
|
# # -------------------------
|
||||||
|
#
|
||||||
|
# q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
|
||||||
|
# q_max_step = self.model.upperPositionLimit[:7] - q[:7]
|
||||||
|
#
|
||||||
|
# lb = np.maximum(lb, q_min_step)
|
||||||
|
# ub = np.minimum(ub, q_max_step)
|
||||||
|
#
|
||||||
|
# # -------------------------
|
||||||
|
# # Solve QP
|
||||||
|
# # ------------------------
|
||||||
|
# # Update solver
|
||||||
|
# self.osqp_solver.update(
|
||||||
|
# Px=H_triu.data,
|
||||||
|
# q=g,
|
||||||
|
# l=lb,
|
||||||
|
# u=ub
|
||||||
|
# )
|
||||||
|
#
|
||||||
|
# # Solve
|
||||||
|
# result = self.osqp_solver.solve()
|
||||||
|
#
|
||||||
|
# if result.info.status != 'solved':
|
||||||
|
# break
|
||||||
|
#
|
||||||
|
# dq = result.x
|
||||||
|
#
|
||||||
|
# if dq is None:
|
||||||
|
# break
|
||||||
|
#
|
||||||
|
# # Apply joint limits with scaling
|
||||||
|
# alpha = 0.5
|
||||||
|
# q = pin.integrate(self.model, q, alpha * dq)
|
||||||
|
#
|
||||||
|
# prev_error = error_norm
|
||||||
|
# iter_count += 1
|
||||||
|
#
|
||||||
|
# if best_solution is not None:
|
||||||
|
# return best_solution, True, best_error
|
||||||
|
# else:
|
||||||
|
# return None, False, None
|
||||||
|
|
||||||
|
def compute_jacobian(self, joint_angles, tool="ee"):
|
||||||
|
"""Compute geometric Jacobian (6x7)"""
|
||||||
|
q = pin.neutral(self.model)
|
||||||
|
for i, angle in enumerate(joint_angles):
|
||||||
|
q[i] = angle
|
||||||
|
|
||||||
|
pin.forwardKinematics(self.model, self.data, q)
|
||||||
|
pin.updateFramePlacements(self.model, self.data)
|
||||||
|
ee_frame_id = self.tool_frames[tool]
|
||||||
|
J = pin.computeFrameJacobian(self.model, self.data, q, ee_frame_id)
|
||||||
|
|
||||||
|
return J
|
||||||
|
|
||||||
|
def get_subchain_jacobian(self,
|
||||||
|
joint_angles,
|
||||||
|
frame_names
|
||||||
|
):
|
||||||
|
|
||||||
|
q = pin.neutral(self.model)
|
||||||
|
|
||||||
|
all_active_joints = self.get_active_joints_from_frame(frame_names)
|
||||||
|
|
||||||
|
for i in range(7):
|
||||||
|
q[i] = joint_angles[i]
|
||||||
|
|
||||||
|
pin.forwardKinematics(self.model, self.data, q)
|
||||||
|
pin.updateFramePlacements(self.model, self.data)
|
||||||
|
pin.computeJointJacobians(self.model, self.data, q)
|
||||||
|
|
||||||
|
Js = []
|
||||||
|
|
||||||
|
for frame_name, active_joints in zip(frame_names, all_active_joints):
|
||||||
|
frame_id = self.model.getFrameId(frame_name)
|
||||||
|
|
||||||
|
J = pin.getFrameJacobian(
|
||||||
|
self.model,
|
||||||
|
self.data,
|
||||||
|
frame_id,
|
||||||
|
pin.ReferenceFrame.LOCAL
|
||||||
|
)
|
||||||
|
Js.append(J[:, active_joints])
|
||||||
|
|
||||||
|
return Js
|
||||||
|
|
||||||
|
def get_active_joints_from_frame(self, frame_names):
|
||||||
|
"""
|
||||||
|
Return active joint indices affecting a frame.
|
||||||
|
|
||||||
|
Example:
|
||||||
|
frame_name='link_4'
|
||||||
|
-> [0,1,2,3]
|
||||||
|
"""
|
||||||
|
all_active_joint_ids = []
|
||||||
|
for frame_name in frame_names:
|
||||||
|
frame_id = self.model.getFrameId(frame_name)
|
||||||
|
|
||||||
|
# Parent joint of this frame
|
||||||
|
joint_id = self.model.frames[frame_id].parentJoint
|
||||||
|
|
||||||
|
print(f'frame_id = {frame_id}, and joint_id = {joint_id}')
|
||||||
|
|
||||||
|
active_joint_ids = []
|
||||||
|
|
||||||
|
|
||||||
|
# Traverse upward to root
|
||||||
|
while joint_id > 0:
|
||||||
|
# Pinocchio joint indexing:
|
||||||
|
# universe joint = 0
|
||||||
|
# robot joints start from 1
|
||||||
|
|
||||||
|
active_joint_ids.append(joint_id - 1)
|
||||||
|
|
||||||
|
# Move to parent joint
|
||||||
|
joint_id = self.model.parents[joint_id]
|
||||||
|
|
||||||
|
# Reverse so order becomes base -> tip
|
||||||
|
active_joint_ids.reverse()
|
||||||
|
all_active_joint_ids.append(active_joint_ids)
|
||||||
|
|
||||||
|
return all_active_joint_ids
|
||||||
|
|
||||||
|
def plan_cartesian_trajectory(self, start_pos, end_pos,
|
||||||
|
start_rpy=None, end_rpy=None,
|
||||||
|
num_steps=20, tool='ee'):
|
||||||
|
"""
|
||||||
|
Plan a Cartesian trajectory with IK for each waypoint.
|
||||||
|
"""
|
||||||
|
# Get current end-effector pose if start_rpy not provided
|
||||||
|
if start_rpy is None:
|
||||||
|
# Try to find a valid starting configuration
|
||||||
|
test_angles = [0.1] * 7
|
||||||
|
fk_test = self.forward_kinematics(test_angles,tool=tool)
|
||||||
|
start_rpy = fk_test['rpy']
|
||||||
|
|
||||||
|
if end_rpy is None:
|
||||||
|
end_rpy = start_rpy
|
||||||
|
|
||||||
|
# First, check if target is reachable
|
||||||
|
print(f"\nChecking if target is reachable...")
|
||||||
|
target_pos = end_pos
|
||||||
|
target_rpy = end_rpy
|
||||||
|
|
||||||
|
test_solution, success, error = self.inverse_kinematics(
|
||||||
|
target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool
|
||||||
|
)
|
||||||
|
|
||||||
|
if not success:
|
||||||
|
print(f"Warning: Target may be unreachable or difficult to reach")
|
||||||
|
print(f"Trying with relaxed tolerance...")
|
||||||
|
|
||||||
|
# Initial guess for IK (start with zero configuration)
|
||||||
|
current_angles = [0.1] * 7
|
||||||
|
trajectory = []
|
||||||
|
|
||||||
|
print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})")
|
||||||
|
print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})")
|
||||||
|
print("-" * 60)
|
||||||
|
|
||||||
|
for i in range(num_steps + 1):
|
||||||
|
t = i / num_steps
|
||||||
|
|
||||||
|
# Interpolate position
|
||||||
|
pos = [
|
||||||
|
start_pos[0] + t * (end_pos[0] - start_pos[0]),
|
||||||
|
start_pos[1] + t * (end_pos[1] - start_pos[1]),
|
||||||
|
start_pos[2] + t * (end_pos[2] - start_pos[2])
|
||||||
|
]
|
||||||
|
|
||||||
|
# Interpolate orientation
|
||||||
|
rpy = [
|
||||||
|
start_rpy[0] + t * (end_rpy[0] - start_rpy[0]),
|
||||||
|
start_rpy[1] + t * (end_rpy[1] - start_rpy[1]),
|
||||||
|
start_rpy[2] + t * (end_rpy[2] - start_rpy[2])
|
||||||
|
]
|
||||||
|
|
||||||
|
# Compute IK
|
||||||
|
joint_angles, success, error = self.inverse_kinematics(
|
||||||
|
pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool
|
||||||
|
)
|
||||||
|
|
||||||
|
if not success:
|
||||||
|
print(f" Waypoint {i}: IK failed!")
|
||||||
|
break
|
||||||
|
|
||||||
|
# Verify
|
||||||
|
fk_verify = self.forward_kinematics(joint_angles, tool=tool)
|
||||||
|
|
||||||
|
trajectory.append({
|
||||||
|
'step': i,
|
||||||
|
't': t,
|
||||||
|
'position': pos,
|
||||||
|
'rpy': rpy,
|
||||||
|
'joint_angles': joint_angles,
|
||||||
|
'actual_position': fk_verify['position'],
|
||||||
|
'error': error
|
||||||
|
})
|
||||||
|
|
||||||
|
# Update current angles for next iteration
|
||||||
|
current_angles = joint_angles
|
||||||
|
|
||||||
|
if i % 5 == 0 or i == num_steps:
|
||||||
|
print(f" Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), "
|
||||||
|
f"error={error:.6f}m")
|
||||||
|
|
||||||
|
return trajectory
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
"""Main test function"""
|
||||||
|
|
||||||
|
|
||||||
|
rm75 = KinematicsSolver()
|
||||||
|
|
||||||
|
# Test 1: Forward Kinematics
|
||||||
|
print("\n1. Forward Kinematics Test")
|
||||||
|
print("-" * 40)
|
||||||
|
|
||||||
|
tool_name = "scissor"
|
||||||
|
joint_angles_zero = [0.1] * 7
|
||||||
|
fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name)
|
||||||
|
|
||||||
|
print(f"Init configuration:")
|
||||||
|
print(f" Position: ({fk_result['position'][0]:.3f}, "
|
||||||
|
f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m")
|
||||||
|
|
||||||
|
# Test 2: Inverse Kinematics with more reachable target
|
||||||
|
print("\n2. Inverse Kinematics Test")
|
||||||
|
print("-" * 40)
|
||||||
|
|
||||||
|
# Try a simpler target first
|
||||||
|
target_pos = [0.3, 0.2, 0.4] # More reachable position
|
||||||
|
target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation
|
||||||
|
|
||||||
|
print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
|
||||||
|
|
||||||
|
import time
|
||||||
|
init_joints = [0.2] * 7
|
||||||
|
time0 = time.time()
|
||||||
|
for ii in range(100):
|
||||||
|
joint_solution, success, error = rm75.inverse_kinematics(
|
||||||
|
target_pos, target_rpy=target_rpy, initial_guess=init_joints,
|
||||||
|
max_iter=500, debug=False, tool=tool_name
|
||||||
|
)
|
||||||
|
time1 = time.time()
|
||||||
|
print(f"Time: {time1 - time0}")
|
||||||
|
|
||||||
|
if success:
|
||||||
|
print(f"✓ Solution found! Error: {error:.6f} m")
|
||||||
|
for i, angle in enumerate(joint_solution):
|
||||||
|
print(f" Joint {i + 1}: {degrees(angle):7.2f}°")
|
||||||
|
|
||||||
|
# Verify
|
||||||
|
fk_verify = rm75.forward_kinematics(joint_solution,tool=tool_name)
|
||||||
|
print(
|
||||||
|
f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m")
|
||||||
|
else:
|
||||||
|
print("✗ IK failed to find a solution!")
|
||||||
|
|
||||||
|
# Test 3: Jacobian
|
||||||
|
print("\n3. Jacobian Matrix")
|
||||||
|
print("-" * 40)
|
||||||
|
|
||||||
|
J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name)
|
||||||
|
print(f"Jacobian shape: {J.shape}")
|
||||||
|
for i in range(min(3, J.shape[0])):
|
||||||
|
row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)])
|
||||||
|
print(f" Row {i + 1}: {row_str}")
|
||||||
|
|
||||||
|
# Test 4: Trajectory Planning with reachable positions
|
||||||
|
print("\n4. Cartesian Trajectory Planning")
|
||||||
|
print("-" * 40)
|
||||||
|
|
||||||
|
start_pos = [0.3, 0.0, 0.4] # Start position
|
||||||
|
end_pos = [0.3, 0.0, 0.55] # End position (smaller movement)
|
||||||
|
|
||||||
|
fk0 = rm75.forward_kinematics([0.1] * 7, tool=tool_name)
|
||||||
|
|
||||||
|
trajectory = rm75.plan_cartesian_trajectory(
|
||||||
|
start_pos,
|
||||||
|
end_pos,
|
||||||
|
start_rpy=fk0['rpy'],
|
||||||
|
end_rpy=[
|
||||||
|
fk0['rpy'][0] + radians(10),
|
||||||
|
fk0['rpy'][1],
|
||||||
|
fk0['rpy'][2]
|
||||||
|
],
|
||||||
|
num_steps=10,
|
||||||
|
tool=tool_name
|
||||||
|
)
|
||||||
|
|
||||||
|
if trajectory:
|
||||||
|
print(f"\n✓ Generated {len(trajectory)} waypoints")
|
||||||
|
|
||||||
|
if success:
|
||||||
|
print("✓ Inverse kinematics working (with simplified target)")
|
||||||
|
else:
|
||||||
|
print("⚠ Inverse kinematics may need tuning - try different targets")
|
||||||
|
|
||||||
|
|
||||||
|
print("\n" + "=" * 60)
|
||||||
|
print(f'test subchain Jacobian, for future obstacle avoidance')
|
||||||
|
frame_names = [
|
||||||
|
"link_2",
|
||||||
|
"link_4",
|
||||||
|
"link_7"
|
||||||
|
]
|
||||||
|
Js_sub = rm75.get_subchain_jacobian(
|
||||||
|
joint_angles=joint_angles_zero,
|
||||||
|
frame_names=frame_names
|
||||||
|
)
|
||||||
|
print(f'Js_sub: {Js_sub}')
|
||||||
|
|
||||||
|
return rm75, trajectory
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
rm75, trajectory = main()
|
||||||
|
|
||||||
|
print("\n" + "=" * 60)
|
||||||
|
print("All tests completed!")
|
||||||
|
print("=" * 60)
|
||||||
133
ik_qp/kine_ctrl/rm75_kine_rm.py
Normal file
133
ik_qp/kine_ctrl/rm75_kine_rm.py
Normal file
@ -0,0 +1,133 @@
|
|||||||
|
|
||||||
|
from Robotic_Arm.rm_robot_interface import *
|
||||||
|
import numpy as np
|
||||||
|
import math
|
||||||
|
|
||||||
|
class rm75_kine_api():
|
||||||
|
def __init__(self):
|
||||||
|
# ---------- rm75 official algorithm -----------
|
||||||
|
print(f'------- the realman official kinematic initialising -------')
|
||||||
|
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_65 Robotic arm
|
||||||
|
force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version
|
||||||
|
# Initialize the robotic arm model and sensor type in the algorithm
|
||||||
|
self.robot_kine_rm = Algo(arm_model, force_type)
|
||||||
|
|
||||||
|
self.cfg_j_limit()
|
||||||
|
|
||||||
|
self.work_frames = {
|
||||||
|
'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
|
||||||
|
}
|
||||||
|
|
||||||
|
self.tool_name = "no_tool"
|
||||||
|
self.work_name = "work"
|
||||||
|
|
||||||
|
def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
|
||||||
|
if max_j is None:
|
||||||
|
max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
|
||||||
|
if min_j is None:
|
||||||
|
min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
|
||||||
|
|
||||||
|
max_j = np.array(max_j)
|
||||||
|
min_j = np.array(min_j)
|
||||||
|
if rad_flag:
|
||||||
|
self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
|
||||||
|
self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
|
||||||
|
else:
|
||||||
|
self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist())
|
||||||
|
self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist())
|
||||||
|
|
||||||
|
def cfg_work_frame(self , frame_name):
|
||||||
|
self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name])
|
||||||
|
|
||||||
|
def get_work_frame(self):
|
||||||
|
return self.robot_kine_rm.rm_algo_get_curr_workframe()
|
||||||
|
|
||||||
|
def cfg_tool_frame(self, frame_name ):
|
||||||
|
self.robot_kine_rm.rm_algo_set_toolframe(self.tool_frames[frame_name])
|
||||||
|
|
||||||
|
def get_tool_frame(self):
|
||||||
|
return self.robot_kine_rm.rm_algo_get_curr_toolframe()
|
||||||
|
|
||||||
|
def quaternion_to_euler(self, q):
|
||||||
|
"""
|
||||||
|
Convert quaternion to Euler angles (roll, pitch, yaw)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
qx, qy, qz, qw: quaternion components
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
tuple: (roll, pitch, yaw) in radians
|
||||||
|
"""
|
||||||
|
# Roll (x-axis rotation)
|
||||||
|
sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
|
||||||
|
cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
|
||||||
|
roll = np.arctan2(sinr_cosp, cosr_cosp)
|
||||||
|
|
||||||
|
# Pitch (y-axis rotation)
|
||||||
|
sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
|
||||||
|
if abs(sinp) >= 1:
|
||||||
|
pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
|
||||||
|
else:
|
||||||
|
pitch = np.arcsin(sinp)
|
||||||
|
|
||||||
|
# Yaw (z-axis rotation)
|
||||||
|
siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
|
||||||
|
cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
|
||||||
|
yaw = np.arctan2(siny_cosp, cosy_cosp)
|
||||||
|
|
||||||
|
return [roll, pitch, yaw]
|
||||||
|
|
||||||
|
def add_tool_frames(self, dict_frames):
|
||||||
|
self.tool_frames = {}
|
||||||
|
for tool_name in dict_frames:
|
||||||
|
tool_attr = dict_frames[tool_name]
|
||||||
|
position = tool_attr[0][0:3]
|
||||||
|
rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
|
||||||
|
f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
|
||||||
|
|
||||||
|
self.tool_frames.update({tool_name:f})
|
||||||
|
|
||||||
|
def forward_kinematics(self, joint_angles, flag = 1 , tool="omnipic", work="work"):
|
||||||
|
'''
|
||||||
|
:param joint_angles: list of joint values, in rad
|
||||||
|
:param flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
|
||||||
|
:param return: [x,y,z,rx,ry,rz], m & rad
|
||||||
|
'''
|
||||||
|
if tool != self.tool_name:
|
||||||
|
self.tool_name = tool
|
||||||
|
self.cfg_tool_frame(tool)
|
||||||
|
if work != self.work_name:
|
||||||
|
self.work_name = work
|
||||||
|
self.cfg_work_frame(work)
|
||||||
|
|
||||||
|
return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[q_s*180/math.pi for q_s in joint_angles] , flag=flag)
|
||||||
|
|
||||||
|
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work"):
|
||||||
|
'''
|
||||||
|
:param target_position: list of position values, m
|
||||||
|
:param target_rpy: list of rpy values, rad
|
||||||
|
:param initial_guess: initial guess of angles, rad
|
||||||
|
:param tool: tool name, refer to self.tool_frames
|
||||||
|
:param work: work name, refer to self.work_frames
|
||||||
|
|
||||||
|
return ret: state of ik calculation, 0:success, -2: out of workspace
|
||||||
|
[q_]: the ik calculated angles for joints, rad
|
||||||
|
'''
|
||||||
|
if tool != self.tool_name:
|
||||||
|
self.tool_name = tool
|
||||||
|
self.cfg_tool_frame(tool)
|
||||||
|
if work != self.work_name:
|
||||||
|
self.work_name = work
|
||||||
|
self.cfg_work_frame(work)
|
||||||
|
|
||||||
|
target = target_position + target_rpy
|
||||||
|
|
||||||
|
if initial_guess is not None:
|
||||||
|
q_ref = [ 180/math.pi * ig for ig in initial_guess ]
|
||||||
|
else:
|
||||||
|
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
|
||||||
|
ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
|
||||||
|
params = rm_inverse_kinematics_params_t(q_ref,
|
||||||
|
target, 1)
|
||||||
|
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
|
||||||
|
return ret, [ q/180*math.pi for q in q_out]
|
||||||
81
ik_qp/kine_ctrl/rm75_mjc.py
Normal file
81
ik_qp/kine_ctrl/rm75_mjc.py
Normal file
@ -0,0 +1,81 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Compatibility adapter for the original MuJoCo controller import path."""
|
||||||
|
|
||||||
|
from time import sleep
|
||||||
|
|
||||||
|
import mujoco.viewer
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from rm75_ik.mujoco_backend import DualArmMuJoCo
|
||||||
|
|
||||||
|
|
||||||
|
class MuJoCoPositionController:
|
||||||
|
"""Legacy facade backed by the threadless normalized dual-arm scene."""
|
||||||
|
|
||||||
|
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):
|
||||||
|
return None
|
||||||
|
|
||||||
|
def stop(self):
|
||||||
|
if self.viewer is not None:
|
||||||
|
self.viewer.close()
|
||||||
|
sleep(0.2)
|
||||||
|
self.viewer = None
|
||||||
|
|
||||||
|
def send_command(self, joint_positions):
|
||||||
|
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):
|
||||||
|
return self.backend.get_arm_configuration(self.controlled_arm)
|
||||||
|
|
||||||
|
def get_target(self):
|
||||||
|
return self.get_feedback()
|
||||||
|
|
||||||
|
def move_to_joints(self, target, duration=1.0):
|
||||||
|
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):
|
||||||
|
del tolerance, timeout
|
||||||
|
return True
|
||||||
|
|
||||||
|
def print_state(self):
|
||||||
|
print("Current joints (rad):", self.get_feedback().tolist())
|
||||||
|
|
||||||
|
|
||||||
|
def demo_position_control():
|
||||||
|
from rm75_ik.stage2_demo import main
|
||||||
|
|
||||||
|
return main([])
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(demo_position_control())
|
||||||
9
ik_qp/kine_ctrl/urdf_rm75/RM75-B.csv
Normal file
9
ik_qp/kine_ctrl/urdf_rm75/RM75-B.csv
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
|
||||||
|
base_link,0.00049987,5.2709E-05,0.060019,0,0,0,0.83887,0.0017232,-3.1058E-06,-3.7924E-05,0.0017051,1.3691E-06,0.00090158,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,,连杆1-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
|
||||||
|
link_1,1.4803E-07,-0.021108,-0.025186,0,0,0,0.59354,0.0012661,6.0354E-09,-6.3788E-09,0.0011817,-0.00021121,0.00056132,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,,连杆2-1,link_1,joint_1,joint_1,revolute,0,0,0.2405,0,0,0,base_link,0,0,1,60,3.14,-3.106,3.106,,,,,,,,
|
||||||
|
link_2,4.2145E-07,-0.076129,0.011078,0,0,0,0.43285,0.0012584,1.4694E-09,-5.7413E-09,0.00031747,0.000279,0.0012225,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,,连杆3-1,link_2,joint_2,joint_2,revolute,0,0,0,-1.5708,0,0,link_1,0,0,1,60,3.14,-2.2689,2.2689,,,,,,,,
|
||||||
|
link_3,-3.2093E-07,-0.023545,-0.027347,0,0,0,0.43132,0.00079433,1.02E-09,1.3908E-08,0.00073037,-0.00014262,0.00031507,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,,连杆4-1,link_3,joint_3,joint_3,revolute,0,-0.256,0,1.5708,0,0,link_2,0,0,1,30,3.14,-3.106,3.106,,,,,,,,
|
||||||
|
link_4,5.0722E-06,-0.059593,0.010569,0,0,0,0.28963,0.00063737,7.0681E-08,3.8708E-08,0.00015648,0.00014461,0.00061418,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,,连杆5-1,link_4,joint_4,joint_4,revolute,0,0,0,-1.5708,0,0,link_3,0,0,1,30,3.14,-2.356,2.356,,,,,,,,
|
||||||
|
link_5,2.7551E-07,-0.018042,-0.02154,0,0,0,0.23942,0.00028595,1.9823E-09,-1.192E-09,0.00026273,-4.424E-05,0.0001199,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,,连杆6-1,link_5,joint_5,joint_5,revolute,0,-0.21,0,1.5708,0,0,link_4,0,0,1,10,3.14,-3.106,3.106,,,,,,,,
|
||||||
|
link_6,3.4947E-06,-0.059381,0.0073681,0,0,0,0.2188,0.00035054,3.4456E-08,1.7975E-08,0.00010493,7.8243E-05,0.00033448,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,,连杆7-1,link_6,joint_6,joint_6,revolute,0,0,0,-1.5708,0,0,link_5,0,0,1,10,3.14,-2.234,2.234,,,,,,,,
|
||||||
|
link_7,0.00081557,1.3323E-05,-0.012705,0,0,0,0.065037,2.1144E-05,2.2774E-08,2.5471E-08,1.8109E-05,1.019E-08,3.19E-05,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,,末端法兰件 方案一-1,link_7,joint_7,joint_7,revolute,0,-0.144,0,1.5708,0,0,link_6,0,0,1,10,3.14,-6.28,6.28,,,,,,,,
|
||||||
|
453
ik_qp/kine_ctrl/urdf_rm75/RM75-B.urdf
Normal file
453
ik_qp/kine_ctrl/urdf_rm75/RM75-B.urdf
Normal file
@ -0,0 +1,453 @@
|
|||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||||
|
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||||
|
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||||
|
<robot
|
||||||
|
name="RM75-B">
|
||||||
|
<link
|
||||||
|
name="base_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.00049987 5.2709E-05 0.060019"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="1.862" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.0017232"
|
||||||
|
ixy="-3.1058E-06"
|
||||||
|
ixz="-3.7924E-05"
|
||||||
|
iyy="0.0017051"
|
||||||
|
iyz="1.3691E-06"
|
||||||
|
izz="0.00090158" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/base_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/base_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link
|
||||||
|
name="link_1">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.000241 -0.013273 -0.00995"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="1.574" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.002487573"
|
||||||
|
ixy="0.000009663"
|
||||||
|
ixz="-0.000007909"
|
||||||
|
iyy="0.002321038"
|
||||||
|
iyz="0.000179393"
|
||||||
|
izz="0.001450554" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_1.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_1.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_1"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0.2405"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="base_link" />
|
||||||
|
<child
|
||||||
|
link="link_1" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 1" />
|
||||||
|
<limit
|
||||||
|
lower="-3.106"
|
||||||
|
upper="3.106"
|
||||||
|
effort="60"
|
||||||
|
velocity="3.14" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_2">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-0.000357 -0.106789 0.005329"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="1.217" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.003494121"
|
||||||
|
ixy="0.000002921"
|
||||||
|
ixz="-0.000005613"
|
||||||
|
iyy="0.000892721"
|
||||||
|
iyz="-0.000583884"
|
||||||
|
izz="0.003444080" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_2.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_2.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_2"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="-1.5708 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link_1" />
|
||||||
|
<child
|
||||||
|
link="link_2" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 1" />
|
||||||
|
<limit
|
||||||
|
lower="-2.2689"
|
||||||
|
upper="2.2689"
|
||||||
|
effort="60"
|
||||||
|
velocity="3.14" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_3">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.000003 -0.01398 -0.011324"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="1.11" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.001836663"
|
||||||
|
ixy="0.000002259"
|
||||||
|
ixz="-0.000004216"
|
||||||
|
iyy="0.001498875"
|
||||||
|
iyz="0.000037167"
|
||||||
|
izz="0.001062545" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_3.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_3.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_3"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0 -0.256 0"
|
||||||
|
rpy="1.5708 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link_2" />
|
||||||
|
<child
|
||||||
|
link="link_3" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 1" />
|
||||||
|
<limit
|
||||||
|
lower="-3.106"
|
||||||
|
upper="3.106"
|
||||||
|
effort="30"
|
||||||
|
velocity="3.14" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_4">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-0.000005 -0.084658 0.004747"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.685" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.001282444"
|
||||||
|
ixy="-0.000000551"
|
||||||
|
ixz="-0.000000630"
|
||||||
|
iyy="0.000373013"
|
||||||
|
iyz="-0.000232084"
|
||||||
|
izz="0.001256177" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_4.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_4.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_4"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="-1.5708 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link_3" />
|
||||||
|
<child
|
||||||
|
link="link_4" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 1" />
|
||||||
|
<limit
|
||||||
|
lower="-2.356"
|
||||||
|
upper="2.356"
|
||||||
|
effort="30"
|
||||||
|
velocity="3.14" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_5">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.000078 -0.012937 -0.008781"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.619" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.000627336"
|
||||||
|
ixy="0.000001636"
|
||||||
|
ixz="-0.000001345"
|
||||||
|
iyy="0.000542455"
|
||||||
|
iyz="0.000034970"
|
||||||
|
izz="0.000370291" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_5.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_5.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_5"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0 -0.21 0"
|
||||||
|
rpy="1.5708 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link_4" />
|
||||||
|
<child
|
||||||
|
link="link_5" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 1" />
|
||||||
|
<limit
|
||||||
|
lower="-3.106"
|
||||||
|
upper="3.106"
|
||||||
|
effort="10"
|
||||||
|
velocity="3.14" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_6">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-0.000014 -0.078524 0.002819"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.602" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.000780774"
|
||||||
|
ixy="-0.000000121"
|
||||||
|
ixz="-0.000000469"
|
||||||
|
iyy="0.000289973"
|
||||||
|
iyz="-0.000120513"
|
||||||
|
izz="0.000763955" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_6.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_6.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_6"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="-1.5708 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link_5" />
|
||||||
|
<child
|
||||||
|
link="link_6" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 1" />
|
||||||
|
<limit
|
||||||
|
lower="-2.234"
|
||||||
|
upper="2.234"
|
||||||
|
effort="10"
|
||||||
|
velocity="3.14" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_7">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.001094 -0.000077 -0.010119"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.107" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.000044123"
|
||||||
|
ixy="-0.000000064"
|
||||||
|
ixz="0.0000003"
|
||||||
|
iyy="0.000035078"
|
||||||
|
iyz="-0.000000029"
|
||||||
|
izz="0.000065445" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_7.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/link_7.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_7"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0 -0.144 0"
|
||||||
|
rpy="1.5708 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link_6" />
|
||||||
|
<child
|
||||||
|
link="link_7" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 1" />
|
||||||
|
<limit
|
||||||
|
lower="-6.28"
|
||||||
|
upper="6.28"
|
||||||
|
effort="10"
|
||||||
|
velocity="3.14" />
|
||||||
|
</joint>
|
||||||
|
</robot>
|
||||||
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/base_link.STL
Normal file
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/base_link.STL
Normal file
Binary file not shown.
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_1.STL
Normal file
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_1.STL
Normal file
Binary file not shown.
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_2.STL
Normal file
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_2.STL
Normal file
Binary file not shown.
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_3.STL
Normal file
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_3.STL
Normal file
Binary file not shown.
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_4.STL
Normal file
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_4.STL
Normal file
Binary file not shown.
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_5.STL
Normal file
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_5.STL
Normal file
Binary file not shown.
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_6.STL
Normal file
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_6.STL
Normal file
Binary file not shown.
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_7.STL
Normal file
BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_7.STL
Normal file
Binary file not shown.
374
ik_qp/models/dual_arm_mujoco_fixed.urdf
Normal file
374
ik_qp/models/dual_arm_mujoco_fixed.urdf
Normal file
@ -0,0 +1,374 @@
|
|||||||
|
<?xml version='1.0' encoding='utf-8'?>
|
||||||
|
<robot name="frame">
|
||||||
|
<mujoco>
|
||||||
|
<compiler meshdir="dual_arm_obj" discardvisual="false" strippath="true" balanceinertia="true" boundmass="0.001" boundinertia="0.000001" />
|
||||||
|
</mujoco>
|
||||||
|
<link name="robot_base">
|
||||||
|
<visual name="frame">
|
||||||
|
<origin rpy="3.141403 3.190764 1.571314" xyz="-0.027426 0.006053 0.430676" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_robot_base_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="robot_base_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<visual name="base_link_left">
|
||||||
|
<origin rpy="3.141593 1.570796 0.000000" xyz="-0.052856 0.033288 0.701971" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_base_link_left_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="base_link_left_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<visual name="base_link_right">
|
||||||
|
<origin rpy="-3.141593 -1.570796 0.000000" xyz="-0.002856 0.033288 0.701971" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_base_link_right_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="base_link_right_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint1">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-2.9670597283906" effort="60.0" velocity="0.22689280275928" upper="0.0" />
|
||||||
|
<parent link="robot_base" />
|
||||||
|
<child link="link1" />
|
||||||
|
<origin rpy="3.141589 1.570796 0.000000" xyz="-0.293356 0.033288 0.701971" />
|
||||||
|
</joint>
|
||||||
|
<link name="link1">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link1">
|
||||||
|
<origin rpy="1.570796 3.141593 3.054326" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link1_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link1_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint2">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="0.0" effort="60.0" velocity="0.57595865315817" upper="2.0943951023933" />
|
||||||
|
<parent link="link1" />
|
||||||
|
<child link="link2" />
|
||||||
|
<origin rpy="1.570800 3.141593 3.054326" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link2">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link2">
|
||||||
|
<origin rpy="-1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link2_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link2_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint3">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-3.1" effort="30.0" velocity="0.57595865315817" upper="3.1" />
|
||||||
|
<parent link="link2" />
|
||||||
|
<child link="link3" />
|
||||||
|
<origin rpy="-1.570800 3.141593 3.141593" xyz="-0.000000 -0.256000 -0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link3">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link3">
|
||||||
|
<origin rpy="1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link3_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link3_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint4">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-2.355" effort="30.0" velocity="0.57595865315817" upper="2.355" />
|
||||||
|
<parent link="link3" />
|
||||||
|
<child link="link4" />
|
||||||
|
<origin rpy="1.570800 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link4">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link4">
|
||||||
|
<origin rpy="-1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link4_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link4_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint5">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-3.1" effort="10.0" velocity="0.57595865315817" upper="3.1" />
|
||||||
|
<parent link="link4" />
|
||||||
|
<child link="link5" />
|
||||||
|
<origin rpy="-1.570800 3.141593 3.141593" xyz="0.000000 -0.210000 -0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link5">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link5">
|
||||||
|
<origin rpy="1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link5_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link5_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint6">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-2.233" effort="10.0" velocity="0.57595865315817" upper="2.233" />
|
||||||
|
<parent link="link5" />
|
||||||
|
<child link="link6" />
|
||||||
|
<origin rpy="1.570800 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link6">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link6">
|
||||||
|
<origin rpy="-1.570796 3.141593 -3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link6_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link6_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="continuous" name="joint7">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<parent link="link6" />
|
||||||
|
<child link="link7" />
|
||||||
|
<origin rpy="-1.570796 3.141593 -3.141593" xyz="-0.000000 -0.144000 0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link7">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link7">
|
||||||
|
<origin rpy="-3.141593 3.141593 -3.141593" xyz="0.000000 0.000000 -0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link7_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link7_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<visual name="gripper1">
|
||||||
|
<origin rpy="-3.141593 3.141593 -3.141593" xyz="0.000000 -0.000000 0.092000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_gripper1_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="gripper1_material">
|
||||||
|
<color rgba="0.400000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint8">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="0.0" effort="60.0" velocity="0.22689280275928" upper="3.0543261909903" />
|
||||||
|
<parent link="robot_base" />
|
||||||
|
<child link="link8" />
|
||||||
|
<origin rpy="3.141589 -1.570796 0.000000" xyz="0.238644 0.033288 0.701971" />
|
||||||
|
</joint>
|
||||||
|
<link name="link8">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link8">
|
||||||
|
<origin rpy="1.570796 3.141593 -3.054326" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link8_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link8_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint9">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-2.0943951023933" effort="60.0" velocity="0.57595865315817" upper="0.0" />
|
||||||
|
<parent link="link8" />
|
||||||
|
<child link="link9" />
|
||||||
|
<origin rpy="1.570800 3.141593 -3.054326" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link9">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link9">
|
||||||
|
<origin rpy="-1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link9_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link9_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint10">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-3.1" effort="30.0" velocity="0.57595865315817" upper="3.1" />
|
||||||
|
<parent link="link9" />
|
||||||
|
<child link="link10" />
|
||||||
|
<origin rpy="-1.570800 3.141593 -3.141593" xyz="0.000000 -0.256000 0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link10">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link10">
|
||||||
|
<origin rpy="1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link10_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link10_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint11">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-2.355" effort="30.0" velocity="0.57595865315817" upper="2.355" />
|
||||||
|
<parent link="link10" />
|
||||||
|
<child link="link11" />
|
||||||
|
<origin rpy="1.570800 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link11">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link11">
|
||||||
|
<origin rpy="-1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link11_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link11_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint12">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-3.1" effort="10.0" velocity="0.57595865315817" upper="3.1" />
|
||||||
|
<parent link="link11" />
|
||||||
|
<child link="link12" />
|
||||||
|
<origin rpy="-1.570800 3.141593 -3.141593" xyz="0.000000 -0.210000 -0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link12">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link12">
|
||||||
|
<origin rpy="1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link12_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link12_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint13">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-2.233" effort="10.0" velocity="0.57595865315817" upper="2.233" />
|
||||||
|
<parent link="link12" />
|
||||||
|
<child link="link13" />
|
||||||
|
<origin rpy="1.570800 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link13">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link13">
|
||||||
|
<origin rpy="-1.570796 3.141593 3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link13_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link13_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint type="revolute" name="joint14">
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
<limit lower="-6.28" effort="10.0" velocity="0.57595865315817" upper="6.28" />
|
||||||
|
<parent link="link13" />
|
||||||
|
<child link="link14" />
|
||||||
|
<origin rpy="-1.570796 3.141593 3.141593" xyz="-0.000000 -0.144000 -0.000000" />
|
||||||
|
</joint>
|
||||||
|
<link name="link14">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.5" />
|
||||||
|
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||||
|
</inertial>
|
||||||
|
<visual name="link14">
|
||||||
|
<origin rpy="-3.141593 3.141593 -3.141593" xyz="0.000000 0.000000 0.000000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_link14_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="link14_material">
|
||||||
|
<color rgba="1.000000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<visual name="gripper2">
|
||||||
|
<origin rpy="-3.141593 3.141593 -3.141593" xyz="0.000000 0.000000 0.092000" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="dual_arm_gripper2_vis_1.obj" />
|
||||||
|
</geometry>
|
||||||
|
<material name="gripper2_material">
|
||||||
|
<color rgba="0.400000 1.000000 1.000000 1.0" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
</robot>
|
||||||
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>
|
||||||
40
ik_qp/pyproject.toml
Normal file
40
ik_qp/pyproject.toml
Normal file
@ -0,0 +1,40 @@
|
|||||||
|
[build-system]
|
||||||
|
requires = ["setuptools>=68", "wheel"]
|
||||||
|
build-backend = "setuptools.build_meta"
|
||||||
|
|
||||||
|
[project]
|
||||||
|
name = "rm75-ik-qp"
|
||||||
|
version = "0.3.0"
|
||||||
|
description = "Validated Pinocchio and OSQP inverse kinematics for RealMan RM75-B"
|
||||||
|
readme = "README.md"
|
||||||
|
requires-python = "==3.10.*"
|
||||||
|
dependencies = [
|
||||||
|
"numpy==1.23.5",
|
||||||
|
"scipy==1.10.1",
|
||||||
|
"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]
|
||||||
|
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"], "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"]
|
||||||
|
|
||||||
|
[tool.pytest.ini_options]
|
||||||
|
testpaths = ["tests"]
|
||||||
|
addopts = "-ra"
|
||||||
72
ik_qp/src/rm75_ik/__init__.py
Normal file
72
ik_qp/src/rm75_ik/__init__.py
Normal file
@ -0,0 +1,72 @@
|
|||||||
|
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,
|
||||||
|
IkStatus,
|
||||||
|
JointLimits,
|
||||||
|
joint_limit_profile,
|
||||||
|
physical_joint_limits,
|
||||||
|
teleop_joint_limits,
|
||||||
|
)
|
||||||
|
|
||||||
|
__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)
|
||||||
114
ik_qp/src/rm75_ik/cli.py
Normal file
114
ik_qp/src/rm75_ik/cli.py
Normal file
@ -0,0 +1,114 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Optional, Sequence
|
||||||
|
|
||||||
|
from .realman_reference import RealManFkReference
|
||||||
|
from .validation import (
|
||||||
|
Stage1Validator,
|
||||||
|
ValidationSettings,
|
||||||
|
load_project_tools,
|
||||||
|
write_validation_report,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _source_project_root() -> Optional[Path]:
|
||||||
|
candidate = Path(__file__).resolve().parents[3]
|
||||||
|
if (candidate / "xr_rm_bringup").is_dir():
|
||||||
|
return candidate
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def _default_output_dir() -> Path:
|
||||||
|
package_root = Path(__file__).resolve().parents[2]
|
||||||
|
if (package_root / "pyproject.toml").is_file():
|
||||||
|
return package_root / "artifacts" / "stage1"
|
||||||
|
return Path.cwd() / "stage1_artifacts"
|
||||||
|
|
||||||
|
|
||||||
|
def _default_tools_config() -> Optional[Path]:
|
||||||
|
root = _source_project_root()
|
||||||
|
if root is None:
|
||||||
|
return None
|
||||||
|
candidate = root / "xr_rm_bringup" / "config" / "peripherals_rm75.yaml"
|
||||||
|
return candidate if candidate.is_file() else None
|
||||||
|
|
||||||
|
|
||||||
|
def build_parser() -> argparse.ArgumentParser:
|
||||||
|
parser = argparse.ArgumentParser(
|
||||||
|
description="Offline RM75-B stage-1 kinematics and QP IK validation"
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--sdk-root",
|
||||||
|
type=Path,
|
||||||
|
help="directory containing the RealMan Robotic_Arm Python package",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--tools-config",
|
||||||
|
type=Path,
|
||||||
|
default=_default_tools_config(),
|
||||||
|
help="peripherals_rm75.yaml used for tool-frame FK checks",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--skip-tools",
|
||||||
|
action="store_true",
|
||||||
|
help="skip project tool-frame verification",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--output-dir",
|
||||||
|
type=Path,
|
||||||
|
default=_default_output_dir(),
|
||||||
|
help="directory for JSON, CSV and Markdown reports",
|
||||||
|
)
|
||||||
|
parser.add_argument("--seed", type=int, default=20260629)
|
||||||
|
parser.add_argument(
|
||||||
|
"--quick",
|
||||||
|
action="store_true",
|
||||||
|
help="run a small smoke-validation sample set",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--report-only",
|
||||||
|
action="store_true",
|
||||||
|
help="always 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)
|
||||||
|
settings = (
|
||||||
|
ValidationSettings.quick(seed=args.seed, strict=not args.report_only)
|
||||||
|
if args.quick
|
||||||
|
else ValidationSettings(seed=args.seed, strict=not args.report_only)
|
||||||
|
)
|
||||||
|
tools = {}
|
||||||
|
if not args.skip_tools:
|
||||||
|
if args.tools_config is None:
|
||||||
|
raise SystemExit(
|
||||||
|
"tool validation requested but peripherals_rm75.yaml was not found; "
|
||||||
|
"pass --tools-config or --skip-tools"
|
||||||
|
)
|
||||||
|
tools = load_project_tools(args.tools_config)
|
||||||
|
|
||||||
|
reference = RealManFkReference(args.sdk_root)
|
||||||
|
validator = Stage1Validator(reference, settings, tools)
|
||||||
|
summary = validator.run()
|
||||||
|
paths = write_validation_report(args.output_dir, summary, validator.failures)
|
||||||
|
|
||||||
|
result_text = "PASS" if summary["passed"] else "FAIL"
|
||||||
|
print(f"RM75-B stage-1 validation: {result_text}")
|
||||||
|
for name, check in summary["checks"].items():
|
||||||
|
print(f" [{'PASS' if check['passed'] else 'FAIL'}] {name}")
|
||||||
|
print("Reports:")
|
||||||
|
for path in paths:
|
||||||
|
print(f" {path}")
|
||||||
|
if args.report_only or summary["passed"]:
|
||||||
|
return 0
|
||||||
|
return 1
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
sys.exit(main())
|
||||||
|
|
||||||
138
ik_qp/src/rm75_ik/dual_arm.py
Normal file
138
ik_qp/src/rm75_ik/dual_arm.py
Normal file
@ -0,0 +1,138 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import sysconfig
|
||||||
|
import xml.etree.ElementTree as ET
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pinocchio as pin
|
||||||
|
|
||||||
|
from .kinematics import RM75Kinematics
|
||||||
|
from .types import JointLimits, physical_joint_limits
|
||||||
|
|
||||||
|
|
||||||
|
def default_dual_source_path() -> Path:
|
||||||
|
source_path = (
|
||||||
|
Path(__file__).resolve().parents[2]
|
||||||
|
/ "models"
|
||||||
|
/ "dual_arm_mujoco_fixed.urdf"
|
||||||
|
)
|
||||||
|
if source_path.is_file():
|
||||||
|
return source_path
|
||||||
|
installed_candidates = [
|
||||||
|
Path(sysconfig.get_path("data"))
|
||||||
|
/ "share"
|
||||||
|
/ "rm75_ik"
|
||||||
|
/ "models"
|
||||||
|
/ "dual_arm_mujoco_fixed.urdf"
|
||||||
|
]
|
||||||
|
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")
|
||||||
|
|
||||||
|
|
||||||
|
def _origin_to_se3(element: ET.Element) -> pin.SE3:
|
||||||
|
origin = element.find("origin")
|
||||||
|
if origin is None:
|
||||||
|
return pin.SE3.Identity()
|
||||||
|
xyz = np.fromstring(origin.get("xyz", "0 0 0"), sep=" ", dtype=float)
|
||||||
|
rpy = np.fromstring(origin.get("rpy", "0 0 0"), sep=" ", dtype=float)
|
||||||
|
if xyz.shape != (3,) or rpy.shape != (3,):
|
||||||
|
raise ValueError(f"invalid URDF origin on element {element.get('name')!r}")
|
||||||
|
return pin.SE3(pin.rpy.rpyToMatrix(*rpy), xyz)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class DualArmMounts:
|
||||||
|
left_base: pin.SE3
|
||||||
|
right_base: pin.SE3
|
||||||
|
right_visual_origin_delta_m: float
|
||||||
|
|
||||||
|
|
||||||
|
def load_dual_arm_mounts(source_urdf: Optional[Path | str] = None) -> DualArmMounts:
|
||||||
|
path = Path(source_urdf) if source_urdf is not None else default_dual_source_path()
|
||||||
|
root = ET.parse(path).getroot()
|
||||||
|
joints = {joint.get("name"): joint for joint in root.findall("joint")}
|
||||||
|
try:
|
||||||
|
world_left_joint1 = _origin_to_se3(joints["joint1"])
|
||||||
|
world_right_joint1 = _origin_to_se3(joints["joint8"])
|
||||||
|
except KeyError as exc:
|
||||||
|
raise ValueError("dual-arm source URDF must contain joint1 and joint8") from exc
|
||||||
|
|
||||||
|
base_to_joint1 = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.2405]))
|
||||||
|
left_base = world_left_joint1 * base_to_joint1.inverse()
|
||||||
|
right_base = world_right_joint1 * base_to_joint1.inverse()
|
||||||
|
|
||||||
|
right_visual = root.find(
|
||||||
|
"./link[@name='robot_base']/visual[@name='base_link_right']"
|
||||||
|
)
|
||||||
|
if right_visual is None:
|
||||||
|
visual_delta = float("nan")
|
||||||
|
else:
|
||||||
|
visual_pose = _origin_to_se3(right_visual)
|
||||||
|
visual_delta = float(
|
||||||
|
np.linalg.norm(right_base.translation - visual_pose.translation)
|
||||||
|
)
|
||||||
|
return DualArmMounts(left_base, right_base, visual_delta)
|
||||||
|
|
||||||
|
|
||||||
|
class DualArmAssembly:
|
||||||
|
"""Two independent RM75-B chains placed in a common world frame."""
|
||||||
|
|
||||||
|
dof = 14
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
mounts: DualArmMounts,
|
||||||
|
left: RM75Kinematics,
|
||||||
|
right: RM75Kinematics,
|
||||||
|
) -> None:
|
||||||
|
self.mounts = mounts
|
||||||
|
self._kinematics = {"left": left, "right": right}
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_source_urdf(
|
||||||
|
cls,
|
||||||
|
source_urdf: Optional[Path | str] = None,
|
||||||
|
limits: Optional[JointLimits] = None,
|
||||||
|
) -> "DualArmAssembly":
|
||||||
|
selected_limits = limits or physical_joint_limits()
|
||||||
|
return cls(
|
||||||
|
load_dual_arm_mounts(source_urdf),
|
||||||
|
RM75Kinematics(limits=selected_limits),
|
||||||
|
RM75Kinematics(limits=selected_limits),
|
||||||
|
)
|
||||||
|
|
||||||
|
def local_forward(
|
||||||
|
self,
|
||||||
|
arm: str,
|
||||||
|
q_rad: np.ndarray,
|
||||||
|
tool: Optional[pin.SE3] = None,
|
||||||
|
) -> pin.SE3:
|
||||||
|
try:
|
||||||
|
kinematics = self._kinematics[arm]
|
||||||
|
except KeyError as exc:
|
||||||
|
raise ValueError("arm must be 'left' or 'right'") from exc
|
||||||
|
return kinematics.forward(q_rad, tool)
|
||||||
|
|
||||||
|
def forward(
|
||||||
|
self,
|
||||||
|
arm: str,
|
||||||
|
q_rad: np.ndarray,
|
||||||
|
tool: Optional[pin.SE3] = None,
|
||||||
|
) -> pin.SE3:
|
||||||
|
local = self.local_forward(arm, q_rad, tool)
|
||||||
|
if arm == "left":
|
||||||
|
return self.mounts.left_base * local
|
||||||
|
if arm == "right":
|
||||||
|
return self.mounts.right_base * local
|
||||||
|
raise ValueError("arm must be 'left' or 'right'")
|
||||||
129
ik_qp/src/rm75_ik/kinematics.py
Normal file
129
ik_qp/src/rm75_ik/kinematics.py
Normal file
@ -0,0 +1,129 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import sysconfig
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Optional, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pinocchio as pin
|
||||||
|
|
||||||
|
from .types import JointLimits, physical_joint_limits
|
||||||
|
|
||||||
|
|
||||||
|
EXPECTED_JOINT_NAMES = tuple(f"joint_{index}" for index in range(1, 8))
|
||||||
|
FLANGE_FRAME = "link_7"
|
||||||
|
|
||||||
|
|
||||||
|
def default_urdf_path() -> Path:
|
||||||
|
source_path = (
|
||||||
|
Path(__file__).resolve().parents[2]
|
||||||
|
/ "kine_ctrl"
|
||||||
|
/ "urdf_rm75"
|
||||||
|
/ "RM75-B.urdf"
|
||||||
|
)
|
||||||
|
if source_path.is_file():
|
||||||
|
return source_path
|
||||||
|
installed_candidates = [
|
||||||
|
Path(sysconfig.get_path("data"))
|
||||||
|
/ "share"
|
||||||
|
/ "rm75_ik"
|
||||||
|
/ "models"
|
||||||
|
/ "RM75-B.urdf"
|
||||||
|
]
|
||||||
|
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")
|
||||||
|
|
||||||
|
|
||||||
|
def validate_se3(value: pin.SE3, name: str = "pose") -> None:
|
||||||
|
if not isinstance(value, pin.SE3):
|
||||||
|
raise TypeError(f"{name} must be pinocchio.SE3")
|
||||||
|
rotation = np.asarray(value.rotation)
|
||||||
|
translation = np.asarray(value.translation)
|
||||||
|
if rotation.shape != (3, 3) or translation.shape != (3,):
|
||||||
|
raise ValueError(f"{name} has invalid dimensions")
|
||||||
|
if not np.all(np.isfinite(rotation)) or not np.all(np.isfinite(translation)):
|
||||||
|
raise ValueError(f"{name} must be finite")
|
||||||
|
if not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-7):
|
||||||
|
raise ValueError(f"{name} rotation must be orthonormal")
|
||||||
|
if not np.isclose(np.linalg.det(rotation), 1.0, atol=1e-7):
|
||||||
|
raise ValueError(f"{name} rotation determinant must be +1")
|
||||||
|
|
||||||
|
|
||||||
|
def pose_errors(current: pin.SE3, target: pin.SE3) -> Tuple[float, float]:
|
||||||
|
validate_se3(current, "current")
|
||||||
|
validate_se3(target, "target")
|
||||||
|
position_error = float(np.linalg.norm(current.translation - target.translation))
|
||||||
|
rotation_delta = current.rotation.T @ target.rotation
|
||||||
|
orientation_error = float(np.linalg.norm(pin.log3(rotation_delta)))
|
||||||
|
return position_error, orientation_error
|
||||||
|
|
||||||
|
|
||||||
|
class RM75Kinematics:
|
||||||
|
"""Pinocchio kinematics for one RM75-B.
|
||||||
|
|
||||||
|
Instances own mutable Pinocchio data and are intentionally not thread-safe.
|
||||||
|
Use one instance per arm/control thread.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
urdf_path: Optional[Path | str] = None,
|
||||||
|
limits: Optional[JointLimits] = None,
|
||||||
|
) -> None:
|
||||||
|
self.urdf_path = Path(urdf_path) if urdf_path is not None else default_urdf_path()
|
||||||
|
if not self.urdf_path.is_file():
|
||||||
|
raise FileNotFoundError(self.urdf_path)
|
||||||
|
self.model = pin.buildModelFromUrdf(str(self.urdf_path))
|
||||||
|
if self.model.nq != 7 or self.model.nv != 7:
|
||||||
|
raise ValueError(
|
||||||
|
f"expected RM75 model nq=nv=7, got nq={self.model.nq}, nv={self.model.nv}"
|
||||||
|
)
|
||||||
|
joint_names = tuple(self.model.names[1:])
|
||||||
|
if joint_names != EXPECTED_JOINT_NAMES:
|
||||||
|
raise ValueError(f"unexpected RM75 joint order: {joint_names}")
|
||||||
|
frame_id = self.model.getFrameId(FLANGE_FRAME)
|
||||||
|
if frame_id >= len(self.model.frames):
|
||||||
|
raise ValueError(f"missing flange frame {FLANGE_FRAME!r}")
|
||||||
|
self.flange_frame_id = frame_id
|
||||||
|
self.limits = limits or physical_joint_limits()
|
||||||
|
self.model.lowerPositionLimit[:7] = self.limits.lower
|
||||||
|
self.model.upperPositionLimit[:7] = self.limits.upper
|
||||||
|
self.data = self.model.createData()
|
||||||
|
|
||||||
|
def validate_q(self, q_rad: np.ndarray, *, require_within_limits: bool = True) -> np.ndarray:
|
||||||
|
q = np.asarray(q_rad, dtype=float)
|
||||||
|
if q.shape != (7,):
|
||||||
|
raise ValueError(f"RM75 configuration must have shape (7,), got {q.shape}")
|
||||||
|
if not np.all(np.isfinite(q)):
|
||||||
|
raise ValueError("RM75 configuration must be finite")
|
||||||
|
if require_within_limits and not self.limits.contains(q):
|
||||||
|
raise ValueError(f"configuration is outside {self.limits.name} joint limits")
|
||||||
|
return q.copy()
|
||||||
|
|
||||||
|
def forward(self, q_rad: np.ndarray, tool: Optional[pin.SE3] = None) -> pin.SE3:
|
||||||
|
q = self.validate_q(q_rad)
|
||||||
|
pin.framesForwardKinematics(self.model, self.data, q)
|
||||||
|
flange = self.data.oMf[self.flange_frame_id]
|
||||||
|
result = pin.SE3(flange.rotation.copy(), flange.translation.copy())
|
||||||
|
if tool is not None:
|
||||||
|
validate_se3(tool, "tool")
|
||||||
|
result = result * tool
|
||||||
|
return result
|
||||||
|
|
||||||
|
def jacobian(self, q_rad: np.ndarray) -> np.ndarray:
|
||||||
|
q = self.validate_q(q_rad)
|
||||||
|
jacobian = pin.computeFrameJacobian(
|
||||||
|
self.model,
|
||||||
|
self.data,
|
||||||
|
q,
|
||||||
|
self.flange_frame_id,
|
||||||
|
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
|
||||||
217
ik_qp/src/rm75_ik/mujoco_robot.py
Normal file
217
ik_qp/src/rm75_ik/mujoco_robot.py
Normal file
@ -0,0 +1,217 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from contextlib import nullcontext
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from time import sleep
|
||||||
|
from typing import Callable, 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_joint_positions: Optional[Dict[ArmName, np.ndarray]] = None
|
||||||
|
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"])
|
||||||
|
self._initial_joint_positions = {
|
||||||
|
arm: solved[arm].copy() for arm in ("left", "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 reset_to_initial(self) -> None:
|
||||||
|
"""Restore the solved startup state without recreating the MuJoCo model."""
|
||||||
|
|
||||||
|
self._require_open()
|
||||||
|
if self._initial_joint_positions is None:
|
||||||
|
raise RuntimeError("MuJoCo initial joint positions are unavailable")
|
||||||
|
viewer_lock = self._viewer.lock() if self._viewer is not None else nullcontext()
|
||||||
|
with viewer_lock:
|
||||||
|
self.scene.set_dual_configuration(
|
||||||
|
self._initial_joint_positions["left"],
|
||||||
|
self._initial_joint_positions["right"],
|
||||||
|
)
|
||||||
|
self.scene.data.qvel[:] = 0.0
|
||||||
|
for arm in ("left", "right"):
|
||||||
|
self.set_target_tcp_pose(arm, self.profiles[arm].initial_tcp)
|
||||||
|
self._stopped_arms.update(("left", "right"))
|
||||||
|
|
||||||
|
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, key_callback: Optional[Callable[[int], None]] = None):
|
||||||
|
self._require_open()
|
||||||
|
if self._viewer is None:
|
||||||
|
import mujoco.viewer
|
||||||
|
|
||||||
|
self._viewer = mujoco.viewer.launch_passive(
|
||||||
|
self.scene.model,
|
||||||
|
self.scene.data,
|
||||||
|
key_callback=key_callback,
|
||||||
|
)
|
||||||
|
self._viewer.set_texts(
|
||||||
|
(
|
||||||
|
None,
|
||||||
|
mujoco.mjtGridPos.mjGRID_BOTTOMLEFT,
|
||||||
|
"Reset dual arms",
|
||||||
|
"R / Home",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
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)
|
||||||
95
ik_qp/src/rm75_ik/realman_reference.py
Normal file
95
ik_qp/src/rm75_ik/realman_reference.py
Normal file
@ -0,0 +1,95 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import importlib
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pinocchio as pin
|
||||||
|
|
||||||
|
from .kinematics import validate_se3
|
||||||
|
|
||||||
|
|
||||||
|
class RealManFkReference:
|
||||||
|
"""Offline RealMan Algo FK reference; this class never opens a robot connection."""
|
||||||
|
|
||||||
|
def __init__(self, sdk_root: Optional[Path | str] = None) -> None:
|
||||||
|
selected_root = sdk_root or os.environ.get("REALMAN_SDK_ROOT")
|
||||||
|
if selected_root is not None:
|
||||||
|
root = Path(selected_root).expanduser().resolve()
|
||||||
|
if not (root / "Robotic_Arm").is_dir():
|
||||||
|
raise FileNotFoundError(
|
||||||
|
f"RealMan SDK root must contain Robotic_Arm/: {root}"
|
||||||
|
)
|
||||||
|
root_text = str(root)
|
||||||
|
if root_text not in sys.path:
|
||||||
|
sys.path.insert(0, root_text)
|
||||||
|
try:
|
||||||
|
module = importlib.import_module("Robotic_Arm.rm_robot_interface")
|
||||||
|
ctypes_module = importlib.import_module("Robotic_Arm.rm_ctypes_wrap")
|
||||||
|
except ImportError as exc:
|
||||||
|
raise ImportError(
|
||||||
|
"RealMan API2 Python SDK is unavailable; set REALMAN_SDK_ROOT "
|
||||||
|
"or pass sdk_root"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
self._rm_frame_t = module.rm_frame_t
|
||||||
|
self._algo = module.Algo(
|
||||||
|
module.rm_robot_arm_model_e.RM_MODEL_RM_75_E,
|
||||||
|
module.rm_force_type_e.RM_MODEL_RM_B_E,
|
||||||
|
)
|
||||||
|
self.api_version = str(ctypes_module.rm_api_version())
|
||||||
|
self._active_tool_key: tuple[float, ...] | None = None
|
||||||
|
self._set_work_frame_identity()
|
||||||
|
self._set_tool_frame(None)
|
||||||
|
|
||||||
|
def _set_work_frame_identity(self) -> None:
|
||||||
|
frame = self._rm_frame_t(
|
||||||
|
frame_name="s1_work",
|
||||||
|
pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
|
||||||
|
payload=0.0,
|
||||||
|
x=0.0,
|
||||||
|
y=0.0,
|
||||||
|
z=0.0,
|
||||||
|
)
|
||||||
|
self._algo.rm_algo_set_workframe(frame)
|
||||||
|
|
||||||
|
def _set_tool_frame(self, tool: Optional[pin.SE3]) -> None:
|
||||||
|
if tool is None:
|
||||||
|
pose = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||||
|
else:
|
||||||
|
validate_se3(tool, "tool")
|
||||||
|
rpy = pin.rpy.matrixToRpy(tool.rotation)
|
||||||
|
pose = tuple(float(value) for value in (*tool.translation, *rpy))
|
||||||
|
key = tuple(round(value, 12) for value in pose)
|
||||||
|
if key == self._active_tool_key:
|
||||||
|
return
|
||||||
|
frame = self._rm_frame_t(
|
||||||
|
frame_name="s1_tool",
|
||||||
|
pose=pose,
|
||||||
|
payload=0.0,
|
||||||
|
x=0.0,
|
||||||
|
y=0.0,
|
||||||
|
z=0.0,
|
||||||
|
)
|
||||||
|
self._algo.rm_algo_set_toolframe(frame)
|
||||||
|
self._active_tool_key = key
|
||||||
|
|
||||||
|
def forward(self, q_rad: np.ndarray, tool: Optional[pin.SE3] = None) -> pin.SE3:
|
||||||
|
q = np.asarray(q_rad, dtype=float)
|
||||||
|
if q.shape != (7,) or not np.all(np.isfinite(q)):
|
||||||
|
raise ValueError("RealMan FK configuration must be a finite shape-(7,) vector")
|
||||||
|
self._set_tool_frame(tool)
|
||||||
|
pose = self._algo.rm_algo_forward_kinematics(np.rad2deg(q).tolist(), flag=0)
|
||||||
|
if len(pose) != 7 or not np.all(np.isfinite(pose)):
|
||||||
|
raise RuntimeError(f"RealMan Algo returned an invalid FK pose: {pose!r}")
|
||||||
|
quaternion_values = np.asarray(pose[3:7], dtype=float)
|
||||||
|
norm = float(np.linalg.norm(quaternion_values))
|
||||||
|
if norm <= 0.0:
|
||||||
|
raise RuntimeError("RealMan Algo returned a zero quaternion")
|
||||||
|
qw, qx, qy, qz = quaternion_values / norm
|
||||||
|
quaternion = pin.Quaternion(qw, qx, qy, qz)
|
||||||
|
return pin.SE3(quaternion.matrix(), np.asarray(pose[:3], dtype=float))
|
||||||
|
|
||||||
45
ik_qp/src/rm75_ik/robot_backend.py
Normal file
45
ik_qp/src/rm75_ik/robot_backend.py
Normal file
@ -0,0 +1,45 @@
|
|||||||
|
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 reset_to_initial(self) -> None: ...
|
||||||
|
|
||||||
|
def stop(self, arms: Collection[ArmName] = ("left", "right")) -> None: ...
|
||||||
|
|
||||||
|
def close(self) -> None: ...
|
||||||
295
ik_qp/src/rm75_ik/solver.py
Normal file
295
ik_qp/src/rm75_ik/solver.py
Normal file
@ -0,0 +1,295 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import replace
|
||||||
|
from time import perf_counter
|
||||||
|
from typing import Iterable, List
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import osqp
|
||||||
|
import pinocchio as pin
|
||||||
|
from scipy import sparse
|
||||||
|
|
||||||
|
from .kinematics import RM75Kinematics, pose_errors, validate_se3
|
||||||
|
from .types import IkOptions, IkResult, IkStatus, JointLimits
|
||||||
|
|
||||||
|
|
||||||
|
class RM75IkSolver:
|
||||||
|
"""Single-seed differential IK solved with a reused OSQP workspace."""
|
||||||
|
|
||||||
|
def __init__(self, kinematics: RM75Kinematics) -> None:
|
||||||
|
self.kinematics = kinematics
|
||||||
|
self.model = kinematics.model
|
||||||
|
self.data = kinematics.data
|
||||||
|
self.frame_id = kinematics.flange_frame_id
|
||||||
|
self._n = 7
|
||||||
|
joint_range = kinematics.limits.upper - kinematics.limits.lower
|
||||||
|
self._joint_mid = 0.5 * (
|
||||||
|
kinematics.limits.lower + kinematics.limits.upper
|
||||||
|
)
|
||||||
|
self._joint_limit_metric_diag = 1.0 / np.square(joint_range)
|
||||||
|
|
||||||
|
pattern = sparse.triu(np.ones((self._n, self._n)), format="csc")
|
||||||
|
self._p_rows = pattern.indices.copy()
|
||||||
|
self._p_cols = np.repeat(np.arange(self._n), np.diff(pattern.indptr))
|
||||||
|
constraints = sparse.eye(self._n, format="csc")
|
||||||
|
self._osqp = osqp.OSQP()
|
||||||
|
self._osqp.setup(
|
||||||
|
P=pattern,
|
||||||
|
q=np.zeros(self._n),
|
||||||
|
A=constraints,
|
||||||
|
l=-np.ones(self._n),
|
||||||
|
u=np.ones(self._n),
|
||||||
|
verbose=False,
|
||||||
|
warm_start=True,
|
||||||
|
polish=False,
|
||||||
|
eps_abs=1e-6,
|
||||||
|
eps_rel=1e-6,
|
||||||
|
max_iter=1000,
|
||||||
|
)
|
||||||
|
|
||||||
|
def _regularization_terms(
|
||||||
|
self,
|
||||||
|
q: np.ndarray,
|
||||||
|
q_reference: np.ndarray,
|
||||||
|
options: IkOptions,
|
||||||
|
damping: float,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray]:
|
||||||
|
"""Return Hessian and gradient terms unrelated to the TCP task."""
|
||||||
|
|
||||||
|
motion_weights = np.asarray(options.joint_motion_weights, dtype=float)
|
||||||
|
diagonal = damping * damping * motion_weights
|
||||||
|
diagonal += options.posture_weight
|
||||||
|
diagonal += (
|
||||||
|
options.joint_limit_mid_weight * self._joint_limit_metric_diag
|
||||||
|
)
|
||||||
|
gradient = options.posture_weight * (q - q_reference)
|
||||||
|
gradient += (
|
||||||
|
options.joint_limit_mid_weight
|
||||||
|
* self._joint_limit_metric_diag
|
||||||
|
* (q - self._joint_mid)
|
||||||
|
)
|
||||||
|
return np.diag(diagonal), gradient
|
||||||
|
|
||||||
|
def _step_bounds(
|
||||||
|
self, q: np.ndarray, options: IkOptions
|
||||||
|
) -> tuple[np.ndarray, np.ndarray]:
|
||||||
|
if options.joint_step_limits_rad is None:
|
||||||
|
step_limits = np.full(self._n, options.trust_region_rad)
|
||||||
|
else:
|
||||||
|
step_limits = np.asarray(options.joint_step_limits_rad, dtype=float)
|
||||||
|
lower = np.maximum(
|
||||||
|
-step_limits,
|
||||||
|
self.kinematics.limits.lower - q,
|
||||||
|
)
|
||||||
|
upper = np.minimum(
|
||||||
|
step_limits,
|
||||||
|
self.kinematics.limits.upper - q,
|
||||||
|
)
|
||||||
|
return lower, upper
|
||||||
|
|
||||||
|
def solve(
|
||||||
|
self,
|
||||||
|
target_se3: pin.SE3,
|
||||||
|
seed_rad: np.ndarray,
|
||||||
|
options: IkOptions = IkOptions(),
|
||||||
|
) -> IkResult:
|
||||||
|
started = perf_counter()
|
||||||
|
try:
|
||||||
|
validate_se3(target_se3, "target_se3")
|
||||||
|
q = self.kinematics.validate_q(seed_rad)
|
||||||
|
except (TypeError, ValueError) as exc:
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.INVALID_INPUT,
|
||||||
|
None,
|
||||||
|
float("inf"),
|
||||||
|
float("inf"),
|
||||||
|
0,
|
||||||
|
perf_counter() - started,
|
||||||
|
message=str(exc),
|
||||||
|
)
|
||||||
|
|
||||||
|
q_reference = q.copy()
|
||||||
|
weights = np.diag(np.asarray(options.task_weights, dtype=float))
|
||||||
|
damping = options.damping_initial
|
||||||
|
previous_error = float("inf")
|
||||||
|
best_error = float("inf")
|
||||||
|
stagnant_iterations = 0
|
||||||
|
last_osqp_status = ""
|
||||||
|
position_error = float("inf")
|
||||||
|
orientation_error = float("inf")
|
||||||
|
|
||||||
|
for iteration in range(options.max_iterations + 1):
|
||||||
|
elapsed = perf_counter() - started
|
||||||
|
if options.time_limit_sec is not None and elapsed >= options.time_limit_sec:
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.TIME_LIMIT,
|
||||||
|
None,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
iteration,
|
||||||
|
elapsed,
|
||||||
|
last_osqp_status,
|
||||||
|
"IK time budget exhausted",
|
||||||
|
)
|
||||||
|
|
||||||
|
pin.computeJointJacobians(self.model, self.data, q)
|
||||||
|
pin.framesForwardKinematics(self.model, self.data, q)
|
||||||
|
current = self.data.oMf[self.frame_id]
|
||||||
|
position_error, orientation_error = pose_errors(current, target_se3)
|
||||||
|
if (
|
||||||
|
position_error <= options.position_tolerance_m
|
||||||
|
and orientation_error <= options.orientation_tolerance_rad
|
||||||
|
):
|
||||||
|
solution = q.copy()
|
||||||
|
solution.setflags(write=False)
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.SUCCESS,
|
||||||
|
solution,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
iteration,
|
||||||
|
perf_counter() - started,
|
||||||
|
last_osqp_status,
|
||||||
|
)
|
||||||
|
|
||||||
|
if iteration == options.max_iterations:
|
||||||
|
break
|
||||||
|
|
||||||
|
error_transform = current.actInv(target_se3)
|
||||||
|
error_vector = pin.log6(error_transform).vector
|
||||||
|
error_norm = float(np.linalg.norm(error_vector))
|
||||||
|
if error_norm < best_error - options.stagnation_delta:
|
||||||
|
best_error = error_norm
|
||||||
|
stagnant_iterations = 0
|
||||||
|
else:
|
||||||
|
stagnant_iterations += 1
|
||||||
|
if stagnant_iterations >= options.stagnation_iterations:
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.STAGNATED,
|
||||||
|
None,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
iteration,
|
||||||
|
perf_counter() - started,
|
||||||
|
last_osqp_status,
|
||||||
|
"SE(3) error stopped improving",
|
||||||
|
)
|
||||||
|
|
||||||
|
if error_norm > previous_error * 1.1 and iteration > 10:
|
||||||
|
damping = min(options.damping_max, damping * 1.5)
|
||||||
|
else:
|
||||||
|
damping = max(options.damping_min, damping * options.damping_reduction)
|
||||||
|
|
||||||
|
jacobian = pin.getFrameJacobian(
|
||||||
|
self.model,
|
||||||
|
self.data,
|
||||||
|
self.frame_id,
|
||||||
|
pin.ReferenceFrame.LOCAL,
|
||||||
|
)
|
||||||
|
effective_jacobian = pin.Jlog6(error_transform) @ jacobian
|
||||||
|
hessian = effective_jacobian.T @ weights @ effective_jacobian
|
||||||
|
gradient = -effective_jacobian.T @ weights @ error_vector
|
||||||
|
regularization_hessian, regularization_gradient = (
|
||||||
|
self._regularization_terms(
|
||||||
|
q,
|
||||||
|
q_reference,
|
||||||
|
options,
|
||||||
|
damping,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
hessian += regularization_hessian
|
||||||
|
gradient += regularization_gradient
|
||||||
|
|
||||||
|
lower, upper = self._step_bounds(q, options)
|
||||||
|
p_values = hessian[self._p_rows, self._p_cols]
|
||||||
|
self._osqp.update(Px=p_values, q=gradient, l=lower, u=upper)
|
||||||
|
osqp_result = self._osqp.solve()
|
||||||
|
last_osqp_status = str(osqp_result.info.status)
|
||||||
|
if last_osqp_status.lower() != "solved" or osqp_result.x is None:
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.OSQP_FAILURE,
|
||||||
|
None,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
iteration,
|
||||||
|
perf_counter() - started,
|
||||||
|
last_osqp_status,
|
||||||
|
"OSQP did not return a solved step",
|
||||||
|
)
|
||||||
|
step = np.asarray(osqp_result.x, dtype=float)
|
||||||
|
if step.shape != (7,) or not np.all(np.isfinite(step)):
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.OSQP_FAILURE,
|
||||||
|
None,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
iteration,
|
||||||
|
perf_counter() - started,
|
||||||
|
last_osqp_status,
|
||||||
|
"OSQP returned a non-finite step",
|
||||||
|
)
|
||||||
|
q = pin.integrate(self.model, q, step)
|
||||||
|
q = np.clip(
|
||||||
|
q,
|
||||||
|
self.kinematics.limits.lower,
|
||||||
|
self.kinematics.limits.upper,
|
||||||
|
)
|
||||||
|
previous_error = error_norm
|
||||||
|
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.MAX_ITERATIONS,
|
||||||
|
None,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
options.max_iterations,
|
||||||
|
perf_counter() - started,
|
||||||
|
last_osqp_status,
|
||||||
|
"maximum IK iterations reached",
|
||||||
|
)
|
||||||
|
|
||||||
|
def solve_multistart(
|
||||||
|
self,
|
||||||
|
target_se3: pin.SE3,
|
||||||
|
seeds_rad: Iterable[np.ndarray],
|
||||||
|
options: IkOptions = IkOptions(),
|
||||||
|
) -> IkResult:
|
||||||
|
started = perf_counter()
|
||||||
|
last_result: IkResult | None = None
|
||||||
|
for index, seed in enumerate(seeds_rad, start=1):
|
||||||
|
result = self.solve(target_se3, seed, options)
|
||||||
|
if result.success:
|
||||||
|
return replace(
|
||||||
|
result,
|
||||||
|
solve_time_sec=perf_counter() - started,
|
||||||
|
message=f"converged from recovery seed {index}",
|
||||||
|
)
|
||||||
|
last_result = result
|
||||||
|
if last_result is None:
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.INVALID_INPUT,
|
||||||
|
None,
|
||||||
|
float("inf"),
|
||||||
|
float("inf"),
|
||||||
|
0,
|
||||||
|
perf_counter() - started,
|
||||||
|
message="no recovery seeds were provided",
|
||||||
|
)
|
||||||
|
return replace(
|
||||||
|
last_result,
|
||||||
|
solve_time_sec=perf_counter() - started,
|
||||||
|
message=f"all recovery seeds failed; last error: {last_result.message}",
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def deterministic_recovery_seeds(
|
||||||
|
limits: JointLimits,
|
||||||
|
count: int = 8,
|
||||||
|
random_seed: int = 75,
|
||||||
|
) -> List[np.ndarray]:
|
||||||
|
if count <= 0:
|
||||||
|
raise ValueError("recovery seed count must be positive")
|
||||||
|
seeds = [np.clip(np.zeros(7), limits.lower, limits.upper)]
|
||||||
|
rng = np.random.default_rng(random_seed)
|
||||||
|
while len(seeds) < count:
|
||||||
|
seeds.append(rng.uniform(limits.lower, limits.upper))
|
||||||
|
return seeds
|
||||||
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
|
||||||
229
ik_qp/src/rm75_ik/teleop_config.py
Normal file
229
ik_qp/src/rm75_ik/teleop_config.py
Normal file
@ -0,0 +1,229 @@
|
|||||||
|
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
|
||||||
|
qp_w_limit_mid: float
|
||||||
|
qp_joint_motion_weights: np.ndarray
|
||||||
|
qp_joint_step_limits_rad: 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)
|
||||||
|
motion_weights = _readonly_vector(
|
||||||
|
self.qp_joint_motion_weights,
|
||||||
|
(7,),
|
||||||
|
"qp_joint_motion_weights",
|
||||||
|
)
|
||||||
|
if np.any(motion_weights <= 0.0):
|
||||||
|
raise ValueError("qp_joint_motion_weights must be positive")
|
||||||
|
object.__setattr__(self, "qp_joint_motion_weights", motion_weights)
|
||||||
|
step_limits = _readonly_vector(
|
||||||
|
self.qp_joint_step_limits_rad,
|
||||||
|
(7,),
|
||||||
|
"qp_joint_step_limits_rad",
|
||||||
|
)
|
||||||
|
if np.any(step_limits <= 0.0):
|
||||||
|
raise ValueError("qp_joint_step_limits_rad must be positive")
|
||||||
|
object.__setattr__(self, "qp_joint_step_limits_rad", step_limits)
|
||||||
|
if not np.isfinite(self.qp_w_limit_mid) or self.qp_w_limit_mid < 0.0:
|
||||||
|
raise ValueError("qp_w_limit_mid must be finite and non-negative")
|
||||||
|
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)),
|
||||||
|
qp_w_limit_mid=float(params.get("qp_w_limit_mid", 0.0)),
|
||||||
|
qp_joint_motion_weights=params.get(
|
||||||
|
"qp_joint_motion_weights",
|
||||||
|
[1.0] * 7,
|
||||||
|
),
|
||||||
|
qp_joint_step_limits_rad=params.get(
|
||||||
|
"qp_joint_step_limits_rad",
|
||||||
|
[0.05] * 7,
|
||||||
|
),
|
||||||
|
)
|
||||||
|
return profiles
|
||||||
492
ik_qp/src/rm75_ik/teleop_control.py
Normal file
492
ik_qp/src/rm75_ik/teleop_control.py
Normal file
@ -0,0 +1,492 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass, replace
|
||||||
|
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"
|
||||||
|
RESETTING = "resetting"
|
||||||
|
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._reset_waiting_for_release = False
|
||||||
|
self._closed = False
|
||||||
|
self.robot.connect()
|
||||||
|
|
||||||
|
@property
|
||||||
|
def safety_state(self) -> SafetyState:
|
||||||
|
if self._fault_reason:
|
||||||
|
return SafetyState.FAULT
|
||||||
|
if self._reset_waiting_for_release:
|
||||||
|
return SafetyState.RESETTING
|
||||||
|
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 reset_to_initial(self) -> ControlCycleResult:
|
||||||
|
"""Reset the backend and require fresh dual-grip release before rearming."""
|
||||||
|
|
||||||
|
if self._closed:
|
||||||
|
raise RuntimeError("controller is closed")
|
||||||
|
for mapper in self.mappers.values():
|
||||||
|
mapper.reset()
|
||||||
|
try:
|
||||||
|
self.robot.stop(("left", "right"))
|
||||||
|
self.robot.reset_to_initial()
|
||||||
|
except Exception as exc:
|
||||||
|
self._trip_fault(f"reset/backend failure: {exc}")
|
||||||
|
raise RuntimeError(f"failed to reset robot backend: {exc}") from exc
|
||||||
|
self._latest.clear()
|
||||||
|
self._fault_reason = ""
|
||||||
|
self._reset_waiting_for_release = True
|
||||||
|
return ControlCycleResult(
|
||||||
|
SafetyState.RESETTING,
|
||||||
|
reason="reset complete; waiting for fresh dual-grip release",
|
||||||
|
)
|
||||||
|
|
||||||
|
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)
|
||||||
|
if self._reset_waiting_for_release:
|
||||||
|
if self._can_finish_reset(timestamp_sec):
|
||||||
|
self._reset_waiting_for_release = False
|
||||||
|
self._latest.clear()
|
||||||
|
return ControlCycleResult(SafetyState.IDLE, reason="reset rearmed")
|
||||||
|
return ControlCycleResult(
|
||||||
|
SafetyState.RESETTING,
|
||||||
|
reason="waiting for fresh dual-grip release",
|
||||||
|
)
|
||||||
|
|
||||||
|
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()
|
||||||
|
)
|
||||||
|
arm_ik_options = replace(
|
||||||
|
self.ik_options,
|
||||||
|
joint_limit_mid_weight=self.profiles[arm].qp_w_limit_mid,
|
||||||
|
joint_motion_weights=tuple(
|
||||||
|
float(value)
|
||||||
|
for value in self.profiles[arm].qp_joint_motion_weights
|
||||||
|
),
|
||||||
|
joint_step_limits_rad=tuple(
|
||||||
|
float(value)
|
||||||
|
for value in self.profiles[arm].qp_joint_step_limits_rad
|
||||||
|
),
|
||||||
|
)
|
||||||
|
result = self.solvers[arm].solve(
|
||||||
|
flange_target,
|
||||||
|
q_current,
|
||||||
|
arm_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 _can_finish_reset(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
|
||||||
|
age = timestamp_sec - sample_time
|
||||||
|
if age < -1e-6 or age > 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)
|
||||||
|
self._reset_waiting_for_release = False
|
||||||
|
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:
|
||||||
|
self._reset_waiting_for_release = False
|
||||||
|
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
|
||||||
170
ik_qp/src/rm75_ik/types.py
Normal file
170
ik_qp/src/rm75_ik/types.py
Normal file
@ -0,0 +1,170 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from enum import Enum
|
||||||
|
from math import radians
|
||||||
|
from typing import Optional, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
class IkStatus(str, Enum):
|
||||||
|
SUCCESS = "success"
|
||||||
|
INVALID_INPUT = "invalid_input"
|
||||||
|
OSQP_FAILURE = "osqp_failure"
|
||||||
|
STAGNATED = "stagnated"
|
||||||
|
TIME_LIMIT = "time_limit"
|
||||||
|
MAX_ITERATIONS = "max_iterations"
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class JointLimits:
|
||||||
|
name: str
|
||||||
|
lower: np.ndarray
|
||||||
|
upper: np.ndarray
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
lower = np.asarray(self.lower, dtype=float).copy()
|
||||||
|
upper = np.asarray(self.upper, dtype=float).copy()
|
||||||
|
if lower.shape != (7,) or upper.shape != (7,):
|
||||||
|
raise ValueError("RM75 joint limits must each have shape (7,)")
|
||||||
|
if not np.all(np.isfinite(lower)) or not np.all(np.isfinite(upper)):
|
||||||
|
raise ValueError("joint limits must be finite")
|
||||||
|
if np.any(lower >= upper):
|
||||||
|
raise ValueError("every lower joint limit must be below its upper limit")
|
||||||
|
lower.setflags(write=False)
|
||||||
|
upper.setflags(write=False)
|
||||||
|
object.__setattr__(self, "lower", lower)
|
||||||
|
object.__setattr__(self, "upper", upper)
|
||||||
|
|
||||||
|
def contains(self, q: np.ndarray, tolerance: float = 1e-10) -> bool:
|
||||||
|
values = np.asarray(q, dtype=float)
|
||||||
|
return bool(
|
||||||
|
values.shape == (7,)
|
||||||
|
and np.all(values >= self.lower - tolerance)
|
||||||
|
and np.all(values <= self.upper + tolerance)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def physical_joint_limits() -> JointLimits:
|
||||||
|
upper = np.deg2rad([178.0, 130.0, 178.0, 135.0, 178.0, 128.0, 360.0])
|
||||||
|
return JointLimits("physical", -upper, upper)
|
||||||
|
|
||||||
|
|
||||||
|
def teleop_joint_limits() -> JointLimits:
|
||||||
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
def joint_limit_profile(name: str) -> JointLimits:
|
||||||
|
profiles = {
|
||||||
|
"physical": physical_joint_limits,
|
||||||
|
"teleop": teleop_joint_limits,
|
||||||
|
}
|
||||||
|
try:
|
||||||
|
return profiles[name]()
|
||||||
|
except KeyError as exc:
|
||||||
|
raise ValueError(f"unknown joint limit profile: {name!r}") from exc
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class IkOptions:
|
||||||
|
position_tolerance_m: float = 1e-3
|
||||||
|
orientation_tolerance_rad: float = radians(0.1)
|
||||||
|
max_iterations: int = 500
|
||||||
|
time_limit_sec: Optional[float] = None
|
||||||
|
trust_region_rad: float = 0.05
|
||||||
|
task_weights: Tuple[float, float, float, float, float, float] = (
|
||||||
|
1.0,
|
||||||
|
1.0,
|
||||||
|
1.0,
|
||||||
|
0.4,
|
||||||
|
0.4,
|
||||||
|
0.4,
|
||||||
|
)
|
||||||
|
posture_weight: float = 1e-5
|
||||||
|
joint_limit_mid_weight: float = 0.0
|
||||||
|
joint_motion_weights: Tuple[float, float, float, float, float, float, float] = (
|
||||||
|
1.0,
|
||||||
|
1.0,
|
||||||
|
1.0,
|
||||||
|
1.0,
|
||||||
|
1.0,
|
||||||
|
1.0,
|
||||||
|
1.0,
|
||||||
|
)
|
||||||
|
joint_step_limits_rad: Optional[
|
||||||
|
Tuple[float, float, float, float, float, float, float]
|
||||||
|
] = None
|
||||||
|
damping_initial: float = 0.1
|
||||||
|
damping_min: float = 0.01
|
||||||
|
damping_max: float = 1.0
|
||||||
|
damping_reduction: float = 0.95
|
||||||
|
stagnation_iterations: int = 40
|
||||||
|
stagnation_delta: float = 1e-9
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
positive = {
|
||||||
|
"position_tolerance_m": self.position_tolerance_m,
|
||||||
|
"orientation_tolerance_rad": self.orientation_tolerance_rad,
|
||||||
|
"trust_region_rad": self.trust_region_rad,
|
||||||
|
"damping_initial": self.damping_initial,
|
||||||
|
"damping_min": self.damping_min,
|
||||||
|
"damping_max": self.damping_max,
|
||||||
|
}
|
||||||
|
for name, value in positive.items():
|
||||||
|
if not np.isfinite(value) or value <= 0.0:
|
||||||
|
raise ValueError(f"{name} must be finite and positive")
|
||||||
|
if self.max_iterations <= 0 or self.stagnation_iterations <= 0:
|
||||||
|
raise ValueError("iteration limits must be positive")
|
||||||
|
if self.time_limit_sec is not None and self.time_limit_sec <= 0.0:
|
||||||
|
raise ValueError("time_limit_sec must be positive when set")
|
||||||
|
if len(self.task_weights) != 6 or any(weight <= 0.0 for weight in self.task_weights):
|
||||||
|
raise ValueError("task_weights must contain six positive values")
|
||||||
|
if self.posture_weight < 0.0 or not np.isfinite(self.posture_weight):
|
||||||
|
raise ValueError("posture_weight must be finite and non-negative")
|
||||||
|
if (
|
||||||
|
self.joint_limit_mid_weight < 0.0
|
||||||
|
or not np.isfinite(self.joint_limit_mid_weight)
|
||||||
|
):
|
||||||
|
raise ValueError(
|
||||||
|
"joint_limit_mid_weight must be finite and non-negative"
|
||||||
|
)
|
||||||
|
if len(self.joint_motion_weights) != 7 or any(
|
||||||
|
not np.isfinite(weight) or weight <= 0.0
|
||||||
|
for weight in self.joint_motion_weights
|
||||||
|
):
|
||||||
|
raise ValueError(
|
||||||
|
"joint_motion_weights must contain seven finite positive values"
|
||||||
|
)
|
||||||
|
if self.joint_step_limits_rad is not None and (
|
||||||
|
len(self.joint_step_limits_rad) != 7
|
||||||
|
or any(
|
||||||
|
not np.isfinite(limit) or limit <= 0.0
|
||||||
|
for limit in self.joint_step_limits_rad
|
||||||
|
)
|
||||||
|
):
|
||||||
|
raise ValueError(
|
||||||
|
"joint_step_limits_rad must contain seven finite positive values"
|
||||||
|
)
|
||||||
|
if not self.damping_min <= self.damping_initial <= self.damping_max:
|
||||||
|
raise ValueError("damping_initial must be within damping_min and damping_max")
|
||||||
|
if not 0.0 < self.damping_reduction <= 1.0:
|
||||||
|
raise ValueError("damping_reduction must be in (0, 1]")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class IkResult:
|
||||||
|
status: IkStatus
|
||||||
|
q: Optional[np.ndarray]
|
||||||
|
position_error_m: float
|
||||||
|
orientation_error_rad: float
|
||||||
|
iterations: int
|
||||||
|
solve_time_sec: float
|
||||||
|
osqp_status: str = ""
|
||||||
|
message: str = ""
|
||||||
|
|
||||||
|
@property
|
||||||
|
def success(self) -> bool:
|
||||||
|
return self.status is IkStatus.SUCCESS
|
||||||
718
ik_qp/src/rm75_ik/validation.py
Normal file
718
ik_qp/src/rm75_ik/validation.py
Normal file
@ -0,0 +1,718 @@
|
|||||||
|
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, Iterable, List, Optional, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pinocchio as pin
|
||||||
|
import yaml
|
||||||
|
|
||||||
|
from .dual_arm import DualArmAssembly
|
||||||
|
from .kinematics import RM75Kinematics, pose_errors
|
||||||
|
from .realman_reference import RealManFkReference
|
||||||
|
from .solver import RM75IkSolver, deterministic_recovery_seeds
|
||||||
|
from .types import IkOptions, IkStatus, JointLimits, physical_joint_limits, teleop_joint_limits
|
||||||
|
|
||||||
|
|
||||||
|
FK_POSITION_LIMIT_M = 1e-4
|
||||||
|
FK_ORIENTATION_LIMIT_RAD = radians(0.01)
|
||||||
|
IK_POSITION_LIMIT_M = 1e-3
|
||||||
|
IK_ORIENTATION_LIMIT_RAD = radians(0.1)
|
||||||
|
JACOBIAN_RELATIVE_LIMIT = 1e-3
|
||||||
|
JACOBIAN_ABSOLUTE_LIMIT = 5e-4
|
||||||
|
NEAR_IK_RATE_LIMIT = 0.995
|
||||||
|
CONTINUOUS_IK_RATE_LIMIT = 0.999
|
||||||
|
GLOBAL_RECOVERY_RATE_LIMIT = 0.85
|
||||||
|
NEAR_IK_P99_LIMIT_SEC = 0.008
|
||||||
|
CONTROL_PERIOD_SEC = 1.0 / 90.0
|
||||||
|
MAX_CONTINUOUS_JOINT_STEP_RAD = radians(2.0)
|
||||||
|
|
||||||
|
|
||||||
|
def _validation_ik_options(max_iterations: int) -> IkOptions:
|
||||||
|
# Keep a 10% convergence guard band for independent Algo verification.
|
||||||
|
return IkOptions(
|
||||||
|
position_tolerance_m=0.9 * IK_POSITION_LIMIT_M,
|
||||||
|
orientation_tolerance_rad=0.9 * IK_ORIENTATION_LIMIT_RAD,
|
||||||
|
max_iterations=max_iterations,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ValidationSettings:
|
||||||
|
seed: int = 20260629
|
||||||
|
fk_samples: int = 10_000
|
||||||
|
jacobian_samples: int = 200
|
||||||
|
near_ik_samples: int = 1_000
|
||||||
|
global_samples: int = 200
|
||||||
|
continuous_trajectories: int = 20
|
||||||
|
continuous_points: int = 500
|
||||||
|
tool_samples: int = 100
|
||||||
|
dual_samples: int = 100
|
||||||
|
strict: bool = True
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def quick(cls, seed: int = 20260629, strict: bool = False) -> "ValidationSettings":
|
||||||
|
return cls(
|
||||||
|
seed=seed,
|
||||||
|
fk_samples=100,
|
||||||
|
jacobian_samples=10,
|
||||||
|
near_ik_samples=30,
|
||||||
|
global_samples=10,
|
||||||
|
continuous_trajectories=2,
|
||||||
|
continuous_points=25,
|
||||||
|
tool_samples=10,
|
||||||
|
dual_samples=10,
|
||||||
|
strict=strict,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _percentile(values: Iterable[float], percentile: float) -> float:
|
||||||
|
data = list(values)
|
||||||
|
return float(np.percentile(data, percentile)) if data else float("nan")
|
||||||
|
|
||||||
|
|
||||||
|
def _sample_configurations(
|
||||||
|
rng: np.random.Generator,
|
||||||
|
limits: JointLimits,
|
||||||
|
count: int,
|
||||||
|
margin_rad: Optional[np.ndarray] = None,
|
||||||
|
) -> np.ndarray:
|
||||||
|
margin = np.zeros(7) if margin_rad is None else np.asarray(margin_rad, dtype=float)
|
||||||
|
lower = limits.lower + margin
|
||||||
|
upper = limits.upper - margin
|
||||||
|
if np.any(lower >= upper):
|
||||||
|
raise ValueError(f"sampling margin is too large for {limits.name} limits")
|
||||||
|
return rng.uniform(lower, upper, size=(count, 7))
|
||||||
|
|
||||||
|
|
||||||
|
def _tool_pose_from_values(values: Iterable[float]) -> pin.SE3:
|
||||||
|
pose = np.asarray(list(values), dtype=float)
|
||||||
|
if pose.shape != (7,) or not np.all(np.isfinite(pose)):
|
||||||
|
raise ValueError("tool pose must be [x,y,z,qx,qy,qz,qw]")
|
||||||
|
quaternion = pin.Quaternion(pose[6], pose[3], pose[4], pose[5])
|
||||||
|
if quaternion.norm() <= 0.0:
|
||||||
|
raise ValueError("tool quaternion must be non-zero")
|
||||||
|
quaternion.normalize()
|
||||||
|
return pin.SE3(quaternion.matrix(), pose[:3])
|
||||||
|
|
||||||
|
|
||||||
|
def load_project_tools(config_path: Path | str) -> Dict[str, pin.SE3]:
|
||||||
|
with Path(config_path).open("r", encoding="utf-8") as stream:
|
||||||
|
data = yaml.safe_load(stream)
|
||||||
|
tools = data.get("tools_in_ee", {})
|
||||||
|
selected: Dict[str, pin.SE3] = {}
|
||||||
|
for name in ("scissor", "omnipic", "minisci"):
|
||||||
|
if name not in tools or "pose" not in tools[name]:
|
||||||
|
raise ValueError(f"missing tool pose for {name!r}")
|
||||||
|
selected[name] = _tool_pose_from_values(tools[name]["pose"])
|
||||||
|
return selected
|
||||||
|
|
||||||
|
|
||||||
|
class Stage1Validator:
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
reference: RealManFkReference,
|
||||||
|
settings: ValidationSettings = ValidationSettings(),
|
||||||
|
tools: Optional[Dict[str, pin.SE3]] = None,
|
||||||
|
) -> None:
|
||||||
|
self.reference = reference
|
||||||
|
self.settings = settings
|
||||||
|
self.tools = tools or {}
|
||||||
|
self.rng = np.random.default_rng(settings.seed)
|
||||||
|
self.checks: Dict[str, Dict[str, Any]] = {}
|
||||||
|
self.failures: List[Dict[str, Any]] = []
|
||||||
|
|
||||||
|
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"),
|
||||||
|
profile: str = "",
|
||||||
|
) -> None:
|
||||||
|
if len(self.failures) >= 1000:
|
||||||
|
return
|
||||||
|
self.failures.append(
|
||||||
|
{
|
||||||
|
"category": category,
|
||||||
|
"profile": profile,
|
||||||
|
"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],
|
||||||
|
*,
|
||||||
|
required: bool = True,
|
||||||
|
) -> None:
|
||||||
|
self.checks[name] = {
|
||||||
|
"passed": bool(passed),
|
||||||
|
"required": required,
|
||||||
|
**metrics,
|
||||||
|
}
|
||||||
|
|
||||||
|
def run(self) -> Dict[str, Any]:
|
||||||
|
self._model_checks()
|
||||||
|
self._fk_checks()
|
||||||
|
self._jacobian_checks()
|
||||||
|
self._near_ik_checks()
|
||||||
|
self._continuous_ik_checks()
|
||||||
|
self._global_recovery_checks()
|
||||||
|
self._singularity_checks()
|
||||||
|
self._dual_arm_checks()
|
||||||
|
self._tool_checks()
|
||||||
|
required_checks = [
|
||||||
|
check["passed"]
|
||||||
|
for check in self.checks.values()
|
||||||
|
if check.get("required", True)
|
||||||
|
]
|
||||||
|
return {
|
||||||
|
"schema_version": 1,
|
||||||
|
"generated_at": datetime.now(timezone.utc).isoformat(),
|
||||||
|
"seed": self.settings.seed,
|
||||||
|
"strict": self.settings.strict,
|
||||||
|
"realman_api_version": self.reference.api_version,
|
||||||
|
"passed": bool(all(required_checks)),
|
||||||
|
"checks": self.checks,
|
||||||
|
"failure_count": len(self.failures),
|
||||||
|
}
|
||||||
|
|
||||||
|
def _model_checks(self) -> None:
|
||||||
|
physical = RM75Kinematics(limits=physical_joint_limits())
|
||||||
|
teleop = RM75Kinematics(limits=teleop_joint_limits())
|
||||||
|
assembly = DualArmAssembly.from_source_urdf(limits=physical_joint_limits())
|
||||||
|
passed = (
|
||||||
|
physical.model.nq == physical.model.nv == 7
|
||||||
|
and teleop.model.nq == teleop.model.nv == 7
|
||||||
|
and assembly.dof == 14
|
||||||
|
and physical.limits.contains(np.zeros(7))
|
||||||
|
and teleop.limits.contains(np.zeros(7))
|
||||||
|
)
|
||||||
|
self._add_check(
|
||||||
|
"model_structure",
|
||||||
|
passed,
|
||||||
|
{
|
||||||
|
"single_arm_nq": physical.model.nq,
|
||||||
|
"single_arm_nv": physical.model.nv,
|
||||||
|
"dual_arm_dof": assembly.dof,
|
||||||
|
"right_visual_origin_delta_m": assembly.mounts.right_visual_origin_delta_m,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
def _fk_checks(self) -> None:
|
||||||
|
for limits in (physical_joint_limits(), teleop_joint_limits()):
|
||||||
|
kinematics = RM75Kinematics(limits=limits)
|
||||||
|
samples = _sample_configurations(
|
||||||
|
self.rng, limits, self.settings.fk_samples
|
||||||
|
)
|
||||||
|
position_errors: List[float] = []
|
||||||
|
orientation_errors: List[float] = []
|
||||||
|
for index, q in enumerate(samples):
|
||||||
|
pin_pose = kinematics.forward(q)
|
||||||
|
reference_pose = self.reference.forward(q)
|
||||||
|
position_error, orientation_error = pose_errors(pin_pose, reference_pose)
|
||||||
|
position_errors.append(position_error)
|
||||||
|
orientation_errors.append(orientation_error)
|
||||||
|
if (
|
||||||
|
position_error >= FK_POSITION_LIMIT_M
|
||||||
|
or orientation_error >= FK_ORIENTATION_LIMIT_RAD
|
||||||
|
):
|
||||||
|
self._record_failure(
|
||||||
|
"fk",
|
||||||
|
index,
|
||||||
|
"FK residual exceeded limit",
|
||||||
|
q,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
limits.name,
|
||||||
|
)
|
||||||
|
max_position = max(position_errors, default=float("inf"))
|
||||||
|
max_orientation = max(orientation_errors, default=float("inf"))
|
||||||
|
self._add_check(
|
||||||
|
f"fk_{limits.name}",
|
||||||
|
max_position < FK_POSITION_LIMIT_M
|
||||||
|
and max_orientation < FK_ORIENTATION_LIMIT_RAD,
|
||||||
|
{
|
||||||
|
"samples": len(samples),
|
||||||
|
"max_position_error_m": max_position,
|
||||||
|
"p99_position_error_m": _percentile(position_errors, 99),
|
||||||
|
"max_orientation_error_rad": max_orientation,
|
||||||
|
"p99_orientation_error_rad": _percentile(
|
||||||
|
orientation_errors, 99
|
||||||
|
),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
def _numeric_reference_jacobian(self, q: np.ndarray, step: float = 2e-3) -> np.ndarray:
|
||||||
|
center = self.reference.forward(q)
|
||||||
|
numeric = np.zeros((6, 7))
|
||||||
|
for joint_index in range(7):
|
||||||
|
delta = np.zeros(7)
|
||||||
|
delta[joint_index] = step
|
||||||
|
plus = self.reference.forward(q + delta)
|
||||||
|
minus = self.reference.forward(q - delta)
|
||||||
|
plus_twist = pin.log6(center.actInv(plus)).vector
|
||||||
|
minus_twist = pin.log6(center.actInv(minus)).vector
|
||||||
|
numeric[:, joint_index] = (plus_twist - minus_twist) / (2.0 * step)
|
||||||
|
return numeric
|
||||||
|
|
||||||
|
def _jacobian_checks(self) -> None:
|
||||||
|
limits = physical_joint_limits()
|
||||||
|
kinematics = RM75Kinematics(limits=limits)
|
||||||
|
# Algo FK is returned as float32. A 2e-3 rad central-difference step
|
||||||
|
# keeps quantization below the analytic-Jacobian acceptance limit.
|
||||||
|
margin = np.full(7, 3e-3)
|
||||||
|
samples = _sample_configurations(
|
||||||
|
self.rng, limits, self.settings.jacobian_samples, margin
|
||||||
|
)
|
||||||
|
relative_errors: List[float] = []
|
||||||
|
absolute_errors: List[float] = []
|
||||||
|
for index, q in enumerate(samples):
|
||||||
|
analytic = kinematics.jacobian(q)
|
||||||
|
numeric = self._numeric_reference_jacobian(q)
|
||||||
|
difference = analytic - numeric
|
||||||
|
relative = float(
|
||||||
|
np.linalg.norm(difference) / max(np.linalg.norm(numeric), 1e-12)
|
||||||
|
)
|
||||||
|
absolute = float(np.max(np.abs(difference)))
|
||||||
|
relative_errors.append(relative)
|
||||||
|
absolute_errors.append(absolute)
|
||||||
|
if relative >= JACOBIAN_RELATIVE_LIMIT or absolute >= JACOBIAN_ABSOLUTE_LIMIT:
|
||||||
|
self._record_failure(
|
||||||
|
"jacobian",
|
||||||
|
index,
|
||||||
|
f"relative={relative:.6g}, absolute={absolute:.6g}",
|
||||||
|
q,
|
||||||
|
profile=limits.name,
|
||||||
|
)
|
||||||
|
max_relative = max(relative_errors, default=float("inf"))
|
||||||
|
max_absolute = max(absolute_errors, default=float("inf"))
|
||||||
|
self._add_check(
|
||||||
|
"jacobian",
|
||||||
|
max_relative < JACOBIAN_RELATIVE_LIMIT
|
||||||
|
and max_absolute < JACOBIAN_ABSOLUTE_LIMIT,
|
||||||
|
{
|
||||||
|
"samples": len(samples),
|
||||||
|
"max_relative_error": max_relative,
|
||||||
|
"max_absolute_error": max_absolute,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
def _externally_accept_solution(
|
||||||
|
self,
|
||||||
|
target: pin.SE3,
|
||||||
|
result_q: Optional[np.ndarray],
|
||||||
|
limits: JointLimits,
|
||||||
|
) -> Tuple[bool, float, float]:
|
||||||
|
if result_q is None or not limits.contains(result_q):
|
||||||
|
return False, float("inf"), float("inf")
|
||||||
|
verified = self.reference.forward(result_q)
|
||||||
|
position_error, orientation_error = pose_errors(verified, target)
|
||||||
|
return (
|
||||||
|
position_error <= IK_POSITION_LIMIT_M
|
||||||
|
and orientation_error <= IK_ORIENTATION_LIMIT_RAD,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
)
|
||||||
|
|
||||||
|
def _near_ik_checks(self) -> None:
|
||||||
|
all_times: List[float] = []
|
||||||
|
profile_rates: Dict[str, float] = {}
|
||||||
|
for limits in (physical_joint_limits(), teleop_joint_limits()):
|
||||||
|
kinematics = RM75Kinematics(limits=limits)
|
||||||
|
solver = RM75IkSolver(kinematics)
|
||||||
|
margin = np.deg2rad([5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 10.0])
|
||||||
|
targets_q = _sample_configurations(
|
||||||
|
self.rng, limits, self.settings.near_ik_samples, margin
|
||||||
|
)
|
||||||
|
successes = 0
|
||||||
|
profile_times: List[float] = []
|
||||||
|
options = _validation_ik_options(max_iterations=200)
|
||||||
|
for index, target_q in enumerate(targets_q):
|
||||||
|
seed = np.clip(
|
||||||
|
target_q + self.rng.uniform(-radians(10), radians(10), 7),
|
||||||
|
limits.lower,
|
||||||
|
limits.upper,
|
||||||
|
)
|
||||||
|
target = self.reference.forward(target_q)
|
||||||
|
result = solver.solve(target, seed, options)
|
||||||
|
profile_times.append(result.solve_time_sec)
|
||||||
|
all_times.append(result.solve_time_sec)
|
||||||
|
accepted, position_error, orientation_error = self._externally_accept_solution(
|
||||||
|
target, result.q, limits
|
||||||
|
)
|
||||||
|
if result.success and accepted:
|
||||||
|
successes += 1
|
||||||
|
else:
|
||||||
|
self._record_failure(
|
||||||
|
"near_ik",
|
||||||
|
index,
|
||||||
|
f"status={result.status.value}; {result.message}",
|
||||||
|
seed,
|
||||||
|
position_error if np.isfinite(position_error) else result.position_error_m,
|
||||||
|
orientation_error if np.isfinite(orientation_error) else result.orientation_error_rad,
|
||||||
|
limits.name,
|
||||||
|
)
|
||||||
|
rate = successes / max(len(targets_q), 1)
|
||||||
|
profile_rates[limits.name] = rate
|
||||||
|
self._add_check(
|
||||||
|
f"near_ik_{limits.name}",
|
||||||
|
rate >= NEAR_IK_RATE_LIMIT,
|
||||||
|
{
|
||||||
|
"samples": len(targets_q),
|
||||||
|
"successes": successes,
|
||||||
|
"success_rate": rate,
|
||||||
|
"p99_time_sec": _percentile(profile_times, 99),
|
||||||
|
"max_time_sec": max(profile_times, default=float("nan")),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
p99_time = _percentile(all_times, 99)
|
||||||
|
max_time = max(all_times, default=float("inf"))
|
||||||
|
self._add_check(
|
||||||
|
"near_ik_performance",
|
||||||
|
p99_time < NEAR_IK_P99_LIMIT_SEC and max_time < CONTROL_PERIOD_SEC,
|
||||||
|
{
|
||||||
|
"p99_time_sec": p99_time,
|
||||||
|
"max_time_sec": max_time,
|
||||||
|
"p99_limit_sec": NEAR_IK_P99_LIMIT_SEC,
|
||||||
|
"control_period_sec": CONTROL_PERIOD_SEC,
|
||||||
|
},
|
||||||
|
required=self.settings.strict,
|
||||||
|
)
|
||||||
|
|
||||||
|
def _continuous_ik_checks(self) -> None:
|
||||||
|
limits = teleop_joint_limits()
|
||||||
|
kinematics = RM75Kinematics(limits=limits)
|
||||||
|
solver = RM75IkSolver(kinematics)
|
||||||
|
options = _validation_ik_options(max_iterations=100)
|
||||||
|
successes = 0
|
||||||
|
total = 0
|
||||||
|
max_joint_step = 0.0
|
||||||
|
for trajectory_index in range(self.settings.continuous_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.continuous_points) / 90.0
|
||||||
|
path = center + amplitude * np.sin(
|
||||||
|
2.0 * np.pi * times[:, None] * frequency + phase
|
||||||
|
)
|
||||||
|
seed = path[0].copy()
|
||||||
|
previous_solution = seed.copy()
|
||||||
|
for point_index, target_q in enumerate(path):
|
||||||
|
total += 1
|
||||||
|
target = self.reference.forward(target_q)
|
||||||
|
result = solver.solve(target, seed, options)
|
||||||
|
accepted, position_error, orientation_error = self._externally_accept_solution(
|
||||||
|
target, result.q, limits
|
||||||
|
)
|
||||||
|
if result.success and accepted and result.q is not None:
|
||||||
|
joint_step = float(np.max(np.abs(result.q - previous_solution)))
|
||||||
|
max_joint_step = max(max_joint_step, joint_step)
|
||||||
|
previous_solution = result.q
|
||||||
|
seed = result.q
|
||||||
|
successes += 1
|
||||||
|
else:
|
||||||
|
self._record_failure(
|
||||||
|
"continuous_ik",
|
||||||
|
trajectory_index * self.settings.continuous_points + point_index,
|
||||||
|
f"status={result.status.value}; {result.message}",
|
||||||
|
seed,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
limits.name,
|
||||||
|
)
|
||||||
|
rate = successes / max(total, 1)
|
||||||
|
self._add_check(
|
||||||
|
"continuous_ik",
|
||||||
|
rate >= CONTINUOUS_IK_RATE_LIMIT
|
||||||
|
and max_joint_step <= MAX_CONTINUOUS_JOINT_STEP_RAD,
|
||||||
|
{
|
||||||
|
"trajectories": self.settings.continuous_trajectories,
|
||||||
|
"points": total,
|
||||||
|
"successes": successes,
|
||||||
|
"success_rate": rate,
|
||||||
|
"max_joint_step_rad": max_joint_step,
|
||||||
|
"joint_step_limit_rad": MAX_CONTINUOUS_JOINT_STEP_RAD,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
def _global_recovery_checks(self) -> None:
|
||||||
|
limits = physical_joint_limits()
|
||||||
|
kinematics = RM75Kinematics(limits=limits)
|
||||||
|
solver = RM75IkSolver(kinematics)
|
||||||
|
options = _validation_ik_options(max_iterations=500)
|
||||||
|
margin = np.deg2rad([5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 10.0])
|
||||||
|
target_configurations = _sample_configurations(
|
||||||
|
self.rng, limits, self.settings.global_samples, margin
|
||||||
|
)
|
||||||
|
recovery_seeds = deterministic_recovery_seeds(limits)
|
||||||
|
single_successes = 0
|
||||||
|
recovery_successes = 0
|
||||||
|
recovery_times: List[float] = []
|
||||||
|
for index, target_q in enumerate(target_configurations):
|
||||||
|
target = self.reference.forward(target_q)
|
||||||
|
random_seed = self.rng.uniform(limits.lower, limits.upper)
|
||||||
|
single = solver.solve(target, random_seed, options)
|
||||||
|
single_accepted, _, _ = self._externally_accept_solution(
|
||||||
|
target, single.q, limits
|
||||||
|
)
|
||||||
|
single_successes += int(single.success and single_accepted)
|
||||||
|
|
||||||
|
recovered = solver.solve_multistart(target, recovery_seeds, options)
|
||||||
|
recovery_times.append(recovered.solve_time_sec)
|
||||||
|
accepted, position_error, orientation_error = self._externally_accept_solution(
|
||||||
|
target, recovered.q, limits
|
||||||
|
)
|
||||||
|
if recovered.success and accepted:
|
||||||
|
recovery_successes += 1
|
||||||
|
else:
|
||||||
|
self._record_failure(
|
||||||
|
"global_recovery",
|
||||||
|
index,
|
||||||
|
f"status={recovered.status.value}; {recovered.message}",
|
||||||
|
target_q,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
limits.name,
|
||||||
|
)
|
||||||
|
count = max(len(target_configurations), 1)
|
||||||
|
recovery_rate = recovery_successes / count
|
||||||
|
self._add_check(
|
||||||
|
"global_recovery",
|
||||||
|
recovery_rate >= GLOBAL_RECOVERY_RATE_LIMIT,
|
||||||
|
{
|
||||||
|
"samples": len(target_configurations),
|
||||||
|
"single_seed_success_rate": single_successes / count,
|
||||||
|
"recovery_successes": recovery_successes,
|
||||||
|
"recovery_success_rate": recovery_rate,
|
||||||
|
"recovery_p95_time_sec": _percentile(recovery_times, 95),
|
||||||
|
"recovery_max_time_sec": max(recovery_times, default=float("nan")),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
def _singularity_checks(self) -> None:
|
||||||
|
limits = physical_joint_limits()
|
||||||
|
solver = RM75IkSolver(RM75Kinematics(limits=limits))
|
||||||
|
singular_degrees = np.asarray(
|
||||||
|
[
|
||||||
|
[0, 0, 0, 90, 0, 0, 0],
|
||||||
|
[0, 60, 0, 0, 0, 90, 0],
|
||||||
|
[0, 0, 90, 90, 0, 90, 0],
|
||||||
|
[0, 90, 90, 90, 90, 0, 0],
|
||||||
|
],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
invalid_results = 0
|
||||||
|
total = 0
|
||||||
|
statuses: Dict[str, int] = {}
|
||||||
|
for case_index, singular_q in enumerate(np.deg2rad(singular_degrees)):
|
||||||
|
for perturbation in (-radians(0.1), 0.0, radians(0.1)):
|
||||||
|
total += 1
|
||||||
|
target_q = singular_q.copy()
|
||||||
|
target_q[case_index % 7] += perturbation
|
||||||
|
target = self.reference.forward(target_q)
|
||||||
|
seed = np.clip(
|
||||||
|
target_q + self.rng.uniform(-radians(0.5), radians(0.5), 7),
|
||||||
|
limits.lower,
|
||||||
|
limits.upper,
|
||||||
|
)
|
||||||
|
result = solver.solve(
|
||||||
|
target,
|
||||||
|
seed,
|
||||||
|
_validation_ik_options(max_iterations=200),
|
||||||
|
)
|
||||||
|
statuses[result.status.value] = statuses.get(result.status.value, 0) + 1
|
||||||
|
finite_diagnostics = np.isfinite(result.position_error_m) and np.isfinite(
|
||||||
|
result.orientation_error_rad
|
||||||
|
)
|
||||||
|
accepted, _, _ = self._externally_accept_solution(target, result.q, limits)
|
||||||
|
pseudo_success = result.status is IkStatus.SUCCESS and not accepted
|
||||||
|
if not finite_diagnostics or pseudo_success:
|
||||||
|
invalid_results += 1
|
||||||
|
self._record_failure(
|
||||||
|
"singularity",
|
||||||
|
total - 1,
|
||||||
|
"non-finite diagnostic or false success",
|
||||||
|
seed,
|
||||||
|
result.position_error_m,
|
||||||
|
result.orientation_error_rad,
|
||||||
|
limits.name,
|
||||||
|
)
|
||||||
|
self._add_check(
|
||||||
|
"singularity_behavior",
|
||||||
|
invalid_results == 0,
|
||||||
|
{
|
||||||
|
"samples": total,
|
||||||
|
"invalid_results": invalid_results,
|
||||||
|
"statuses": statuses,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
def _dual_arm_checks(self) -> None:
|
||||||
|
limits = physical_joint_limits()
|
||||||
|
assembly = DualArmAssembly.from_source_urdf(limits=limits)
|
||||||
|
samples = _sample_configurations(
|
||||||
|
self.rng, limits, self.settings.dual_samples
|
||||||
|
)
|
||||||
|
max_position = 0.0
|
||||||
|
max_orientation = 0.0
|
||||||
|
failures = 0
|
||||||
|
for arm in ("left", "right"):
|
||||||
|
mount = (
|
||||||
|
assembly.mounts.left_base if arm == "left" else assembly.mounts.right_base
|
||||||
|
)
|
||||||
|
for index, q in enumerate(samples):
|
||||||
|
world_pose = assembly.forward(arm, q)
|
||||||
|
local_pose = mount.actInv(world_pose)
|
||||||
|
reference_pose = self.reference.forward(q)
|
||||||
|
position_error, orientation_error = pose_errors(local_pose, reference_pose)
|
||||||
|
max_position = max(max_position, position_error)
|
||||||
|
max_orientation = max(max_orientation, orientation_error)
|
||||||
|
if (
|
||||||
|
position_error >= FK_POSITION_LIMIT_M
|
||||||
|
or orientation_error >= FK_ORIENTATION_LIMIT_RAD
|
||||||
|
):
|
||||||
|
failures += 1
|
||||||
|
self._record_failure(
|
||||||
|
"dual_arm",
|
||||||
|
index,
|
||||||
|
f"{arm} local FK residual exceeded limit",
|
||||||
|
q,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
arm,
|
||||||
|
)
|
||||||
|
self._add_check(
|
||||||
|
"dual_arm_assembly",
|
||||||
|
failures == 0,
|
||||||
|
{
|
||||||
|
"samples_per_arm": len(samples),
|
||||||
|
"max_position_error_m": max_position,
|
||||||
|
"max_orientation_error_rad": max_orientation,
|
||||||
|
"right_visual_origin_delta_m": assembly.mounts.right_visual_origin_delta_m,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
def _tool_checks(self) -> None:
|
||||||
|
if not self.tools:
|
||||||
|
self._add_check(
|
||||||
|
"tool_frames",
|
||||||
|
True,
|
||||||
|
{"samples": 0, "message": "no tool configuration supplied"},
|
||||||
|
required=False,
|
||||||
|
)
|
||||||
|
return
|
||||||
|
limits = teleop_joint_limits()
|
||||||
|
kinematics = RM75Kinematics(limits=limits)
|
||||||
|
samples = _sample_configurations(
|
||||||
|
self.rng, limits, self.settings.tool_samples
|
||||||
|
)
|
||||||
|
max_position = 0.0
|
||||||
|
max_orientation = 0.0
|
||||||
|
failures = 0
|
||||||
|
for tool_name, tool in self.tools.items():
|
||||||
|
for index, q in enumerate(samples):
|
||||||
|
pin_pose = kinematics.forward(q, tool)
|
||||||
|
reference_pose = self.reference.forward(q, tool)
|
||||||
|
position_error, orientation_error = pose_errors(pin_pose, reference_pose)
|
||||||
|
max_position = max(max_position, position_error)
|
||||||
|
max_orientation = max(max_orientation, orientation_error)
|
||||||
|
if (
|
||||||
|
position_error >= FK_POSITION_LIMIT_M
|
||||||
|
or orientation_error >= FK_ORIENTATION_LIMIT_RAD
|
||||||
|
):
|
||||||
|
failures += 1
|
||||||
|
self._record_failure(
|
||||||
|
"tool_frame",
|
||||||
|
index,
|
||||||
|
f"{tool_name} residual exceeded limit",
|
||||||
|
q,
|
||||||
|
position_error,
|
||||||
|
orientation_error,
|
||||||
|
tool_name,
|
||||||
|
)
|
||||||
|
self._add_check(
|
||||||
|
"tool_frames",
|
||||||
|
failures == 0,
|
||||||
|
{
|
||||||
|
"tools": sorted(self.tools),
|
||||||
|
"samples_per_tool": len(samples),
|
||||||
|
"max_position_error_m": max_position,
|
||||||
|
"max_orientation_error_rad": max_orientation,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def write_validation_report(
|
||||||
|
output_dir: Path | str,
|
||||||
|
summary: Dict[str, Any],
|
||||||
|
failures: List[Dict[str, Any]],
|
||||||
|
) -> Tuple[Path, Path, Path]:
|
||||||
|
directory = Path(output_dir)
|
||||||
|
directory.mkdir(parents=True, exist_ok=True)
|
||||||
|
json_path = directory / "stage1_summary.json"
|
||||||
|
csv_path = directory / "stage1_failures.csv"
|
||||||
|
markdown_path = directory / "stage1_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")
|
||||||
|
|
||||||
|
fieldnames = [
|
||||||
|
"category",
|
||||||
|
"profile",
|
||||||
|
"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=fieldnames)
|
||||||
|
writer.writeheader()
|
||||||
|
writer.writerows(failures)
|
||||||
|
|
||||||
|
lines = [
|
||||||
|
"# RM75-B Stage 1 Validation",
|
||||||
|
"",
|
||||||
|
f"- Overall: **{'PASS' if summary['passed'] else 'FAIL'}**",
|
||||||
|
f"- Seed: `{summary['seed']}`",
|
||||||
|
f"- RealMan API: `{summary['realman_api_version']}`",
|
||||||
|
f"- Failures recorded: `{summary['failure_count']}`",
|
||||||
|
"",
|
||||||
|
"| Check | Required | Result | Key metrics |",
|
||||||
|
"|---|---:|---:|---|",
|
||||||
|
]
|
||||||
|
for name, check in summary["checks"].items():
|
||||||
|
metrics = {
|
||||||
|
key: value
|
||||||
|
for key, value in check.items()
|
||||||
|
if key not in {"passed", "required"}
|
||||||
|
}
|
||||||
|
metrics_text = json.dumps(metrics, ensure_ascii=True, sort_keys=True)
|
||||||
|
lines.append(
|
||||||
|
f"| `{name}` | {check['required']} | "
|
||||||
|
f"{'PASS' if check['passed'] else 'FAIL'} | `{metrics_text}` |"
|
||||||
|
)
|
||||||
|
markdown_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||||||
|
return json_path, csv_path, markdown_path
|
||||||
64
ik_qp/tests/test_kinematics.py
Normal file
64
ik_qp/tests/test_kinematics.py
Normal file
@ -0,0 +1,64 @@
|
|||||||
|
import numpy as np
|
||||||
|
import pinocchio as pin
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from rm75_ik import (
|
||||||
|
RM75Kinematics,
|
||||||
|
physical_joint_limits,
|
||||||
|
pose_errors,
|
||||||
|
teleop_joint_limits,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_limit_profiles_match_stage_one_contract():
|
||||||
|
physical = physical_joint_limits()
|
||||||
|
teleop = teleop_joint_limits()
|
||||||
|
|
||||||
|
np.testing.assert_allclose(
|
||||||
|
np.rad2deg(physical.upper), [178, 130, 178, 135, 178, 128, 360]
|
||||||
|
)
|
||||||
|
np.testing.assert_allclose(
|
||||||
|
np.rad2deg(teleop.lower), [-150, -30, -170, -130, -175, -125, -179]
|
||||||
|
)
|
||||||
|
np.testing.assert_allclose(
|
||||||
|
np.rad2deg(teleop.upper), [150, 110, 170, 130, 175, 125, 179]
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_zero_configuration_reaches_documented_flange_height():
|
||||||
|
kinematics = RM75Kinematics()
|
||||||
|
pose = kinematics.forward(np.zeros(7))
|
||||||
|
|
||||||
|
np.testing.assert_allclose(pose.translation, [0.0, 0.0, 0.8505], atol=3e-6)
|
||||||
|
np.testing.assert_allclose(pose.rotation, np.eye(3), atol=1e-7)
|
||||||
|
|
||||||
|
|
||||||
|
def test_tool_pose_is_composed_after_flange():
|
||||||
|
kinematics = RM75Kinematics()
|
||||||
|
q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
|
||||||
|
tool = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.16]))
|
||||||
|
|
||||||
|
expected = kinematics.forward(q) * tool
|
||||||
|
actual = kinematics.forward(q, tool)
|
||||||
|
|
||||||
|
assert pose_errors(expected, actual) == pytest.approx((0.0, 0.0), abs=1e-12)
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
"configuration",
|
||||||
|
[
|
||||||
|
np.zeros(6),
|
||||||
|
np.full(7, np.nan),
|
||||||
|
np.deg2rad([179, 0, 0, 0, 0, 0, 0]),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_invalid_configuration_is_rejected(configuration):
|
||||||
|
with pytest.raises(ValueError):
|
||||||
|
RM75Kinematics().forward(configuration)
|
||||||
|
|
||||||
|
|
||||||
|
def test_jacobian_has_expected_shape_and_is_finite():
|
||||||
|
jacobian = RM75Kinematics().jacobian(np.deg2rad([10, 20, -30, 40, 50, -60, 70]))
|
||||||
|
assert jacobian.shape == (6, 7)
|
||||||
|
assert np.all(np.isfinite(jacobian))
|
||||||
|
|
||||||
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
|
||||||
67
ik_qp/tests/test_reference_and_dual.py
Normal file
67
ik_qp/tests/test_reference_and_dual.py
Normal file
@ -0,0 +1,67 @@
|
|||||||
|
import os
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pinocchio as pin
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from rm75_ik import (
|
||||||
|
DualArmAssembly,
|
||||||
|
RM75Kinematics,
|
||||||
|
RealManFkReference,
|
||||||
|
pose_errors,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.fixture(scope="module")
|
||||||
|
def reference():
|
||||||
|
sdk_root = os.environ.get("REALMAN_SDK_ROOT")
|
||||||
|
if not sdk_root:
|
||||||
|
pytest.skip("REALMAN_SDK_ROOT is not set")
|
||||||
|
return RealManFkReference(Path(sdk_root))
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
"q_deg",
|
||||||
|
[
|
||||||
|
[0, 0, 0, 0, 0, 0, 0],
|
||||||
|
[30, -20, 40, 60, -50, 25, 90],
|
||||||
|
[-100, 80, -90, -70, 120, -60, -180],
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_pinocchio_fk_matches_realman_algo(reference, q_deg):
|
||||||
|
q = np.deg2rad(q_deg)
|
||||||
|
position_error, orientation_error = pose_errors(
|
||||||
|
RM75Kinematics().forward(q), reference.forward(q)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert position_error < 1e-4
|
||||||
|
assert orientation_error < np.deg2rad(0.01)
|
||||||
|
|
||||||
|
|
||||||
|
def test_tool_fk_matches_realman_algo(reference):
|
||||||
|
q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
|
||||||
|
tool = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.19]))
|
||||||
|
position_error, orientation_error = pose_errors(
|
||||||
|
RM75Kinematics().forward(q, tool), reference.forward(q, tool)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert position_error < 1e-4
|
||||||
|
assert orientation_error < np.deg2rad(0.01)
|
||||||
|
|
||||||
|
|
||||||
|
def test_dual_arm_mounts_reuse_single_arm_geometry(reference):
|
||||||
|
assembly = DualArmAssembly.from_source_urdf()
|
||||||
|
q = np.deg2rad([20, -10, 30, 40, -20, 15, 80])
|
||||||
|
|
||||||
|
assert assembly.dof == 14
|
||||||
|
assert assembly.mounts.right_visual_origin_delta_m == pytest.approx(0.001, abs=1e-8)
|
||||||
|
for arm, mount in (
|
||||||
|
("left", assembly.mounts.left_base),
|
||||||
|
("right", assembly.mounts.right_base),
|
||||||
|
):
|
||||||
|
local = mount.actInv(assembly.forward(arm, q))
|
||||||
|
errors = pose_errors(local, reference.forward(q))
|
||||||
|
assert errors[0] < 1e-4
|
||||||
|
assert errors[1] < np.deg2rad(0.01)
|
||||||
|
|
||||||
116
ik_qp/tests/test_solver.py
Normal file
116
ik_qp/tests/test_solver.py
Normal file
@ -0,0 +1,116 @@
|
|||||||
|
import numpy as np
|
||||||
|
import pinocchio as pin
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from rm75_ik import IkOptions, IkStatus, RM75IkSolver, RM75Kinematics, pose_errors
|
||||||
|
|
||||||
|
|
||||||
|
def test_near_seed_round_trip_converges():
|
||||||
|
kinematics = RM75Kinematics()
|
||||||
|
solver = RM75IkSolver(kinematics)
|
||||||
|
target_q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
|
||||||
|
seed = target_q + np.deg2rad([2, -1, 2, -1, 1, -2, 3])
|
||||||
|
|
||||||
|
result = solver.solve(kinematics.forward(target_q), seed)
|
||||||
|
|
||||||
|
assert result.status is IkStatus.SUCCESS
|
||||||
|
assert result.q is not None
|
||||||
|
position_error, orientation_error = pose_errors(
|
||||||
|
kinematics.forward(result.q), kinematics.forward(target_q)
|
||||||
|
)
|
||||||
|
assert position_error <= 1e-3
|
||||||
|
assert orientation_error <= np.deg2rad(0.1)
|
||||||
|
|
||||||
|
|
||||||
|
def test_invalid_seed_returns_no_solution():
|
||||||
|
kinematics = RM75Kinematics()
|
||||||
|
result = RM75IkSolver(kinematics).solve(
|
||||||
|
kinematics.forward(np.zeros(7)), np.full(7, np.nan)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert result.status is IkStatus.INVALID_INPUT
|
||||||
|
assert result.q is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_invalid_rotation_returns_no_solution():
|
||||||
|
kinematics = RM75Kinematics()
|
||||||
|
invalid_target = pin.SE3(2.0 * np.eye(3), np.zeros(3))
|
||||||
|
result = RM75IkSolver(kinematics).solve(invalid_target, np.zeros(7))
|
||||||
|
|
||||||
|
assert result.status is IkStatus.INVALID_INPUT
|
||||||
|
assert result.q is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_expired_time_budget_returns_no_solution():
|
||||||
|
kinematics = RM75Kinematics()
|
||||||
|
result = RM75IkSolver(kinematics).solve(
|
||||||
|
kinematics.forward(np.deg2rad([30, 20, -40, 60, 50, -25, 90])),
|
||||||
|
np.zeros(7),
|
||||||
|
IkOptions(time_limit_sec=1e-12),
|
||||||
|
)
|
||||||
|
|
||||||
|
assert result.status is IkStatus.TIME_LIMIT
|
||||||
|
assert result.q is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_joint_limit_mid_regularization_points_toward_range_center():
|
||||||
|
kinematics = RM75Kinematics()
|
||||||
|
solver = RM75IkSolver(kinematics)
|
||||||
|
limits = kinematics.limits
|
||||||
|
midpoint = 0.5 * (limits.lower + limits.upper)
|
||||||
|
joint_range = limits.upper - limits.lower
|
||||||
|
options = IkOptions(
|
||||||
|
posture_weight=0.0,
|
||||||
|
joint_limit_mid_weight=2e-5,
|
||||||
|
joint_motion_weights=(1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2),
|
||||||
|
)
|
||||||
|
|
||||||
|
center_hessian, center_gradient = solver._regularization_terms(
|
||||||
|
midpoint, midpoint, options, damping=0.1
|
||||||
|
)
|
||||||
|
np.testing.assert_allclose(center_gradient, 0.0, atol=1e-15)
|
||||||
|
|
||||||
|
above = midpoint + 0.4 * joint_range
|
||||||
|
below = midpoint - 0.4 * joint_range
|
||||||
|
_, above_gradient = solver._regularization_terms(
|
||||||
|
above, above, options, damping=0.1
|
||||||
|
)
|
||||||
|
_, below_gradient = solver._regularization_terms(
|
||||||
|
below, below, options, damping=0.1
|
||||||
|
)
|
||||||
|
assert np.all(above_gradient > 0.0)
|
||||||
|
np.testing.assert_allclose(below_gradient, -above_gradient, rtol=1e-12)
|
||||||
|
assert center_hessian[6, 6] < center_hessian[0, 0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_per_joint_step_limits_and_hard_position_limits_are_combined():
|
||||||
|
kinematics = RM75Kinematics()
|
||||||
|
solver = RM75IkSolver(kinematics)
|
||||||
|
limits = kinematics.limits
|
||||||
|
midpoint = 0.5 * (limits.lower + limits.upper)
|
||||||
|
configured = np.array([0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10])
|
||||||
|
options = IkOptions(joint_step_limits_rad=tuple(configured))
|
||||||
|
|
||||||
|
lower, upper = solver._step_bounds(midpoint, options)
|
||||||
|
np.testing.assert_allclose(lower, -configured)
|
||||||
|
np.testing.assert_allclose(upper, configured)
|
||||||
|
|
||||||
|
near_upper = midpoint.copy()
|
||||||
|
near_upper[6] = limits.upper[6] - 0.01
|
||||||
|
lower, upper = solver._step_bounds(near_upper, options)
|
||||||
|
assert upper[6] == pytest.approx(0.01)
|
||||||
|
assert lower[6] == pytest.approx(-0.10)
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
"kwargs",
|
||||||
|
[
|
||||||
|
{"joint_limit_mid_weight": -1.0},
|
||||||
|
{"joint_motion_weights": (1.0,) * 6 + (0.0,)},
|
||||||
|
{"joint_step_limits_rad": (0.05,) * 6},
|
||||||
|
{"joint_step_limits_rad": (0.05,) * 6 + (float("nan"),)},
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_invalid_joint_regularization_options_are_rejected(kwargs):
|
||||||
|
with pytest.raises(ValueError):
|
||||||
|
IkOptions(**kwargs)
|
||||||
274
ik_qp/tests/test_stage3_control.py
Normal file
274
ik_qp/tests/test_stage3_control.py
Normal file
@ -0,0 +1,274 @@
|
|||||||
|
import os
|
||||||
|
from dataclasses import replace
|
||||||
|
from pathlib import Path
|
||||||
|
from types import SimpleNamespace
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from rm75_ik import IkResult, IkStatus, 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]],
|
||||||
|
)
|
||||||
|
for profile in profiles.values():
|
||||||
|
assert profile.qp_w_limit_mid == pytest.approx(2e-5)
|
||||||
|
np.testing.assert_allclose(
|
||||||
|
profile.qp_joint_motion_weights,
|
||||||
|
[1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2],
|
||||||
|
)
|
||||||
|
np.testing.assert_allclose(
|
||||||
|
profile.qp_joint_step_limits_rad,
|
||||||
|
[0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10],
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@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_controller_passes_online_joint_regularization_options(
|
||||||
|
profiles, monkeypatch
|
||||||
|
):
|
||||||
|
robot = MujocoRobot(profiles)
|
||||||
|
controller = DualArmQpTeleopController(robot, profiles)
|
||||||
|
captured = {}
|
||||||
|
|
||||||
|
def solve(target, seed, options):
|
||||||
|
del target
|
||||||
|
captured["options"] = options
|
||||||
|
return IkResult(
|
||||||
|
IkStatus.SUCCESS,
|
||||||
|
np.asarray(seed, dtype=float).copy(),
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0,
|
||||||
|
0.0,
|
||||||
|
)
|
||||||
|
|
||||||
|
try:
|
||||||
|
monkeypatch.setattr(controller.solvers["left"], "solve", solve)
|
||||||
|
controller.update_sample(sample("left", True), 0.0)
|
||||||
|
assert controller.step(0.0).state is SafetyState.ACTIVE
|
||||||
|
|
||||||
|
options = captured["options"]
|
||||||
|
assert options.joint_limit_mid_weight == pytest.approx(2e-5)
|
||||||
|
assert options.joint_motion_weights == pytest.approx(
|
||||||
|
(1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2)
|
||||||
|
)
|
||||||
|
assert options.joint_step_limits_rad == pytest.approx(
|
||||||
|
(0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10)
|
||||||
|
)
|
||||||
|
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_reset_restores_initial_state_and_requires_fresh_dual_release(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)
|
||||||
|
assert controller.step(0.0).state is SafetyState.ACTIVE
|
||||||
|
|
||||||
|
for arm in ("left", "right"):
|
||||||
|
controller.update_sample(sample(arm, True, [0, 0.005, 0]), 0.01)
|
||||||
|
assert controller.step(0.01).state is SafetyState.ACTIVE
|
||||||
|
moved = robot.read_joint_positions().positions_rad
|
||||||
|
assert any(
|
||||||
|
np.max(np.abs(moved[arm] - initial[arm])) > 1e-5
|
||||||
|
for arm in ("left", "right")
|
||||||
|
)
|
||||||
|
|
||||||
|
reset = controller.reset_to_initial()
|
||||||
|
assert reset.state is SafetyState.RESETTING
|
||||||
|
restored = robot.read_joint_positions().positions_rad
|
||||||
|
for arm in ("left", "right"):
|
||||||
|
np.testing.assert_array_equal(restored[arm], initial[arm])
|
||||||
|
|
||||||
|
assert controller.step(0.02).state is SafetyState.RESETTING
|
||||||
|
controller.update_sample(sample("left", False), 0.03)
|
||||||
|
controller.update_sample(sample("right", True), 0.03)
|
||||||
|
assert controller.step(0.03).state is SafetyState.RESETTING
|
||||||
|
controller.update_sample(sample("right", False), 0.04)
|
||||||
|
assert controller.step(0.04).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
|
||||||
35
ik_qp/tests/test_validation_io.py
Normal file
35
ik_qp/tests/test_validation_io.py
Normal file
@ -0,0 +1,35 @@
|
|||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from rm75_ik.validation import load_project_tools, write_validation_report
|
||||||
|
|
||||||
|
|
||||||
|
def test_project_tool_config_and_report_output(tmp_path):
|
||||||
|
tools = load_project_tools(
|
||||||
|
Path(__file__).resolve().parents[2]
|
||||||
|
/ "xr_rm_bringup"
|
||||||
|
/ "config"
|
||||||
|
/ "peripherals_rm75.yaml"
|
||||||
|
)
|
||||||
|
assert set(tools) == {"scissor", "omnipic", "minisci"}
|
||||||
|
|
||||||
|
summary = {
|
||||||
|
"passed": True,
|
||||||
|
"seed": 20260629,
|
||||||
|
"realman_api_version": "v1.1.5",
|
||||||
|
"failure_count": 0,
|
||||||
|
"checks": {
|
||||||
|
"example": {
|
||||||
|
"passed": True,
|
||||||
|
"required": True,
|
||||||
|
"samples": 1,
|
||||||
|
}
|
||||||
|
},
|
||||||
|
}
|
||||||
|
json_path, csv_path, markdown_path = write_validation_report(
|
||||||
|
tmp_path, summary, []
|
||||||
|
)
|
||||||
|
|
||||||
|
assert json.loads(json_path.read_text())["passed"] is True
|
||||||
|
assert csv_path.read_text().startswith("category,profile,sample")
|
||||||
|
assert "Overall: **PASS**" in markdown_path.read_text()
|
||||||
@ -57,6 +57,11 @@ left_arm_teleop:
|
|||||||
max_angular_acc: 2.0
|
max_angular_acc: 2.0
|
||||||
joint_max_speed: 180.0
|
joint_max_speed: 180.0
|
||||||
joint_max_acc: 180.0
|
joint_max_acc: 180.0
|
||||||
|
|
||||||
|
# QP 内部正则与单次迭代步长。较小的 motion weight 会让对应关节更积极参与。
|
||||||
|
qp_w_limit_mid: 0.00002
|
||||||
|
qp_joint_motion_weights: [0.3, 0.3, 0.5, 1.0, 1.0, 1.0, 1.0]
|
||||||
|
qp_joint_step_limits_rad: [0.10, 0.08, 0.08, 0.05, 0.05, 0.05, 0.05]
|
||||||
move_to_initial_pose_on_connect: false
|
move_to_initial_pose_on_connect: false
|
||||||
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
|
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
|
||||||
initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
|
initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
|
||||||
@ -112,6 +117,11 @@ right_arm_teleop:
|
|||||||
max_angular_acc: 2.0
|
max_angular_acc: 2.0
|
||||||
joint_max_speed: 180.0
|
joint_max_speed: 180.0
|
||||||
joint_max_acc: 180.0
|
joint_max_acc: 180.0
|
||||||
|
|
||||||
|
# 与左臂采用相同初值，保留独立配置入口便于后续分别调参。
|
||||||
|
qp_w_limit_mid: 0.00002
|
||||||
|
qp_joint_motion_weights: [0.3, 0.3, 0.5, 1.0, 1.0, 1.0, 1.0]
|
||||||
|
qp_joint_step_limits_rad: [0.10, 0.08, 0.08, 0.05, 0.05, 0.05, 0.05]
|
||||||
move_to_initial_pose_on_connect: false
|
move_to_initial_pose_on_connect: false
|
||||||
initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
|
initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
|
||||||
initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
|
initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
|
||||||
|
|||||||
85
xr_rm_bringup/launch/dual_arm_qp_sim.launch.py
Normal file
85
xr_rm_bringup/launch/dual_arm_qp_sim.launch.py
Normal file
@ -0,0 +1,85 @@
|
|||||||
|
"""PICO/XR to dual-arm QP IK with a shared MuJoCo backend."""
|
||||||
|
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||||
|
from launch.substitutions import (
|
||||||
|
EnvironmentVariable,
|
||||||
|
LaunchConfiguration,
|
||||||
|
PathJoinSubstitution,
|
||||||
|
)
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description() -> LaunchDescription:
|
||||||
|
config_dir = PathJoinSubstitution(
|
||||||
|
[FindPackageShare("xr_rm_bringup"), "config"]
|
||||||
|
)
|
||||||
|
return LaunchDescription(
|
||||||
|
[
|
||||||
|
DeclareLaunchArgument("mujoco_gl", default_value="glfw"),
|
||||||
|
SetEnvironmentVariable(
|
||||||
|
"MUJOCO_GL",
|
||||||
|
LaunchConfiguration("mujoco_gl"),
|
||||||
|
),
|
||||||
|
SetEnvironmentVariable(
|
||||||
|
"PYTHONPATH",
|
||||||
|
[
|
||||||
|
PathJoinSubstitution(
|
||||||
|
[
|
||||||
|
EnvironmentVariable("CONDA_PREFIX"),
|
||||||
|
"lib/python3.10/site-packages",
|
||||||
|
]
|
||||||
|
),
|
||||||
|
":",
|
||||||
|
PathJoinSubstitution(
|
||||||
|
[
|
||||||
|
EnvironmentVariable("CONDA_PREFIX"),
|
||||||
|
"lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages",
|
||||||
|
]
|
||||||
|
),
|
||||||
|
":",
|
||||||
|
EnvironmentVariable("PYTHONPATH", default_value=""),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||||||
|
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||||||
|
DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
|
||||||
|
DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
|
||||||
|
DeclareLaunchArgument("show_viewer", default_value="true"),
|
||||||
|
Node(
|
||||||
|
package="xr_rm_input",
|
||||||
|
executable="udp_controller_receiver",
|
||||||
|
name="udp_controller_receiver",
|
||||||
|
output="screen",
|
||||||
|
parameters=[
|
||||||
|
{
|
||||||
|
"udp_host": LaunchConfiguration("udp_host"),
|
||||||
|
"udp_port": LaunchConfiguration("udp_port"),
|
||||||
|
"timer_hz": LaunchConfiguration("udp_timer_hz"),
|
||||||
|
"left_topic": "/xr/left_controller",
|
||||||
|
"right_topic": "/xr/right_controller",
|
||||||
|
}
|
||||||
|
],
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package="xr_rm_teleop",
|
||||||
|
executable="dual_arm_qp_teleop",
|
||||||
|
name="dual_arm_qp_teleop",
|
||||||
|
output="screen",
|
||||||
|
parameters=[
|
||||||
|
{
|
||||||
|
"robot_backend": "mujoco",
|
||||||
|
"control_rate_hz": LaunchConfiguration("control_rate_hz"),
|
||||||
|
"show_viewer": LaunchConfiguration("show_viewer"),
|
||||||
|
"teleop_config_file": PathJoinSubstitution(
|
||||||
|
[config_dir, "dual_arm_rm75.yaml"]
|
||||||
|
),
|
||||||
|
"peripheral_config_file": PathJoinSubstitution(
|
||||||
|
[config_dir, "peripherals_rm75.yaml"]
|
||||||
|
),
|
||||||
|
}
|
||||||
|
],
|
||||||
|
),
|
||||||
|
]
|
||||||
|
)
|
||||||
@ -10,9 +10,13 @@
|
|||||||
<buildtool_depend>ament_python</buildtool_depend>
|
<buildtool_depend>ament_python</buildtool_depend>
|
||||||
|
|
||||||
<exec_depend>geometry_msgs</exec_depend>
|
<exec_depend>geometry_msgs</exec_depend>
|
||||||
|
<exec_depend>ament_index_python</exec_depend>
|
||||||
<exec_depend>rclpy</exec_depend>
|
<exec_depend>rclpy</exec_depend>
|
||||||
|
<exec_depend>rm75_ik</exec_depend>
|
||||||
|
<exec_depend>sensor_msgs</exec_depend>
|
||||||
<exec_depend>python3-yaml</exec_depend>
|
<exec_depend>python3-yaml</exec_depend>
|
||||||
<exec_depend>std_msgs</exec_depend>
|
<exec_depend>std_msgs</exec_depend>
|
||||||
|
<exec_depend>std_srvs</exec_depend>
|
||||||
<exec_depend>xr_rm_interfaces</exec_depend>
|
<exec_depend>xr_rm_interfaces</exec_depend>
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
|||||||
@ -25,6 +25,7 @@ setup(
|
|||||||
entry_points={
|
entry_points={
|
||||||
"console_scripts": [
|
"console_scripts": [
|
||||||
"single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main",
|
"single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main",
|
||||||
|
"dual_arm_qp_teleop = xr_rm_teleop.dual_arm_qp_teleop:main",
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
|
|||||||
254
xr_rm_teleop/xr_rm_teleop/dual_arm_qp_teleop.py
Normal file
254
xr_rm_teleop/xr_rm_teleop/dual_arm_qp_teleop.py
Normal file
@ -0,0 +1,254 @@
|
|||||||
|
"""Dual-arm XR teleoperation using the independent RM75 QP controller."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import os
|
||||||
|
import threading
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from geometry_msgs.msg import PoseStamped
|
||||||
|
from rclpy.executors import ExternalShutdownException
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import JointState
|
||||||
|
from std_msgs.msg import Bool, String
|
||||||
|
from std_srvs.srv import Trigger
|
||||||
|
from xr_rm_interfaces.msg import XrController
|
||||||
|
|
||||||
|
|
||||||
|
class DualArmQpTeleop(Node):
|
||||||
|
def __init__(self) -> None:
|
||||||
|
super().__init__("dual_arm_qp_teleop")
|
||||||
|
bringup_share = Path(get_package_share_directory("xr_rm_bringup"))
|
||||||
|
self.declare_parameter("robot_backend", "mujoco")
|
||||||
|
self.declare_parameter("control_rate_hz", 90.0)
|
||||||
|
self.declare_parameter("left_controller_topic", "/xr/left_controller")
|
||||||
|
self.declare_parameter("right_controller_topic", "/xr/right_controller")
|
||||||
|
self.declare_parameter(
|
||||||
|
"teleop_config_file", str(bringup_share / "config/dual_arm_rm75.yaml")
|
||||||
|
)
|
||||||
|
self.declare_parameter(
|
||||||
|
"peripheral_config_file",
|
||||||
|
str(bringup_share / "config/peripherals_rm75.yaml"),
|
||||||
|
)
|
||||||
|
self.declare_parameter("show_viewer", True)
|
||||||
|
self.declare_parameter("debug_topic_prefix", "/xr_rm/qp")
|
||||||
|
|
||||||
|
backend_name = str(self.get_parameter("robot_backend").value).lower()
|
||||||
|
if backend_name != "mujoco":
|
||||||
|
raise ValueError(
|
||||||
|
"stage 3 implements only robot_backend='mujoco'; "
|
||||||
|
"the RealmanRobot contract is reserved for the next stage"
|
||||||
|
)
|
||||||
|
control_rate_hz = float(self.get_parameter("control_rate_hz").value)
|
||||||
|
if control_rate_hz <= 0.0:
|
||||||
|
raise ValueError("control_rate_hz must be positive")
|
||||||
|
|
||||||
|
from rm75_ik import DualArmQpTeleopController, load_dual_arm_profiles
|
||||||
|
from rm75_ik.mujoco_robot import MujocoRobot
|
||||||
|
|
||||||
|
profiles = load_dual_arm_profiles(
|
||||||
|
self.get_parameter("teleop_config_file").value,
|
||||||
|
self.get_parameter("peripheral_config_file").value,
|
||||||
|
)
|
||||||
|
self._robot = MujocoRobot(profiles)
|
||||||
|
self._controller = DualArmQpTeleopController(
|
||||||
|
self._robot, profiles, control_rate_hz=control_rate_hz
|
||||||
|
)
|
||||||
|
self._closed = False
|
||||||
|
self._viewer_reset_requested = threading.Event()
|
||||||
|
self._viewer_reset_key_codes: set[int] = set()
|
||||||
|
self._show_viewer = bool(self.get_parameter("show_viewer").value)
|
||||||
|
if self._show_viewer:
|
||||||
|
from mujoco.glfw import glfw
|
||||||
|
|
||||||
|
self._viewer_reset_key_codes = {glfw.KEY_R, glfw.KEY_HOME}
|
||||||
|
self._robot.open_viewer(key_callback=self._on_viewer_key)
|
||||||
|
|
||||||
|
prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
|
||||||
|
self._target_publishers = {
|
||||||
|
arm: self.create_publisher(
|
||||||
|
PoseStamped, f"{prefix}/{arm}/target_tcp", 10
|
||||||
|
)
|
||||||
|
for arm in ("left", "right")
|
||||||
|
}
|
||||||
|
self._current_publishers = {
|
||||||
|
arm: self.create_publisher(
|
||||||
|
PoseStamped, f"{prefix}/{arm}/current_tcp", 10
|
||||||
|
)
|
||||||
|
for arm in ("left", "right")
|
||||||
|
}
|
||||||
|
self._fault_publisher = self.create_publisher(Bool, f"{prefix}/fault", 10)
|
||||||
|
self._state_publisher = self.create_publisher(
|
||||||
|
String, f"{prefix}/control_state", 10
|
||||||
|
)
|
||||||
|
self._joint_state_publisher = self.create_publisher(
|
||||||
|
JointState, f"{prefix}/joint_states", 10
|
||||||
|
)
|
||||||
|
self._reset_service = self.create_service(
|
||||||
|
Trigger,
|
||||||
|
f"{prefix}/reset_to_initial",
|
||||||
|
self._on_reset_service,
|
||||||
|
)
|
||||||
|
self._joint_names = [
|
||||||
|
f"{arm}_joint_{joint}"
|
||||||
|
for arm in ("left", "right")
|
||||||
|
for joint in range(1, 8)
|
||||||
|
]
|
||||||
|
self.create_subscription(
|
||||||
|
XrController,
|
||||||
|
str(self.get_parameter("left_controller_topic").value),
|
||||||
|
lambda message: self._on_controller("left", message),
|
||||||
|
10,
|
||||||
|
)
|
||||||
|
self.create_subscription(
|
||||||
|
XrController,
|
||||||
|
str(self.get_parameter("right_controller_topic").value),
|
||||||
|
lambda message: self._on_controller("right", message),
|
||||||
|
10,
|
||||||
|
)
|
||||||
|
self.create_timer(1.0 / control_rate_hz, self._control_tick)
|
||||||
|
|
||||||
|
for arm, diagnostic in self._robot.initial_pose_diagnostics.items():
|
||||||
|
self.get_logger().info(
|
||||||
|
f"{arm} MuJoCo initialized from initial_tcp_pose: "
|
||||||
|
f"position_error={diagnostic.solved_position_error_m:.6f} m, "
|
||||||
|
f"orientation_error={diagnostic.solved_orientation_error_rad:.6f} rad"
|
||||||
|
)
|
||||||
|
self.get_logger().info(
|
||||||
|
"dual-arm QP teleop ready: backend=mujoco, grip controls each arm, "
|
||||||
|
"R/Home resets both arms, any active-arm fault stops both arms"
|
||||||
|
)
|
||||||
|
|
||||||
|
def _now_sec(self) -> float:
|
||||||
|
return self.get_clock().now().nanoseconds * 1e-9
|
||||||
|
|
||||||
|
def _on_controller(self, arm: str, message: XrController) -> None:
|
||||||
|
try:
|
||||||
|
self._controller.update_controller(
|
||||||
|
message, self._now_sec(), expected_arm=arm
|
||||||
|
)
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().error(
|
||||||
|
f"{arm} controller input rejected: {exc}",
|
||||||
|
throttle_duration_sec=1.0,
|
||||||
|
)
|
||||||
|
self._controller.reject_input(str(exc))
|
||||||
|
|
||||||
|
def _on_viewer_key(self, keycode: int) -> None:
|
||||||
|
if keycode in self._viewer_reset_key_codes:
|
||||||
|
self._viewer_reset_requested.set()
|
||||||
|
|
||||||
|
def _request_reset(self, source: str):
|
||||||
|
result = self._controller.reset_to_initial()
|
||||||
|
self.get_logger().info(
|
||||||
|
f"dual-arm MuJoCo reset requested by {source}; "
|
||||||
|
"waiting for fresh left/right grip release"
|
||||||
|
)
|
||||||
|
return result
|
||||||
|
|
||||||
|
def _on_reset_service(self, request, response):
|
||||||
|
del request
|
||||||
|
try:
|
||||||
|
result = self._request_reset("ROS service")
|
||||||
|
except Exception as exc:
|
||||||
|
response.success = False
|
||||||
|
response.message = str(exc)
|
||||||
|
else:
|
||||||
|
response.success = True
|
||||||
|
response.message = result.reason
|
||||||
|
return response
|
||||||
|
|
||||||
|
def _control_tick(self) -> None:
|
||||||
|
if self._viewer_reset_requested.is_set():
|
||||||
|
self._viewer_reset_requested.clear()
|
||||||
|
try:
|
||||||
|
self._request_reset("MuJoCo viewer key")
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().error(f"dual-arm MuJoCo reset failed: {exc}")
|
||||||
|
result = self._controller.step(self._now_sec())
|
||||||
|
stamp = self.get_clock().now().to_msg()
|
||||||
|
fault = Bool()
|
||||||
|
fault.data = result.state.value == "fault"
|
||||||
|
self._fault_publisher.publish(fault)
|
||||||
|
state_message = String()
|
||||||
|
state_message.data = result.state.value
|
||||||
|
self._state_publisher.publish(state_message)
|
||||||
|
if fault.data:
|
||||||
|
self.get_logger().error(
|
||||||
|
f"dual-arm QP controller stopped: {result.reason}",
|
||||||
|
throttle_duration_sec=1.0,
|
||||||
|
)
|
||||||
|
if result.targets_tcp:
|
||||||
|
for arm, target in result.targets_tcp.items():
|
||||||
|
self._target_publishers[arm].publish(self._pose_message(target))
|
||||||
|
for arm in ("left", "right"):
|
||||||
|
self._current_publishers[arm].publish(
|
||||||
|
self._pose_message(self._robot.get_tcp_pose(arm))
|
||||||
|
)
|
||||||
|
joint_state = self._robot.read_joint_positions().positions_rad
|
||||||
|
joint_message = JointState()
|
||||||
|
joint_message.header.stamp = stamp
|
||||||
|
joint_message.header.frame_id = "dual_rm75"
|
||||||
|
joint_message.name = self._joint_names
|
||||||
|
joint_message.position = [
|
||||||
|
float(value)
|
||||||
|
for arm in ("left", "right")
|
||||||
|
for value in joint_state[arm]
|
||||||
|
]
|
||||||
|
self._joint_state_publisher.publish(joint_message)
|
||||||
|
if self._show_viewer and not self._robot.sync_viewer():
|
||||||
|
self.get_logger().info("MuJoCo viewer closed; shutting down QP teleop")
|
||||||
|
self._close_control()
|
||||||
|
if rclpy.ok():
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
def _pose_message(self, pose) -> PoseStamped:
|
||||||
|
import pinocchio as pin
|
||||||
|
|
||||||
|
quaternion = pin.Quaternion(pose.rotation)
|
||||||
|
message = PoseStamped()
|
||||||
|
message.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
message.header.frame_id = "rm_base"
|
||||||
|
message.pose.position.x = float(pose.translation[0])
|
||||||
|
message.pose.position.y = float(pose.translation[1])
|
||||||
|
message.pose.position.z = float(pose.translation[2])
|
||||||
|
message.pose.orientation.x = float(quaternion.x)
|
||||||
|
message.pose.orientation.y = float(quaternion.y)
|
||||||
|
message.pose.orientation.z = float(quaternion.z)
|
||||||
|
message.pose.orientation.w = float(quaternion.w)
|
||||||
|
return message
|
||||||
|
|
||||||
|
def _close_control(self) -> None:
|
||||||
|
if self._closed:
|
||||||
|
return
|
||||||
|
self._controller.close()
|
||||||
|
self._closed = True
|
||||||
|
|
||||||
|
def destroy_node(self):
|
||||||
|
self._close_control()
|
||||||
|
return super().destroy_node()
|
||||||
|
|
||||||
|
|
||||||
|
def main(args=None) -> None:
|
||||||
|
os.environ.setdefault("MUJOCO_GL", "glfw")
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = None
|
||||||
|
try:
|
||||||
|
node = DualArmQpTeleop()
|
||||||
|
rclpy.spin(node)
|
||||||
|
except (KeyboardInterrupt, ExternalShutdownException):
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
if node is not None:
|
||||||
|
try:
|
||||||
|
node.destroy_node()
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
if rclpy.ok():
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
Reference in New Issue
Block a user