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/
|
||||
.mypy_cache/
|
||||
*.egg-info/
|
||||
ik_qp/artifacts/
|
||||
*.log
|
||||
qtcreator-*
|
||||
*.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
|
||||
src/
|
||||
├── README.md # 项目主文档
|
||||
├── CODEX.md # Codex/Claude Code 项目工作流和安全规则
|
||||
├── AGENTS.md # Codex/Claude Code 项目工作流和安全规则
|
||||
├── docs/
|
||||
│ └── pico_udp_sender_ubuntu22_setup.md # Ubuntu 22.04 下 PICO UDP Sender 配置教程
|
||||
├── 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:
|
||||
1. Time consumption.
|
||||
2. Success rate
|
||||
3. Minial joint variation.
|
||||
MuJoCo、MJCF、碰撞规避和真实机器人控制明确不在本阶段范围内。
|
||||
|
||||
Next:\
|
||||
Comparison with Realman official IK method.
|
||||
Embedded with current demo.
|
||||
## 环境
|
||||
|
||||
经过验证的环境定义在 `environment.yml` 中：
|
||||
|
||||
### Comparison (05June2026):
|
||||
|
||||
- With current dual arm joint limit,
|
||||
```bash
|
||||
cd /home/robot/WS_xr/src/ik_qp
|
||||
conda env update -f environment.yml
|
||||
conda run -n qp python -m pip install -e . --no-deps
|
||||
```
|
||||
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,
|
||||
```
|
||||
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%**.
|
||||
RealMan API2 SDK 是外部二进制依赖，不会复制到本包中。请为验证程序指定包含
|
||||
`Robotic_Arm/` 的目录：
|
||||
|
||||
```bash
|
||||
export REALMAN_SDK_ROOT=/path/to/RM_API2/Python
|
||||
```
|
||||
|
||||
## 公共 API
|
||||
|
||||
```python
|
||||
from rm75_ik import (
|
||||
DualArmAssembly,
|
||||
RM75IkSolver,
|
||||
RM75Kinematics,
|
||||
RealManFkReference,
|
||||
teleop_joint_limits,
|
||||
)
|
||||
|
||||
kinematics = RM75Kinematics(limits=teleop_joint_limits())
|
||||
solver = RM75IkSolver(kinematics)
|
||||
target = kinematics.forward(target_q_rad)
|
||||
result = solver.solve(target, current_q_rad)
|
||||
|
||||
if result.success:
|
||||
solution_q_rad = result.q
|
||||
```
|
||||
|
||||
对于任何失败状态，`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."""
|
||||
|
||||
|
||||
# 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
|
||||
|
||||
from rm75_ik.cli import main
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
raise SystemExit(main())
|
||||
|
||||
@ -1,809 +1,104 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import os
|
||||
"""Compatibility adapter for the original experimental import path.
|
||||
|
||||
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 osqp
|
||||
from scipy import sparse
|
||||
from math import radians, degrees, pi, cos, sin
|
||||
import time
|
||||
import threading
|
||||
import pinocchio as pin
|
||||
|
||||
from rm75_ik import IkOptions, JointLimits, RM75IkSolver, RM75Kinematics
|
||||
|
||||
|
||||
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 __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
|
||||
"""
|
||||
for realman 75b
|
||||
Initialize robotic arm kinematics using Pinocchio (ROS2 version).
|
||||
unit: m, rad
|
||||
"""
|
||||
print(f' ------------ the qp based kinematic initialising -----------')
|
||||
self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
|
||||
|
||||
|
||||
|
||||
self.cfg_j_limit()
|
||||
|
||||
# ---------- 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 _rebuild(self):
|
||||
self.kinematics = RM75Kinematics(self._urdf_path, self._limits)
|
||||
self.solver = RM75IkSolver(self.kinematics)
|
||||
self.model = self.kinematics.model
|
||||
self.data = self.kinematics.data
|
||||
|
||||
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):
|
||||
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:
|
||||
max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
|
||||
if rad_flag:
|
||||
for i in range(7):
|
||||
self.model.lowerPositionLimit[i] = min_j[i]
|
||||
self.model.upperPositionLimit[i] = max_j[i]
|
||||
else:
|
||||
for i in range(7):
|
||||
self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi
|
||||
self.model.upperPositionLimit[i] = max_j[i] / 180 * pi
|
||||
max_j = [pi, 2.2689, pi, 2.3562, pi, 2.234, 2 * pi]
|
||||
lower = np.asarray(min_j, dtype=float)
|
||||
upper = np.asarray(max_j, dtype=float)
|
||||
if not rad_flag:
|
||||
lower = np.deg2rad(lower)
|
||||
upper = np.deg2rad(upper)
|
||||
self._limits = JointLimits("legacy", lower, upper)
|
||||
self._rebuild()
|
||||
|
||||
def forward_kinematics(self, joint_angles, tool="omnipic"):
|
||||
"""
|
||||
Compute forward kinematics.
|
||||
def _tool(self, name):
|
||||
try:
|
||||
return self._tools[name]
|
||||
except KeyError as exc:
|
||||
raise ValueError(f"unknown tool frame: {name!r}") from exc
|
||||
|
||||
Args:
|
||||
joint_angles: List or array of 7 joint angles (radians)
|
||||
tool: Name of frame to compute
|
||||
|
||||
Returns:
|
||||
dict: Position, rotation, rpy, quaternion
|
||||
unit: position: m
|
||||
rpy: rad
|
||||
"""
|
||||
if len(joint_angles) != 7:
|
||||
raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
|
||||
|
||||
# Create configuration vector
|
||||
q = pin.neutral(self.model)
|
||||
for i, angle in enumerate(joint_angles):
|
||||
q[i] = angle
|
||||
|
||||
# Compute forward kinematics
|
||||
pin.forwardKinematics(self.model, self.data, q)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
|
||||
# Get frame transform
|
||||
frame_id = self.tool_frames[tool]
|
||||
frame_transform = self.data.oMf[frame_id]
|
||||
|
||||
# Extract results
|
||||
position = frame_transform.translation.copy()
|
||||
rotation = frame_transform.rotation.copy()
|
||||
|
||||
# Compute RPY
|
||||
rpy = pin.rpy.matrixToRpy(rotation)
|
||||
|
||||
# Compute quaternion
|
||||
# quat = pin.Quaternion(rotation)
|
||||
pose = np.concatenate([position, rpy], axis=0)
|
||||
return pose
|
||||
# return {
|
||||
# 'position': position,
|
||||
# # 'rotation': rotation,
|
||||
# 'rpy': rpy,
|
||||
# 'quaternion': [quat.x, quat.y, quat.z, quat.w],
|
||||
# # 'transform': frame_transform
|
||||
# }
|
||||
|
||||
def inverse_kinematics(self, target_position, target_rpy=None,
|
||||
target_quat=None, initial_guess=None,
|
||||
max_iter=500, tolerance=5e-3, debug=False, tool="ee"):
|
||||
"""
|
||||
Compute inverse kinematics using differential IK with multiple strategies.
|
||||
|
||||
Args:
|
||||
target_position: [x, y, z] target position (meters)
|
||||
target_rpy: [roll, pitch, yaw] target orientation (radians)
|
||||
target_quat: [x, y, z, w] target orientation as quaternion
|
||||
initial_guess: Initial joint angles (radians)
|
||||
max_iter: Maximum iterations
|
||||
tolerance: Error tolerance
|
||||
debug: Print debug information
|
||||
tool: the frame name ('scissor', 'camera', 'ee')
|
||||
|
||||
Returns:
|
||||
tuple: (joint_angles, success, error)
|
||||
"""
|
||||
# Build target SE3 placement
|
||||
if target_quat is not None:
|
||||
quat = pin.Quaternion(target_quat[3], target_quat[0], target_quat[1], target_quat[2])
|
||||
target_rotation = quat.matrix()
|
||||
elif target_rpy is not None:
|
||||
target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
|
||||
target_rpy[1],
|
||||
target_rpy[2])
|
||||
else:
|
||||
target_rotation = np.eye(3)
|
||||
|
||||
target_placement = pin.SE3(target_rotation, np.array(target_position))
|
||||
|
||||
# Try multiple initial guesses
|
||||
initial_guesses = []
|
||||
|
||||
if initial_guess is not None:
|
||||
initial_guesses.append(initial_guess)
|
||||
else:
|
||||
# Try different initial configurations
|
||||
initial_guesses.append([0.1] * 7) # Zero config
|
||||
|
||||
|
||||
best_solution = None
|
||||
best_error = float('inf')
|
||||
|
||||
for guess_idx, guess in enumerate(initial_guesses):
|
||||
q = pin.neutral(self.model)
|
||||
for i, angle in enumerate(guess):
|
||||
if i < len(q):
|
||||
q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
|
||||
self.model.upperPositionLimit[i])
|
||||
q_ref = q.copy()
|
||||
|
||||
# Differential IK with adaptive damping
|
||||
damping = 0.1
|
||||
damping_reduction = 0.95
|
||||
iter_count = 0
|
||||
prev_error = float('inf')
|
||||
|
||||
ee_frame_id = self.tool_frames[tool]
|
||||
|
||||
J = pin.computeFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
q,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL
|
||||
def forward_kinematics(self, joint_angles, tool="no_tool"):
|
||||
pose = self.kinematics.forward(np.asarray(joint_angles), self._tool(tool))
|
||||
return np.concatenate(
|
||||
[pose.translation.copy(), pin.rpy.matrixToRpy(pose.rotation)]
|
||||
)
|
||||
|
||||
pin.forwardKinematics(self.model, self.data, q)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
|
||||
current_placement = self.data.oMf[ee_frame_id]
|
||||
|
||||
error_SE3 = current_placement.actInv(target_placement)
|
||||
error_vec = pin.log(error_SE3).vector
|
||||
|
||||
# print("\n initial error =", np.linalg.norm(error_vec))
|
||||
# print(error_vec)
|
||||
|
||||
while iter_count < max_iter:
|
||||
# Compute forward kinematics
|
||||
|
||||
pin.computeJointJacobians(self.model, self.data, q)
|
||||
pin.framesForwardKinematics(self.model, self.data, q)
|
||||
|
||||
# Get current end-effector placement
|
||||
current_placement = self.data.oMf[ee_frame_id]
|
||||
|
||||
# Compute error
|
||||
error_SE3 = current_placement.actInv(target_placement)
|
||||
error_vec = pin.log(error_SE3).vector
|
||||
error_norm = np.linalg.norm(error_vec)
|
||||
|
||||
if error_norm < tolerance:
|
||||
if error_norm < best_error:
|
||||
best_error = error_norm
|
||||
best_solution = q[:7].copy()
|
||||
break
|
||||
|
||||
# Check if error is increasing (diverging)
|
||||
if error_norm > prev_error * 1.1 and iter_count > 10:
|
||||
damping = min(1.0, damping * 1.5)
|
||||
else:
|
||||
damping = max(0.01, damping * damping_reduction)
|
||||
|
||||
|
||||
J = pin.getFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL
|
||||
)
|
||||
|
||||
# =========================
|
||||
# QP-based IK
|
||||
# =========================
|
||||
w_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
|
||||
def inverse_kinematics(
|
||||
self,
|
||||
target_position,
|
||||
target_rpy=None,
|
||||
target_quat=None,
|
||||
initial_guess=None,
|
||||
max_iter=500,
|
||||
tolerance=1e-3,
|
||||
debug=False,
|
||||
tool="no_tool",
|
||||
):
|
||||
|
||||
q = pin.neutral(self.model)
|
||||
|
||||
all_active_joints = self.get_active_joints_from_frame(frame_names)
|
||||
|
||||
for i in range(7):
|
||||
q[i] = joint_angles[i]
|
||||
|
||||
pin.forwardKinematics(self.model, self.data, q)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
pin.computeJointJacobians(self.model, self.data, q)
|
||||
|
||||
Js = []
|
||||
|
||||
for frame_name, active_joints in zip(frame_names, all_active_joints):
|
||||
frame_id = self.model.getFrameId(frame_name)
|
||||
|
||||
J = pin.getFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
frame_id,
|
||||
pin.ReferenceFrame.LOCAL
|
||||
)
|
||||
Js.append(J[:, active_joints])
|
||||
|
||||
return Js
|
||||
|
||||
def get_active_joints_from_frame(self, frame_names):
|
||||
"""
|
||||
Return active joint indices affecting a frame.
|
||||
|
||||
Example:
|
||||
frame_name='link_4'
|
||||
-> [0,1,2,3]
|
||||
"""
|
||||
all_active_joint_ids = []
|
||||
for frame_name in frame_names:
|
||||
frame_id = self.model.getFrameId(frame_name)
|
||||
|
||||
# Parent joint of this frame
|
||||
joint_id = self.model.frames[frame_id].parentJoint
|
||||
|
||||
print(f'frame_id = {frame_id}, and joint_id = {joint_id}')
|
||||
|
||||
active_joint_ids = []
|
||||
|
||||
|
||||
# Traverse upward to root
|
||||
while joint_id > 0:
|
||||
# Pinocchio joint indexing:
|
||||
# universe joint = 0
|
||||
# robot joints start from 1
|
||||
|
||||
active_joint_ids.append(joint_id - 1)
|
||||
|
||||
# Move to parent joint
|
||||
joint_id = self.model.parents[joint_id]
|
||||
|
||||
# Reverse so order becomes base -> tip
|
||||
active_joint_ids.reverse()
|
||||
all_active_joint_ids.append(active_joint_ids)
|
||||
|
||||
return all_active_joint_ids
|
||||
|
||||
def plan_cartesian_trajectory(self, start_pos, end_pos,
|
||||
start_rpy=None, end_rpy=None,
|
||||
num_steps=20, tool='ee'):
|
||||
"""
|
||||
Plan a Cartesian trajectory with IK for each waypoint.
|
||||
"""
|
||||
# Get current end-effector pose if start_rpy not provided
|
||||
if start_rpy is None:
|
||||
# Try to find a valid starting configuration
|
||||
test_angles = [0.1] * 7
|
||||
fk_test = self.forward_kinematics(test_angles,tool=tool)
|
||||
start_rpy = fk_test['rpy']
|
||||
|
||||
if end_rpy is None:
|
||||
end_rpy = start_rpy
|
||||
|
||||
# First, check if target is reachable
|
||||
print(f"\nChecking if target is reachable...")
|
||||
target_pos = end_pos
|
||||
target_rpy = end_rpy
|
||||
|
||||
test_solution, success, error = self.inverse_kinematics(
|
||||
target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool
|
||||
)
|
||||
|
||||
if not success:
|
||||
print(f"Warning: Target may be unreachable or difficult to reach")
|
||||
print(f"Trying with relaxed tolerance...")
|
||||
|
||||
# Initial guess for IK (start with zero configuration)
|
||||
current_angles = [0.1] * 7
|
||||
trajectory = []
|
||||
|
||||
print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})")
|
||||
print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})")
|
||||
print("-" * 60)
|
||||
|
||||
for i in range(num_steps + 1):
|
||||
t = i / num_steps
|
||||
|
||||
# Interpolate position
|
||||
pos = [
|
||||
start_pos[0] + t * (end_pos[0] - start_pos[0]),
|
||||
start_pos[1] + t * (end_pos[1] - start_pos[1]),
|
||||
start_pos[2] + t * (end_pos[2] - start_pos[2])
|
||||
]
|
||||
|
||||
# Interpolate orientation
|
||||
rpy = [
|
||||
start_rpy[0] + t * (end_rpy[0] - start_rpy[0]),
|
||||
start_rpy[1] + t * (end_rpy[1] - start_rpy[1]),
|
||||
start_rpy[2] + t * (end_rpy[2] - start_rpy[2])
|
||||
]
|
||||
|
||||
# Compute IK
|
||||
joint_angles, success, error = self.inverse_kinematics(
|
||||
pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool
|
||||
)
|
||||
|
||||
if not success:
|
||||
print(f" Waypoint {i}: IK failed!")
|
||||
break
|
||||
|
||||
# Verify
|
||||
fk_verify = self.forward_kinematics(joint_angles, tool=tool)
|
||||
|
||||
trajectory.append({
|
||||
'step': i,
|
||||
't': t,
|
||||
'position': pos,
|
||||
'rpy': rpy,
|
||||
'joint_angles': joint_angles,
|
||||
'actual_position': fk_verify['position'],
|
||||
'error': error
|
||||
})
|
||||
|
||||
# Update current angles for next iteration
|
||||
current_angles = joint_angles
|
||||
|
||||
if i % 5 == 0 or i == num_steps:
|
||||
print(f" Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), "
|
||||
f"error={error:.6f}m")
|
||||
|
||||
return trajectory
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
"""Main test function"""
|
||||
|
||||
|
||||
rm75 = KinematicsSolver()
|
||||
|
||||
# Test 1: Forward Kinematics
|
||||
print("\n1. Forward Kinematics Test")
|
||||
print("-" * 40)
|
||||
|
||||
tool_name = "scissor"
|
||||
joint_angles_zero = [0.1] * 7
|
||||
fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name)
|
||||
|
||||
print(f"Init configuration:")
|
||||
print(f" Position: ({fk_result['position'][0]:.3f}, "
|
||||
f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m")
|
||||
|
||||
# Test 2: Inverse Kinematics with more reachable target
|
||||
print("\n2. Inverse Kinematics Test")
|
||||
print("-" * 40)
|
||||
|
||||
# Try a simpler target first
|
||||
target_pos = [0.3, 0.2, 0.4] # More reachable position
|
||||
target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation
|
||||
|
||||
print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
|
||||
|
||||
import time
|
||||
init_joints = [0.2] * 7
|
||||
time0 = time.time()
|
||||
for ii in range(100):
|
||||
joint_solution, success, error = rm75.inverse_kinematics(
|
||||
target_pos, target_rpy=target_rpy, initial_guess=init_joints,
|
||||
max_iter=500, debug=False, tool=tool_name
|
||||
)
|
||||
time1 = time.time()
|
||||
print(f"Time: {time1 - time0}")
|
||||
|
||||
if success:
|
||||
print(f"✓ Solution found! Error: {error:.6f} m")
|
||||
for i, angle in enumerate(joint_solution):
|
||||
print(f" Joint {i + 1}: {degrees(angle):7.2f}°")
|
||||
|
||||
# Verify
|
||||
fk_verify = rm75.forward_kinematics(joint_solution,tool=tool_name)
|
||||
print(
|
||||
f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m")
|
||||
del debug
|
||||
if target_quat is not None:
|
||||
values = np.asarray(target_quat, dtype=float)
|
||||
quaternion = pin.Quaternion(values[3], values[0], values[1], values[2])
|
||||
rotation = quaternion.matrix()
|
||||
elif target_rpy is not None:
|
||||
rotation = pin.rpy.rpyToMatrix(*target_rpy)
|
||||
else:
|
||||
print("✗ IK failed to find a solution!")
|
||||
|
||||
# Test 3: Jacobian
|
||||
print("\n3. Jacobian Matrix")
|
||||
print("-" * 40)
|
||||
|
||||
J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name)
|
||||
print(f"Jacobian shape: {J.shape}")
|
||||
for i in range(min(3, J.shape[0])):
|
||||
row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)])
|
||||
print(f" Row {i + 1}: {row_str}")
|
||||
|
||||
# Test 4: Trajectory Planning with reachable positions
|
||||
print("\n4. Cartesian Trajectory Planning")
|
||||
print("-" * 40)
|
||||
|
||||
start_pos = [0.3, 0.0, 0.4] # Start position
|
||||
end_pos = [0.3, 0.0, 0.55] # End position (smaller movement)
|
||||
|
||||
fk0 = rm75.forward_kinematics([0.1] * 7, tool=tool_name)
|
||||
|
||||
trajectory = rm75.plan_cartesian_trajectory(
|
||||
start_pos,
|
||||
end_pos,
|
||||
start_rpy=fk0['rpy'],
|
||||
end_rpy=[
|
||||
fk0['rpy'][0] + radians(10),
|
||||
fk0['rpy'][1],
|
||||
fk0['rpy'][2]
|
||||
],
|
||||
num_steps=10,
|
||||
tool=tool_name
|
||||
rotation = np.eye(3)
|
||||
tool_pose = self._tool(tool)
|
||||
target_tool = pin.SE3(rotation, np.asarray(target_position, dtype=float))
|
||||
target_flange = target_tool * tool_pose.inverse()
|
||||
seed = np.zeros(7) if initial_guess is None else np.asarray(initial_guess, dtype=float)
|
||||
result = self.solver.solve(
|
||||
target_flange,
|
||||
seed,
|
||||
IkOptions(
|
||||
position_tolerance_m=tolerance,
|
||||
orientation_tolerance_rad=tolerance,
|
||||
max_iterations=max_iter,
|
||||
),
|
||||
)
|
||||
return (0, result.q.tolist()) if result.success else (-1, [])
|
||||
|
||||
if trajectory:
|
||||
print(f"\n✓ Generated {len(trajectory)} waypoints")
|
||||
|
||||
if success:
|
||||
print("✓ Inverse kinematics working (with simplified target)")
|
||||
else:
|
||||
print("⚠ Inverse kinematics may need tuning - try different targets")
|
||||
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print(f'test subchain Jacobian, for future obstacle avoidance')
|
||||
frame_names = [
|
||||
"link_2",
|
||||
"link_4",
|
||||
"link_7"
|
||||
]
|
||||
Js_sub = rm75.get_subchain_jacobian(
|
||||
joint_angles=joint_angles_zero,
|
||||
frame_names=frame_names
|
||||
)
|
||||
print(f'Js_sub: {Js_sub}')
|
||||
|
||||
return rm75, trajectory
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
rm75, trajectory = main()
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print("All tests completed!")
|
||||
print("=" * 60)
|
||||
def compute_jacobian(self, joint_angles, tool="no_tool"):
|
||||
del tool
|
||||
return self.kinematics.jacobian(np.asarray(joint_angles, dtype=float))
|
||||
|
||||
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