diff --git a/.gitignore b/.gitignore index 31fc06f..7b8d87a 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,7 @@ __pycache__/ .pytest_cache/ .mypy_cache/ *.egg-info/ +ik_qp/artifacts/ *.log qtcreator-* *.user diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..6da3504 --- /dev/null +++ b/AGENTS.md @@ -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 代码时，优先构建目标软件包，再进行完整工作空间构建。自动化测试无法单独证明真实硬件行为安全。 diff --git a/CODEX.md b/CODEX.md deleted file mode 100644 index 14f77c6..0000000 --- a/CODEX.md +++ /dev/null @@ -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//current_pose - -> /xr_rm//raw_target_pose - -> /xr_rm//target_pose - -> /xr_rm//cmd_vel - -> /xr_rm//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. diff --git a/README.md b/README.md index b152525..cfbe974 100755 --- a/README.md +++ b/README.md @@ -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/ diff --git a/ik_qp/README.md b/ik_qp/README.md index f4b3ee2..ea37693 100644 --- a/ik_qp/README.md +++ b/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 基座至第一关节偏移。 diff --git a/ik_qp/STAGE1_VALIDATION.md b/ik_qp/STAGE1_VALIDATION.md new file mode 100644 index 0000000..3f3078d --- /dev/null +++ b/ik_qp/STAGE1_VALIDATION.md @@ -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. + diff --git a/ik_qp/environment.yml b/ik_qp/environment.yml new file mode 100644 index 0000000..7b2c24a --- /dev/null +++ b/ik_qp/environment.yml @@ -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 + diff --git a/ik_qp/kine_ctrl/main.py b/ik_qp/kine_ctrl/main.py index fbabac8..f87c15a 100644 --- a/ik_qp/kine_ctrl/main.py +++ b/ik_qp/kine_ctrl/main.py @@ -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()) diff --git a/ik_qp/kine_ctrl/rm75_kine_qp.py b/ik_qp/kine_ctrl/rm75_kine_qp.py index 009d241..26853b2 100644 --- a/ik_qp/kine_ctrl/rm75_kine_qp.py +++ b/ik_qp/kine_ctrl/rm75_kine_qp.py @@ -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) + 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]) - - self.cfg_j_limit() - - # ---------- for reused qp_solver ------------------ - self.nv = 7 - - # Full dense symmetric matrix structure - # P_template = np.triu(np.ones((7, 7))) - self.P_pattern = sparse.triu(np.ones((7,7))).tocsc() - - P_sparse = sparse.csc_matrix(self.P_pattern) - - A_sparse = sparse.eye(7, format='csc') - - self.osqp_solver = osqp.OSQP() - - self.osqp_solver.setup( - P=P_sparse, - q=np.zeros(7), - A=A_sparse, - l=-np.ones(7), - u=np.ones(7), - verbose=False, - warm_start=True, - polish=False - ) - - self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4]) - - def add_frame(self,frame_name, position, rotationXYZ): - ''' - :param frame_name: str - :param position: [x, y, z] target position (meters) - :param rotationXYZ: [x, y, z] target rotation (rad) - ''' - camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0], rotationXYZ[1], rotationXYZ[2] ) - camera_offset = pin.SE3( - camera_rotation, - np.array(position) - ) - self.model.addFrame( pin.Frame( frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) ) - - def add_tool_frames(self,dict_frames): - self.tool_frames ={} - for tool_name in dict_frames: - tool_attr = dict_frames[tool_name] - position = tool_attr[0][0:3] - rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7]) - self.add_frame(tool_name, position, rotationXYZ) - self.tool_frames.update({tool_name: self.model.getFrameId(tool_name)}) - self.data = self.model.createData() - - - def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True): + 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 + 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)] + ) - 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 + 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", + ): + del debug 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() + 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: - target_rotation = pin.rpy.rpyToMatrix(target_rpy[0], - target_rpy[1], - target_rpy[2]) + rotation = pin.rpy.rpyToMatrix(*target_rpy) else: - target_rotation = np.eye(3) - - target_placement = pin.SE3(target_rotation, np.array(target_position)) - - # Try multiple initial guesses - initial_guesses = [] - - if initial_guess is not None: - initial_guesses.append(initial_guess) - else: - # Try different initial configurations - initial_guesses.append([0.1] * 7) # Zero config - - - best_solution = None - best_error = float('inf') - - for guess_idx, guess in enumerate(initial_guesses): - q = pin.neutral(self.model) - for i, angle in enumerate(guess): - if i < len(q): - q[i] = np.clip(angle, self.model.lowerPositionLimit[i], - self.model.upperPositionLimit[i]) - q_ref = q.copy() - - # Differential IK with adaptive damping - damping = 0.1 - damping_reduction = 0.95 - iter_count = 0 - prev_error = float('inf') - - ee_frame_id = self.tool_frames[tool] - - J = pin.computeFrameJacobian( - self.model, - self.data, - q, - ee_frame_id, - pin.ReferenceFrame.LOCAL - ) - - pin.forwardKinematics(self.model, self.data, q) - pin.updateFramePlacements(self.model, self.data) - - current_placement = self.data.oMf[ee_frame_id] - - error_SE3 = current_placement.actInv(target_placement) - error_vec = pin.log(error_SE3).vector - - # print("\n initial error =", np.linalg.norm(error_vec)) - # print(error_vec) - - while iter_count < max_iter: - # Compute forward kinematics - - pin.computeJointJacobians(self.model, self.data, q) - pin.framesForwardKinematics(self.model, self.data, q) - - # Get current end-effector placement - current_placement = self.data.oMf[ee_frame_id] - - # Compute error - error_SE3 = current_placement.actInv(target_placement) - error_vec = pin.log(error_SE3).vector - error_norm = np.linalg.norm(error_vec) - - if error_norm < tolerance: - if error_norm < best_error: - best_error = error_norm - best_solution = q[:7].copy() - break - - # Check if error is increasing (diverging) - if error_norm > prev_error * 1.1 and iter_count > 10: - damping = min(1.0, damping * 1.5) - else: - damping = max(0.01, damping * damping_reduction) - - - J = pin.getFrameJacobian( - self.model, - self.data, - ee_frame_id, - pin.ReferenceFrame.LOCAL - ) - - # ========================= - # QP-based IK - # ========================= - w_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 - ): - - 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 + 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 not success: - print(f"Warning: Target may be unreachable or difficult to reach") - print(f"Trying with relaxed tolerance...") - - # Initial guess for IK (start with zero configuration) - current_angles = [0.1] * 7 - trajectory = [] - - print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})") - print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})") - print("-" * 60) - - for i in range(num_steps + 1): - t = i / num_steps - - # Interpolate position - pos = [ - start_pos[0] + t * (end_pos[0] - start_pos[0]), - start_pos[1] + t * (end_pos[1] - start_pos[1]), - start_pos[2] + t * (end_pos[2] - start_pos[2]) - ] - - # Interpolate orientation - rpy = [ - start_rpy[0] + t * (end_rpy[0] - start_rpy[0]), - start_rpy[1] + t * (end_rpy[1] - start_rpy[1]), - start_rpy[2] + t * (end_rpy[2] - start_rpy[2]) - ] - - # Compute IK - joint_angles, success, error = self.inverse_kinematics( - pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool - ) - - if not success: - print(f" Waypoint {i}: IK failed!") - break - - # Verify - fk_verify = self.forward_kinematics(joint_angles, tool=tool) - - trajectory.append({ - 'step': i, - 't': t, - 'position': pos, - 'rpy': rpy, - 'joint_angles': joint_angles, - 'actual_position': fk_verify['position'], - 'error': error - }) - - # Update current angles for next iteration - current_angles = joint_angles - - if i % 5 == 0 or i == num_steps: - print(f" Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), " - f"error={error:.6f}m") - - return trajectory - - - -def main(): - """Main test function""" - - - rm75 = KinematicsSolver() - - # Test 1: Forward Kinematics - print("\n1. Forward Kinematics Test") - print("-" * 40) - - tool_name = "scissor" - joint_angles_zero = [0.1] * 7 - fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name) - - print(f"Init configuration:") - print(f" Position: ({fk_result['position'][0]:.3f}, " - f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m") - - # Test 2: Inverse Kinematics with more reachable target - print("\n2. Inverse Kinematics Test") - print("-" * 40) - - # Try a simpler target first - target_pos = [0.3, 0.2, 0.4] # More reachable position - target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation - - print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m") - - import time - init_joints = [0.2] * 7 - time0 = time.time() - for ii in range(100): - joint_solution, success, error = rm75.inverse_kinematics( - target_pos, target_rpy=target_rpy, initial_guess=init_joints, - max_iter=500, debug=False, tool=tool_name - ) - time1 = time.time() - print(f"Time: {time1 - time0}") - - if success: - print(f"✓ Solution found! Error: {error:.6f} m") - for i, angle in enumerate(joint_solution): - print(f" Joint {i + 1}: {degrees(angle):7.2f}°") - - # Verify - fk_verify = rm75.forward_kinematics(joint_solution,tool=tool_name) - print( - f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m") - else: - print("✗ IK failed to find a solution!") - - # Test 3: Jacobian - print("\n3. Jacobian Matrix") - print("-" * 40) - - J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name) - print(f"Jacobian shape: {J.shape}") - for i in range(min(3, J.shape[0])): - row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)]) - print(f" Row {i + 1}: {row_str}") - - # Test 4: Trajectory Planning with reachable positions - print("\n4. Cartesian Trajectory Planning") - print("-" * 40) - - start_pos = [0.3, 0.0, 0.4] # Start position - end_pos = [0.3, 0.0, 0.55] # End position (smaller movement) - - fk0 = rm75.forward_kinematics([0.1] * 7, tool=tool_name) - - trajectory = rm75.plan_cartesian_trajectory( - start_pos, - end_pos, - start_rpy=fk0['rpy'], - end_rpy=[ - fk0['rpy'][0] + radians(10), - fk0['rpy'][1], - fk0['rpy'][2] - ], - num_steps=10, - tool=tool_name - ) - - if trajectory: - print(f"\n✓ Generated {len(trajectory)} waypoints") - - if success: - print("✓ Inverse kinematics working (with simplified target)") - else: - print("⚠ Inverse kinematics may need tuning - try different targets") - - - print("\n" + "=" * 60) - print(f'test subchain Jacobian, for future obstacle avoidance') - frame_names = [ - "link_2", - "link_4", - "link_7" - ] - Js_sub = rm75.get_subchain_jacobian( - joint_angles=joint_angles_zero, - frame_names=frame_names - ) - print(f'Js_sub: {Js_sub}') - - return rm75, trajectory - - -if __name__ == "__main__": - rm75, trajectory = main() - - print("\n" + "=" * 60) - print("All tests completed!") - print("=" * 60) \ No newline at end of file + def compute_jacobian(self, joint_angles, tool="no_tool"): + del tool + return self.kinematics.jacobian(np.asarray(joint_angles, dtype=float)) diff --git a/ik_qp/models/dual_arm_mujoco_fixed.urdf b/ik_qp/models/dual_arm_mujoco_fixed.urdf new file mode 100644 index 0000000..8f814d0 --- /dev/null +++ b/ik_qp/models/dual_arm_mujoco_fixed.urdf @@ -0,0 +1,374 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ik_qp/pyproject.toml b/ik_qp/pyproject.toml new file mode 100644 index 0000000..50df255 --- /dev/null +++ b/ik_qp/pyproject.toml @@ -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" + diff --git a/ik_qp/src/rm75_ik/__init__.py b/ik_qp/src/rm75_ik/__init__.py new file mode 100644 index 0000000..3c4ec50 --- /dev/null +++ b/ik_qp/src/rm75_ik/__init__.py @@ -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", +] + diff --git a/ik_qp/src/rm75_ik/cli.py b/ik_qp/src/rm75_ik/cli.py new file mode 100644 index 0000000..dd86aa3 --- /dev/null +++ b/ik_qp/src/rm75_ik/cli.py @@ -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()) + diff --git a/ik_qp/src/rm75_ik/dual_arm.py b/ik_qp/src/rm75_ik/dual_arm.py new file mode 100644 index 0000000..0ae3a4f --- /dev/null +++ b/ik_qp/src/rm75_ik/dual_arm.py @@ -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'") + diff --git a/ik_qp/src/rm75_ik/kinematics.py b/ik_qp/src/rm75_ik/kinematics.py new file mode 100644 index 0000000..e2b906a --- /dev/null +++ b/ik_qp/src/rm75_ik/kinematics.py @@ -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() + diff --git a/ik_qp/src/rm75_ik/realman_reference.py b/ik_qp/src/rm75_ik/realman_reference.py new file mode 100644 index 0000000..ecde202 --- /dev/null +++ b/ik_qp/src/rm75_ik/realman_reference.py @@ -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)) + diff --git a/ik_qp/src/rm75_ik/solver.py b/ik_qp/src/rm75_ik/solver.py new file mode 100644 index 0000000..35a52e5 --- /dev/null +++ b/ik_qp/src/rm75_ik/solver.py @@ -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 + diff --git a/ik_qp/src/rm75_ik/types.py b/ik_qp/src/rm75_ik/types.py new file mode 100644 index 0000000..a75000a --- /dev/null +++ b/ik_qp/src/rm75_ik/types.py @@ -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 diff --git a/ik_qp/src/rm75_ik/validation.py b/ik_qp/src/rm75_ik/validation.py new file mode 100644 index 0000000..6669789 --- /dev/null +++ b/ik_qp/src/rm75_ik/validation.py @@ -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 diff --git a/ik_qp/tests/test_kinematics.py b/ik_qp/tests/test_kinematics.py new file mode 100644 index 0000000..c698587 --- /dev/null +++ b/ik_qp/tests/test_kinematics.py @@ -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)) + diff --git a/ik_qp/tests/test_reference_and_dual.py b/ik_qp/tests/test_reference_and_dual.py new file mode 100644 index 0000000..81e637e --- /dev/null +++ b/ik_qp/tests/test_reference_and_dual.py @@ -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) + diff --git a/ik_qp/tests/test_solver.py b/ik_qp/tests/test_solver.py new file mode 100644 index 0000000..845cb74 --- /dev/null +++ b/ik_qp/tests/test_solver.py @@ -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 + diff --git a/ik_qp/tests/test_validation_io.py b/ik_qp/tests/test_validation_io.py new file mode 100644 index 0000000..7f07133 --- /dev/null +++ b/ik_qp/tests/test_validation_io.py @@ -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()