Add IK types, validation, and tests for RM75 kinematics
This commit is contained in:
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
|
||||||
|
|||||||
106
AGENTS.md
Normal file
106
AGENTS.md
Normal file
@ -0,0 +1,106 @@
|
|||||||
|
# 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` 用于记录智能体工作流程和安全规则。
|
||||||
|
- 与具体实现相关的协议、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.
|
|
||||||
@ -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/
|
||||||
|
|||||||
105
ik_qp/README.md
105
ik_qp/README.md
@ -1,36 +1,89 @@
|
|||||||
### This repo is for inverse kinematics and verification
|
# RM75-B 第一阶段运动学与 QP IK
|
||||||
|
|
||||||
In this branch, the qp-based inverse kinematics method is modified as a python class. The user can call it as in `main.py`
|
本目录是一个独立的离线 Python 包，用于验证 RM75-B 的运动学与逆运动学。它不接入
|
||||||
|
ROS 2 遥操作控制链路，也不会建立机器人连接。
|
||||||
|
|
||||||
Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
|
第一阶段包含：
|
||||||
|
|
||||||
Verification is done with Mujoco simulation.
|
- 由 Pinocchio 加载的标准单臂 RM75-B URDF。
|
||||||
|
- 基于 SE(3) 的正运动学、局部坐标雅可比矩阵和位姿残差。
|
||||||
|
- 支持热启动的 OSQP 微分逆运动学。
|
||||||
|
- 作为独立参考的 RealMan API2 Algo FK。
|
||||||
|
- 物理关节限位配置和项目专用的遥操作关节限位配置。
|
||||||
|
- 由两份标准单臂模型组成的双臂装配模型。
|
||||||
|
- 可生成 JSON、CSV 和 Markdown 报告的确定性验证流程。
|
||||||
|
|
||||||
Key specifications:
|
MuJoCo、MJCF、碰撞规避和真实机器人控制明确不在本阶段范围内。
|
||||||
1. Time consumption.
|
|
||||||
2. Success rate
|
|
||||||
3. Minial joint variation.
|
|
||||||
|
|
||||||
Next:\
|
## 环境
|
||||||
Comparison with Realman official IK method.
|
|
||||||
Embedded with current demo.
|
|
||||||
|
|
||||||
|
经过验证的环境定义在 `environment.yml` 中：
|
||||||
|
|
||||||
### Comparison (05June2026):
|
```bash
|
||||||
|
cd /home/robot/WS_xr/src/ik_qp
|
||||||
- With current dual arm joint limit,
|
conda env update -f environment.yml
|
||||||
|
conda run -n qp python -m pip install -e . --no-deps
|
||||||
```
|
```
|
||||||
ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
|
|
||||||
lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
|
|
||||||
```
|
|
||||||
the success rates for **qp-based ik** and **realman Algo ik** are **63%** and **46%**.\
|
|
||||||
At least one solver works out the ik, rate = **74%**.
|
|
||||||
|
|
||||||
- With realman-75 physical joint limit,
|
RealMan API2 SDK 是外部二进制依赖，不会复制到本包中。请为验证程序指定包含
|
||||||
```
|
`Robotic_Arm/` 的目录：
|
||||||
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
|
|
||||||
lb = -ub
|
|
||||||
```
|
|
||||||
the success rates for **qp-based ik** and **realman Algo ik** are **76%** and **51%**.\
|
|
||||||
At least one solver works out the ik, rate = **84%**.
|
|
||||||
|
|
||||||
|
```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
|
||||||
|
```
|
||||||
|
|
||||||
|
对于任何失败状态，`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)。
|
||||||
|
|
||||||
|
## 模型说明
|
||||||
|
|
||||||
|
单臂 URDF 是 RM75-B 运动链几何参数的唯一来源。导入的双臂 URDF 仅用于提供左右
|
||||||
|
安装变换；求解器不使用其中的镜像关节限位和固化的关节零位偏移。
|
||||||
|
|
||||||
|
导入的双臂 URDF 中，右侧基座的视觉原点与运动学原点相差约 1 mm。第一阶段采用
|
||||||
|
第一关节的运动学原点，并叠加文档规定的 240.5 mm 基座至第一关节偏移。
|
||||||
|
|||||||
62
ik_qp/STAGE1_VALIDATION.md
Normal file
62
ik_qp/STAGE1_VALIDATION.md
Normal file
@ -0,0 +1,62 @@
|
|||||||
|
# RM75-B Stage-1 Validation Record
|
||||||
|
|
||||||
|
Date: 2026-06-29
|
||||||
|
Random seed: `20260629`
|
||||||
|
RealMan API2 C API: `v1.1.5`
|
||||||
|
|
||||||
|
## Acceptance Result
|
||||||
|
|
||||||
|
The complete strict benchmark passed every required check with zero recorded
|
||||||
|
failure samples.
|
||||||
|
|
||||||
|
| Check | Samples | Result |
|
||||||
|
|---|---:|---:|
|
||||||
|
| Physical-limit FK | 10,000 | PASS |
|
||||||
|
| Teleop-limit FK | 10,000 | PASS |
|
||||||
|
| Algo finite-difference Jacobian | 200 | PASS |
|
||||||
|
| Physical-limit near-seed IK | 1,000 / 1,000 | PASS |
|
||||||
|
| Teleop-limit near-seed IK | 1,000 / 1,000 | PASS |
|
||||||
|
| Continuous IK | 10,000 / 10,000 | PASS |
|
||||||
|
| Eight-seed global recovery | 200 / 200 | PASS |
|
||||||
|
| Documented singularity families | 12 | PASS |
|
||||||
|
| Dual-arm assembly FK | 100 per arm | PASS |
|
||||||
|
| Project tool-frame FK | 100 per tool | PASS |
|
||||||
|
|
||||||
|
Key measurements:
|
||||||
|
|
||||||
|
- Maximum physical-limit FK error: `0.003868 mm`, `0.001027 deg`.
|
||||||
|
- Maximum teleop-limit FK error: `0.003681 mm`, `0.000957 deg`.
|
||||||
|
- Maximum Jacobian relative/absolute error: `6.97e-5` / `1.70e-4`.
|
||||||
|
- Near-seed IK P99/max time: `2.44 ms` / `7.43 ms`.
|
||||||
|
- Maximum continuous joint step: `0.003216 rad` (`0.184 deg`).
|
||||||
|
- Random single-seed IK success rate: `74%` (diagnostic only).
|
||||||
|
- Eight-seed recovery success rate: `100%`.
|
||||||
|
- Right dual-arm visual/kinematic origin difference: `1.0000004 mm`.
|
||||||
|
|
||||||
|
## Error Definitions
|
||||||
|
|
||||||
|
Position error:
|
||||||
|
|
||||||
|
```text
|
||||||
|
||p_result - p_target||
|
||||||
|
```
|
||||||
|
|
||||||
|
Orientation error:
|
||||||
|
|
||||||
|
```text
|
||||||
|
||log(R_result^T R_target)||
|
||||||
|
```
|
||||||
|
|
||||||
|
IK success is accepted only after applying RealMan Algo FK to the returned joint
|
||||||
|
configuration. Pinocchio does not validate its own IK result.
|
||||||
|
|
||||||
|
The validator asks the numerical solver to converge to `0.9 mm / 0.09 deg`, then
|
||||||
|
applies the independent acceptance limits `1 mm / 0.1 deg`. This guard band
|
||||||
|
prevents boundary false positives caused by the small measured model difference.
|
||||||
|
|
||||||
|
## Boundaries
|
||||||
|
|
||||||
|
This result validates geometry, FK, local Jacobians, numerical IK and fixed tool
|
||||||
|
or mounting transforms. It does not validate dynamics, self-collision,
|
||||||
|
environment collision, torque limits, communication latency or hardware safety.
|
||||||
|
|
||||||
14
ik_qp/environment.yml
Normal file
14
ik_qp/environment.yml
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
name: qp
|
||||||
|
channels:
|
||||||
|
- conda-forge
|
||||||
|
dependencies:
|
||||||
|
- python=3.10.20
|
||||||
|
- numpy=1.23.5
|
||||||
|
- scipy=1.10.1
|
||||||
|
- pinocchio=2.6.20
|
||||||
|
- pip
|
||||||
|
- pip:
|
||||||
|
- osqp==0.6.2.post8
|
||||||
|
- PyYAML==6.0.3
|
||||||
|
- pytest==7.4.4
|
||||||
|
|
||||||
@ -1,128 +1,8 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Compatibility entry point for the stage-1 validation command."""
|
||||||
|
|
||||||
|
from rm75_ik.cli import main
|
||||||
# conda activate coppeliasim
|
|
||||||
# env fix, in terminal: fix_robotics_env.sh
|
|
||||||
|
|
||||||
from rm75_kine_qp import KinematicsSolver as kine_qp
|
|
||||||
from rm75_kine_rm import rm75_kine_api as kine_rm
|
|
||||||
from rm75_mjc import MuJoCoPositionController
|
|
||||||
from Robotic_Arm.rm_robot_interface import *
|
|
||||||
|
|
||||||
import time
|
|
||||||
from math import radians, degrees, pi, cos, sin
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
# pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
|
|
||||||
# load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
|
|
||||||
tools_in_ee = {
|
|
||||||
'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
|
||||||
'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
|
||||||
'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
|
||||||
'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
|
|
||||||
}
|
|
||||||
|
|
||||||
# joint limit
|
|
||||||
ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
|
|
||||||
lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
|
|
||||||
|
|
||||||
|
|
||||||
# ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
|
|
||||||
# lb = -ub
|
|
||||||
|
|
||||||
tool_name = "scissor"
|
|
||||||
|
|
||||||
def main():
|
|
||||||
"""Demonstrate pure position control"""
|
|
||||||
|
|
||||||
# Create controller
|
|
||||||
robot_mjk = MuJoCoPositionController()
|
|
||||||
|
|
||||||
|
|
||||||
# ----------- rm75 qp based kine ------------
|
|
||||||
robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75')
|
|
||||||
robot_kine_qp.add_tool_frames(tools_in_ee)
|
|
||||||
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
|
||||||
|
|
||||||
# ---------- rm75 official algorithm -----------
|
|
||||||
robot_kine_rm = kine_rm()
|
|
||||||
robot_kine_rm.add_tool_frames(tools_in_ee)
|
|
||||||
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# -------------- for comparison ----------------
|
|
||||||
print(f'in the comparison part')
|
|
||||||
|
|
||||||
if True:
|
|
||||||
|
|
||||||
result = np.array([[0,0],[0,0]], dtype=np.int32) # to collect ik result qp_fk, qp_ik, rm_fk, rm_ik
|
|
||||||
|
|
||||||
solve_sum = 0
|
|
||||||
|
|
||||||
for i in range(10):
|
|
||||||
print(f'\n-------------- in i = {i} ----------------')
|
|
||||||
joint_rand = np.random.uniform(ub, lb)
|
|
||||||
print(f'the predefined joints are {joint_rand}')
|
|
||||||
|
|
||||||
# -------------- fk ------------------
|
|
||||||
fk_qp_p1 = robot_kine_qp.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
|
|
||||||
|
|
||||||
fk_rm_p1 = robot_kine_rm.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
|
|
||||||
|
|
||||||
d_fk = cal_pose_deviation(pose1=fk_rm_p1, pose2=fk_qp_p1)
|
|
||||||
print(f'fk_qp_p1 = {fk_qp_p1}, fk_rm_p1 = {fk_rm_p1}, d_fk = {d_fk}\n')
|
|
||||||
|
|
||||||
|
|
||||||
# ----------- ik ----------------
|
|
||||||
t_p = fk_rm_p1
|
|
||||||
joint_rand_init = np.random.uniform(ub, lb)
|
|
||||||
print(f'the guess is {joint_rand_init}')
|
|
||||||
|
|
||||||
ret_qp, q = robot_kine_qp.inverse_kinematics( target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
|
|
||||||
|
|
||||||
if ret_qp == 0:
|
|
||||||
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
|
|
||||||
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
|
|
||||||
print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
|
|
||||||
if d_p_ik < 0.01:
|
|
||||||
result[0][1] += 1
|
|
||||||
|
|
||||||
robot_mjk.send_command(q)
|
|
||||||
robot_mjk.wait_until_reached()
|
|
||||||
robot_mjk.print_state()
|
|
||||||
else:
|
|
||||||
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
|
|
||||||
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
|
|
||||||
print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
|
|
||||||
|
|
||||||
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
|
|
||||||
if ret_rm == 0:
|
|
||||||
fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
|
|
||||||
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
|
|
||||||
print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
|
|
||||||
if d_p_ik < 0.01:
|
|
||||||
result[1][1] += 1
|
|
||||||
else:
|
|
||||||
print(f'== fail in the rm ik, ret = {ret_rm}, q = {q}')
|
|
||||||
|
|
||||||
if ret_qp == 0 or ret_rm == 0:
|
|
||||||
solve_sum += 1
|
|
||||||
|
|
||||||
print(f'results with qp and rm for ik are {result}')
|
|
||||||
print(f'solve_sum is {solve_sum}')
|
|
||||||
|
|
||||||
|
|
||||||
def cal_pose_deviation(pose1, pose2):
|
|
||||||
d_fk_p1 = np.array(pose1) - np.array(pose2)
|
|
||||||
for j in [3, 4, 5]:
|
|
||||||
while d_fk_p1[j] > pi:
|
|
||||||
d_fk_p1[j] -= 2 * pi
|
|
||||||
while d_fk_p1[j] < -pi:
|
|
||||||
d_fk_p1[j] += 2 * pi
|
|
||||||
d_fk = np.linalg.norm(d_fk_p1)
|
|
||||||
return d_fk
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
raise SystemExit(main())
|
||||||
|
|||||||
@ -1,809 +1,104 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
import sys
|
"""Compatibility adapter for the original experimental import path.
|
||||||
import os
|
|
||||||
|
New code should import RM75Kinematics and RM75IkSolver from ``rm75_ik``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from math import pi
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
import pinocchio as pin
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import osqp
|
import pinocchio as pin
|
||||||
from scipy import sparse
|
|
||||||
from math import radians, degrees, pi, cos, sin
|
from rm75_ik import IkOptions, JointLimits, RM75IkSolver, RM75Kinematics
|
||||||
import time
|
|
||||||
import threading
|
|
||||||
|
|
||||||
|
|
||||||
|
class KinematicsSolver:
|
||||||
|
def __init__(self, urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir=None):
|
||||||
|
del mesh_dir
|
||||||
|
selected_path = Path(urdf_path)
|
||||||
|
if not selected_path.is_file() and not selected_path.is_absolute():
|
||||||
|
selected_path = Path(__file__).resolve().parent / selected_path
|
||||||
|
self._urdf_path = selected_path
|
||||||
|
self._limits = None
|
||||||
|
self._tools = {}
|
||||||
|
self._rebuild()
|
||||||
|
|
||||||
class KinematicsSolver():
|
def _rebuild(self):
|
||||||
def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
|
self.kinematics = RM75Kinematics(self._urdf_path, self._limits)
|
||||||
"""
|
self.solver = RM75IkSolver(self.kinematics)
|
||||||
for realman 75b
|
self.model = self.kinematics.model
|
||||||
Initialize robotic arm kinematics using Pinocchio (ROS2 version).
|
self.data = self.kinematics.data
|
||||||
unit: m, rad
|
|
||||||
"""
|
|
||||||
print(f' ------------ the qp based kinematic initialising -----------')
|
|
||||||
self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
|
|
||||||
|
|
||||||
|
def add_tool_frames(self, frames):
|
||||||
|
for name, attributes in frames.items():
|
||||||
|
pose = np.asarray(attributes[0], dtype=float)
|
||||||
|
if pose.shape != (7,):
|
||||||
|
raise ValueError(f"tool {name!r} pose must have seven values")
|
||||||
|
quaternion = pin.Quaternion(pose[6], pose[3], pose[4], pose[5])
|
||||||
|
quaternion.normalize()
|
||||||
|
self._tools[name] = pin.SE3(quaternion.matrix(), pose[:3])
|
||||||
|
|
||||||
|
def cfg_j_limit(self, min_j=None, max_j=None, rad_flag=True):
|
||||||
self.cfg_j_limit()
|
|
||||||
|
|
||||||
# ---------- 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])
|
|
||||||
|
|
||||||
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:
|
if min_j is None:
|
||||||
min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159]
|
min_j = [-pi, -2.2689, -pi, -2.3562, -pi, -2.234, -2 * pi]
|
||||||
if max_j is None:
|
if max_j is None:
|
||||||
max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
|
max_j = [pi, 2.2689, pi, 2.3562, pi, 2.234, 2 * pi]
|
||||||
if rad_flag:
|
lower = np.asarray(min_j, dtype=float)
|
||||||
for i in range(7):
|
upper = np.asarray(max_j, dtype=float)
|
||||||
self.model.lowerPositionLimit[i] = min_j[i]
|
if not rad_flag:
|
||||||
self.model.upperPositionLimit[i] = max_j[i]
|
lower = np.deg2rad(lower)
|
||||||
else:
|
upper = np.deg2rad(upper)
|
||||||
for i in range(7):
|
self._limits = JointLimits("legacy", lower, upper)
|
||||||
self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi
|
self._rebuild()
|
||||||
self.model.upperPositionLimit[i] = max_j[i] / 180 * pi
|
|
||||||
|
|
||||||
def forward_kinematics(self, joint_angles, tool="omnipic"):
|
def _tool(self, name):
|
||||||
"""
|
try:
|
||||||
Compute forward kinematics.
|
return self._tools[name]
|
||||||
|
except KeyError as exc:
|
||||||
|
raise ValueError(f"unknown tool frame: {name!r}") from exc
|
||||||
|
|
||||||
Args:
|
def forward_kinematics(self, joint_angles, tool="no_tool"):
|
||||||
joint_angles: List or array of 7 joint angles (radians)
|
pose = self.kinematics.forward(np.asarray(joint_angles), self._tool(tool))
|
||||||
tool: Name of frame to compute
|
return np.concatenate(
|
||||||
|
[pose.translation.copy(), pin.rpy.matrixToRpy(pose.rotation)]
|
||||||
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)
|
def inverse_kinematics(
|
||||||
pin.updateFramePlacements(self.model, self.data)
|
self,
|
||||||
|
target_position,
|
||||||
current_placement = self.data.oMf[ee_frame_id]
|
target_rpy=None,
|
||||||
|
target_quat=None,
|
||||||
error_SE3 = current_placement.actInv(target_placement)
|
initial_guess=None,
|
||||||
error_vec = pin.log(error_SE3).vector
|
max_iter=500,
|
||||||
|
tolerance=1e-3,
|
||||||
# print("\n initial error =", np.linalg.norm(error_vec))
|
debug=False,
|
||||||
# print(error_vec)
|
tool="no_tool",
|
||||||
|
|
||||||
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_posture = 0.0001
|
|
||||||
|
|
||||||
J_eff = pin.Jlog6(error_SE3) @ J #J #
|
|
||||||
|
|
||||||
H = J_eff.T @ self.W @ J_eff
|
|
||||||
|
|
||||||
|
|
||||||
# H = J.T @ self.W @ J
|
|
||||||
H += damping * damping * np.eye(7)
|
|
||||||
H += w_posture * np.eye(7)
|
|
||||||
|
|
||||||
H_triu = sparse.triu(H).tocsc()
|
|
||||||
|
|
||||||
g = -J_eff.T @ self.W @ error_vec
|
|
||||||
g += w_posture * (q[:7] - q_ref[:7])
|
|
||||||
# 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, #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
|
|
||||||
):
|
):
|
||||||
|
del debug
|
||||||
q = pin.neutral(self.model)
|
if target_quat is not None:
|
||||||
|
values = np.asarray(target_quat, dtype=float)
|
||||||
all_active_joints = self.get_active_joints_from_frame(frame_names)
|
quaternion = pin.Quaternion(values[3], values[0], values[1], values[2])
|
||||||
|
rotation = quaternion.matrix()
|
||||||
for i in range(7):
|
elif target_rpy is not None:
|
||||||
q[i] = joint_angles[i]
|
rotation = pin.rpy.rpyToMatrix(*target_rpy)
|
||||||
|
|
||||||
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:
|
else:
|
||||||
print("✗ IK failed to find a solution!")
|
rotation = np.eye(3)
|
||||||
|
tool_pose = self._tool(tool)
|
||||||
# Test 3: Jacobian
|
target_tool = pin.SE3(rotation, np.asarray(target_position, dtype=float))
|
||||||
print("\n3. Jacobian Matrix")
|
target_flange = target_tool * tool_pose.inverse()
|
||||||
print("-" * 40)
|
seed = np.zeros(7) if initial_guess is None else np.asarray(initial_guess, dtype=float)
|
||||||
|
result = self.solver.solve(
|
||||||
J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name)
|
target_flange,
|
||||||
print(f"Jacobian shape: {J.shape}")
|
seed,
|
||||||
for i in range(min(3, J.shape[0])):
|
IkOptions(
|
||||||
row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)])
|
position_tolerance_m=tolerance,
|
||||||
print(f" Row {i + 1}: {row_str}")
|
orientation_tolerance_rad=tolerance,
|
||||||
|
max_iterations=max_iter,
|
||||||
# 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
|
|
||||||
)
|
)
|
||||||
|
return (0, result.q.tolist()) if result.success else (-1, [])
|
||||||
|
|
||||||
if trajectory:
|
def compute_jacobian(self, joint_angles, tool="no_tool"):
|
||||||
print(f"\n✓ Generated {len(trajectory)} waypoints")
|
del tool
|
||||||
|
return self.kinematics.jacobian(np.asarray(joint_angles, dtype=float))
|
||||||
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)
|
|
||||||
|
|||||||
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>
|
||||||
35
ik_qp/pyproject.toml
Normal file
35
ik_qp/pyproject.toml
Normal file
@ -0,0 +1,35 @@
|
|||||||
|
[build-system]
|
||||||
|
requires = ["setuptools>=68", "wheel"]
|
||||||
|
build-backend = "setuptools.build_meta"
|
||||||
|
|
||||||
|
[project]
|
||||||
|
name = "rm75-ik-qp"
|
||||||
|
version = "0.1.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",
|
||||||
|
]
|
||||||
|
|
||||||
|
[project.optional-dependencies]
|
||||||
|
test = ["pytest==7.4.4"]
|
||||||
|
|
||||||
|
[project.scripts]
|
||||||
|
rm75-stage1-validate = "rm75_ik.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"]}
|
||||||
|
|
||||||
|
[tool.setuptools.packages.find]
|
||||||
|
where = ["src"]
|
||||||
|
|
||||||
|
[tool.pytest.ini_options]
|
||||||
|
testpaths = ["tests"]
|
||||||
|
addopts = "-ra"
|
||||||
|
|
||||||
34
ik_qp/src/rm75_ik/__init__.py
Normal file
34
ik_qp/src/rm75_ik/__init__.py
Normal file
@ -0,0 +1,34 @@
|
|||||||
|
from .dual_arm import DualArmAssembly, DualArmMounts, load_dual_arm_mounts
|
||||||
|
from .kinematics import RM75Kinematics, default_urdf_path, pose_errors, validate_se3
|
||||||
|
from .realman_reference import RealManFkReference
|
||||||
|
from .solver import RM75IkSolver, deterministic_recovery_seeds
|
||||||
|
from .types import (
|
||||||
|
IkOptions,
|
||||||
|
IkResult,
|
||||||
|
IkStatus,
|
||||||
|
JointLimits,
|
||||||
|
joint_limit_profile,
|
||||||
|
physical_joint_limits,
|
||||||
|
teleop_joint_limits,
|
||||||
|
)
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
"DualArmAssembly",
|
||||||
|
"DualArmMounts",
|
||||||
|
"IkOptions",
|
||||||
|
"IkResult",
|
||||||
|
"IkStatus",
|
||||||
|
"JointLimits",
|
||||||
|
"RM75IkSolver",
|
||||||
|
"RM75Kinematics",
|
||||||
|
"RealManFkReference",
|
||||||
|
"default_urdf_path",
|
||||||
|
"deterministic_recovery_seeds",
|
||||||
|
"joint_limit_profile",
|
||||||
|
"load_dual_arm_mounts",
|
||||||
|
"physical_joint_limits",
|
||||||
|
"pose_errors",
|
||||||
|
"teleop_joint_limits",
|
||||||
|
"validate_se3",
|
||||||
|
]
|
||||||
|
|
||||||
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())
|
||||||
|
|
||||||
132
ik_qp/src/rm75_ik/dual_arm.py
Normal file
132
ik_qp/src/rm75_ik/dual_arm.py
Normal file
@ -0,0 +1,132 @@
|
|||||||
|
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_path = (
|
||||||
|
Path(sysconfig.get_path("data"))
|
||||||
|
/ "share"
|
||||||
|
/ "rm75_ik"
|
||||||
|
/ "models"
|
||||||
|
/ "dual_arm_mujoco_fixed.urdf"
|
||||||
|
)
|
||||||
|
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'")
|
||||||
|
|
||||||
124
ik_qp/src/rm75_ik/kinematics.py
Normal file
124
ik_qp/src/rm75_ik/kinematics.py
Normal file
@ -0,0 +1,124 @@
|
|||||||
|
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_path = (
|
||||||
|
Path(sysconfig.get_path("data"))
|
||||||
|
/ "share"
|
||||||
|
/ "rm75_ik"
|
||||||
|
/ "models"
|
||||||
|
/ "RM75-B.urdf"
|
||||||
|
)
|
||||||
|
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()
|
||||||
|
|
||||||
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))
|
||||||
|
|
||||||
252
ik_qp/src/rm75_ik/solver.py
Normal file
252
ik_qp/src/rm75_ik/solver.py
Normal file
@ -0,0 +1,252 @@
|
|||||||
|
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
|
||||||
|
|
||||||
|
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 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
|
||||||
|
hessian += (
|
||||||
|
damping * damping + options.posture_weight
|
||||||
|
) * np.eye(self._n)
|
||||||
|
gradient = -effective_jacobian.T @ weights @ error_vector
|
||||||
|
gradient += options.posture_weight * (q - q_reference)
|
||||||
|
|
||||||
|
lower = np.maximum(
|
||||||
|
-options.trust_region_rad,
|
||||||
|
self.kinematics.limits.lower - q,
|
||||||
|
)
|
||||||
|
upper = np.minimum(
|
||||||
|
options.trust_region_rad,
|
||||||
|
self.kinematics.limits.upper - q,
|
||||||
|
)
|
||||||
|
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
|
||||||
|
|
||||||
133
ik_qp/src/rm75_ik/types.py
Normal file
133
ik_qp/src/rm75_ik/types.py
Normal file
@ -0,0 +1,133 @@
|
|||||||
|
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, -30.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
|
||||||
|
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 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))
|
||||||
|
|
||||||
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)
|
||||||
|
|
||||||
53
ik_qp/tests/test_solver.py
Normal file
53
ik_qp/tests/test_solver.py
Normal file
@ -0,0 +1,53 @@
|
|||||||
|
import numpy as np
|
||||||
|
import pinocchio as pin
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
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()
|
||||||
Reference in New Issue
Block a user