diff --git a/docs/G20_Isaac_实现说明.md b/docs/G20_Isaac_实现说明.md new file mode 100644 index 0000000..751fb08 --- /dev/null +++ b/docs/G20_Isaac_实现说明.md @@ -0,0 +1,328 @@ +# G20 左手 · Isaac Sim 仿真实现说明 + +本文记录 **Isaac G20 PLAN 名义弧度桥** 从 URDF 到 USD、再到可跑仿真的完整实现过程，以及相对原始 URDF / 自动转换结果的全部具体改动。 + +- **函数调用与逐步逻辑**：见 [`G20_Isaac_技术详解.md`](./G20_Isaac_技术详解.md)（推荐先读此文理解代码） +- **MuJoCo 对照**：[`G20_MuJoCo_实现说明.md`](./G20_MuJoCo_实现说明.md) / [`G20_MuJoCo_技术详解.md`](./G20_MuJoCo_技术详解.md) + +--- + +## 1. 目标与数据流 + +### 1.1 目标 + +- 在 Isaac Sim / PhysX 中仿真 LinkerHand G20 **左手**。 +- 默认兼容真机 SDK 话题：`/g20/cb_left_hand_control_cmd`（0~255）。 +- 可选 PLAN 名义弧度话题（按关节名）。 +- 关节名、映射、mimic、拇指上限 **1.47**、对比视角与 MuJoCo 桥对齐。 +- 默认 **位置硬写**（`set_dof_positions`），可选 `--pd-drive`。 + +### 1.2 端到端数据流 + +```text +/g20/cb_left_hand_control_cmd (默认，0~255) + 或 /retarget/g20/left/joint_target_nominal (--plan-nominal) + 或 /cb_left_hand_control_cmd (--legacy-sdk，旧无前缀) + │ + ▼ + joint_mapping.py + · range_to_arc / sdk_range_to_*（hand="G20", side="left"） + · SDK15：thumb_mcp = arc/1.02 + · SDK16..19：*_pip = arc/0.89 + · complete_g20_mimic_positions + │ + ▼ + isaac_g20_plan_bridge.py + · Articulation 硬写 set_dof_positions（默认） + 或 set_dof_position_targets（--pd-drive） + · 每 render 帧内 phys_per_render 次物理步进（默认 16） + │ + ▼ + /sim/isaac/g20/left/joint_state + + Omniverse 透视相机（COMPARE_*） +``` + +--- + +## 2. 关键文件路径 + +| 角色 | 绝对路径 | +|------|----------| +| 启动脚本 | `/home/lxqs/isaacsim/linkerhand/run_g20_plan.sh` | +| 桥主程序 | `/home/lxqs/isaacsim/linkerhand/isaac_g20_plan_bridge.py` | +| 关节映射 | `/home/lxqs/isaacsim/linkerhand/joint_mapping.py` | +| 源 URDF | `/home/lxqs/isaacsim/linkerhand/urdf/g20/left/linkerhand_g20_left.urdf` | +| 网格 | `/home/lxqs/isaacsim/linkerhand/urdf/g20/left/meshes/*.STL` | +| 默认 USD 根 | `/home/lxqs/isaacsim/linkerhand/urdf/g20/left/linkerhand_g20_left/linkerhand_g20_left.usda` | +| Isaac Python | `$ISAAC_SIM_DIR/python.sh`（默认 `/home/lxqs/isaacsim/_build/linux-x86_64/release/python.sh`） | + +**旁路（非 PLAN 桥，勿混淆）：** + +- `linker_hand_dual_sim.py` + `run_sim.sh`：另一套双端/稳定性仿真，默认 PD 参数不同。 + +--- + +## 3. 从 URDF 到 USD + +### 3.1 URDF 来源 + +- SolidWorks → URDF Exporter 导出的 G20 左手。 +- 与 MuJoCo 仓内 URDF/网格同源；网格与 MuJoCo 侧 **MD5 一致**。 +- 当前仅有 **左手** USD；无 G20 右手资产。 + +### 3.2 URDF 上我们改过什么 + +| 改动项 | 具体内容 | +|--------|----------| +| `thumb_cmc_roll` 上限 | `upper="1.47"`（rad）；注释/映射写明相对原 **1.4** 试调 | +| mimic（URDF 内保留） | `thumb_ip` ← `thumb_mcp` × **1.02**；四指 `*_dip` ← `*_pip` × **0.89** | +| 其余限位 | 与 MuJoCo 侧同一套（见 MuJoCo 文档关节表） | + +Isaac 侧 Physics USD 中对应角度上限： + +- `physics:upperLimit = 84.22480`（度）= **1.47 rad**（`payloads/Physics/physics.usda` 中 `thumb_cmc_roll`）。 + +### 3.3 转换方式（重要） + +**运行时不导入 URDF。** + +桥内 `_load_g20()` 使用： + +```text +stage_utils.add_reference_to_stage(usd_path=默认 .usda) +``` + +挂的是 **预烘焙 USD**。根层标注： + +```text +creator = "URDF USD Converter v0.1.3" +``` + +即离线用 Isaac / URDF→USD 转换器生成，再经手改 Physics / 碰撞实例后定稿。 + +### 3.4 USD 资源树（转换产物结构） + +```text +linkerhand_g20_left.usda # 根；默认 Physics variant = "physx" + └── payloads/ + base.usda + robot.usda + geometries.usd / materials.usda + instances.usda # 碰撞实例（convexHull + 选择性开关） + Physics/ + physics.usda # 关节限位等 + physx.usda # PhysX articulation / 自碰等 + mujoco.usda # 转换器附带的 Mjc 层（桥走 physx） + hand_body_limits.usda # 刚体线/角速度与阻尼限制 +``` + +桥启动时显式：`set_prim_variants(..., [("Physics", "physx")])`。 + +### 3.5 相对「纯 URDF→USD」我们改过的 Physics / 碰撞 + +#### A. 自碰与求解器（`physx.usda`） + +| 项 | 值 | +|----|-----| +| `physxArticulation:enabledSelfCollisions` | **1**（开） | +| solver 位置/速度迭代 | **32 / 4** | +| `sleepThreshold` | **0.005** | +| 关节 `maxJointVelocity` | **171.887** | +| 关节 `armature` | **0.05** | + +#### B. 刚体限速（`hand_body_limits.usda`） + +各 link 大致： + +- `maxLinearVelocity = 3.0` +- `maxAngularVelocity = 360.0` +- linear/angular damping **6.0** + +目的：降低 PD 握拳等场景下爆炸/乱飞。 + +#### C. 选择性碰撞（`instances.usda`）——核心手改 + +碰撞近似：`physics:approximation = "convexHull"`，`purpose = "guide"`。 +注释写明策略：**掌心 + 指尖开碰撞；近端/中节多数关掉**，避免张开姿态假嵌入、以及 PD 握拳末端爆炸。 + +| 碰撞 | 典型 link（`physics:collisionEnabled`） | +|------|------------------------------------------| +| **ON (=1)** | `hand_base_link`（掌心）、`thumb_distal`、`index_distal`、`middle_distal`、`pinky_distal`（`ring_distal` 复用 index 远端实例，同样 ON） | +| **OFF (=0)** | CMC/掌骨近端、四指 `proximal`/`middle`、拇指 shaft 等 | + +接触偏移示例：`contactOffset = 0.001`，`restOffset = -0.0005`。 + +说明：这与 MuJoCo 的「胶囊 + exclude + 投影」是 **不同手段**；Isaac 默认硬写时 PhysX 接触 **不能** 像 MuJoCo `collision_project` 那样在写位置前把姿态投影回去，硬写仍可能视觉穿模。 + +#### D. 运行时补丁（桥代码，非 USD 文件） + +| 函数/常量 | 具体行为 | +|-----------|----------| +| `_patch_joint_max_forces` | RevoluteJoint angular drive `maxForce` 若 **< 8.0** 则抬到 **8.0** | +| `_fix_base_to_world` | 钉住 `hand_base_link`（改 `root_joint` 或建 `WorldFixedJoint`） | +| 安装位姿 | `/World/G20Hand` 位置 **`[0.0, 0.45, 0.70]`**（`HAND_MOUNT_Y/Z`） | +| 自碰 API | 默认跟随 USD 开自碰；`--no-self-collision` 时 `set_enabled_self_collisions(False)` | + +--- + +## 4. 关节映射 `joint_mapping.py` + +与 MuJoCo `g20_joints.py` 的 G20 左手数值 **一致**（同一套 SDK 表 / MIN/MAX/DIR / OPEN / mimic / SDK15–19 规则）。 + +### 4.1 SDK → 关节 + +| SDK | 关节 | +|-----|------| +| 0 | `thumb_cmc_pitch` | +| 1–4 | `index/middle/ring/pinky_mcp_pitch` | +| 5 | `thumb_cmc_roll` | +| 6–9 | `index/middle/ring/pinky_mcp_roll` | +| 10 | `thumb_cmc_yaw` | +| 11–14 | 预留 | +| 15 | `thumb_mcp` ← 写入 `arc/1.02` | +| 16–19 | `*_pip` ← 写入 `arc/0.89` | + +### 4.2 我们写入/试调过的表项 + +```text +# 通道5 注释：试调 1.47（原 1.4） +G20_L_MAX[5] = 1.47 + +# 左手侧摆 6..9：与 L20_L_DIR 不同，强制为 +1 +G20_L_DIR = [-1×6, 1,1,1,1, -1, 0×4, -1×5] + +DIP_MIMIC_MULTIPLIER = 0.89 +THUMB_IP_MIMIC_MULTIPLIER = 1.02 +``` + +张开预设 `G20_OPEN_CMD` 与 MuJoCo 相同： + +```text +[255×6, 193, 148, 105, 42, 245, 255×4, 255×5] +``` + +主动 16 关节 / 从动 5 关节（mimic）划分与 MuJoCo 一致。 + +--- + +## 5. 桥节点 `isaac_g20_plan_bridge.py` 做了哪些事 + +### 5.1 话题 + +| 模式 | 订阅 | 发布 | +|------|------|------| +| 默认 | `/g20/cb_left_hand_control_cmd` | `/sim/isaac/g20/left/joint_state` | +| `--plan-nominal` | `/retarget/g20/left/joint_target_nominal` | 同上 | +| `--legacy-sdk` | `/cb_left_hand_control_cmd` | 同上 | + +- 消息类型：`sensor_msgs/JointState`。 +- cmd：优先按 `name` 映射到 SDK 索引；否则按下标。 +- 发布：仿真 DOF 名 + 弧度（及 velocity）。 +- `TOPIC_LEGACY_STATE = /cb_left_hand_state` 已定义，但当前 **未实际发布**（`legacy_pub` 恒为 `None`）。 + +ROS2 节点名：`isaac_g20_plan_bridge`；启用扩展：`isaacsim.ros2.bridge`。 + +### 5.2 写关节方式 + +| 模式 | API | 细节 | +|------|-----|------| +| **默认硬写** | `set_dof_positions` + `set_dof_velocities(0)` | 写 **全关节含 mimic**；gains 全 0，避免与硬写冲突；物理步进后常再钉一次 | +| **`--pd-drive`** | `set_dof_position_targets` | **仅主动关节**；mimic gains=0；拇指四关节 stiff/damp/effort ≈ **10/3/6**，其余 ≈ **15/2.5/8** | + +其它： + +- 物理 `dt = 1/120`；默认 `phys_per_render = 16`。 +- slew：`--slew-rad`，单位是 **rad / 物理步**，默认 **`0.0`（瞬达）**（与 MuJoCo 的 rad/s 不同）。 +- 限位边距：`LIMIT_MARGIN_RAD = 0.05`。 +- `--hand-gravity`：仅 PD 模式下有意义。 +- 检测到 NaN/Inf → 复位到张开位姿。 + +### 5.3 对比视角（与 MuJoCo 对齐） + +| 常量 | 值 | +|------|-----| +| `COMPARE_LOOKAT_LOCAL` | `(0.04, -0.04, 0.12)` | +| `COMPARE_DISTANCE` | `0.50` | +| `COMPARE_ELEVATION_DEG` | `-12.0` | +| 方位 | 相机从 **+X** 看向 lookat（等价 MuJoCo `azimuth=180`） | +| 手世界原点 | `(0, HAND_MOUNT_Y, HAND_MOUNT_Z) = (0, 0.45, 0.70)` | +| 相机 prim | `/OmniverseKit_Persp` | + +### 5.4 CLI 开关一览 + +`--headless`、`--g20-usd`、`--plan-nominal`、`--legacy-sdk`、`--pd-drive`、`--hand-gravity`、`--no-self-collision`、`--slew-rad`、`--phys-per-render`、`--track-test`、`--cmd-topic`、`--nominal-topic`、`--sim-state-topic`、`--max-steps` + +**没有** MuJoCo 那套 `--no-collision-project`（Isaac 侧无同款投影算法）。 + +### 5.5 相对「只加载 URDF 默认导入」桥层额外保证 + +1. 固定使用预烘焙 USD + PhysX variant。 +2. 基座钉世界，手抬离地面安装。 +3. 驱动 maxForce 下限补丁。 +4. 默认硬写零增益；PD 模式单独设拇指/四指增益。 +5. 自碰默认 ON，可用 CLI 关。 +6. ROS2 话题与 MuJoCo/真机协议对齐（状态话题前缀不同：`/sim/isaac/...`）。 + +--- + +## 6. 如何启动 + +```bash +/home/lxqs/isaacsim/linkerhand/run_g20_plan.sh +``` + +脚本等价于： + +```bash +"$ISAAC_SIM_DIR/python.sh" /home/lxqs/isaacsim/linkerhand/isaac_g20_plan_bridge.py "$@" +``` + +常用： + +```bash +./run_g20_plan.sh --plan-nominal +./run_g20_plan.sh --pd-drive +./run_g20_plan.sh --no-self-collision +./run_g20_plan.sh --headless --track-test +./run_g20_plan.sh --slew-rad 0.02 --phys-per-render 8 +``` + +--- + +## 7. 与 MuJoCo 实现的差异（必须分清） + +| 项 | Isaac（本文） | MuJoCo | +|----|---------------|--------| +| 模型 | 预烘焙 **USD** | **MJCF** | +| 运行时是否读 URDF | **否** | 否（读 MJCF；MJCF 由 URDF 转出） | +| 状态话题 | `/sim/isaac/g20/left/joint_state` | `/sim/mujoco/g20/left/joint_state` | +| 默认驱动 | `set_dof_positions` 硬写 | `qpos` 硬写 + `mj_forward` | +| 可选 PD | PhysX angular drive | MJCF `` + `mj_step` | +| **collision project** | **无** | **有**（默认开） | +| 防穿模资产手段 | `instances.usda` 选择性 collider + 自碰 | visual/capsule 分层 + 28 条 exclude | +| slew | rad/**物理步**，默认 0 | rad/**秒**，默认 5.0；另有墙钟 `speed=1.35` | +| 手位姿 | 世界 `(0, 0.45, 0.70)` | 世界原点 | +| 映射 | `joint_mapping.py` | `g20_joints.py`（G20 left 同数值） | + +--- + +## 8. 改动 chronologically（便于对照） + +1. URDF + meshes 入库；离线 **URDF USD Converter v0.1.3** → `linkerhand_g20_left.usda`。 +2. 手改 Physics：自碰、求解器、armature、body limits。 +3. 手改 `instances.usda`：只开掌心 + 指尖等 collider。 +4. `thumb_cmc_roll` 上限改为 **1.47**（URDF + Physics 度制上限 + `G20_L_MAX`）。 +5. 实现 `isaac_g20_plan_bridge.py`：硬写默认、可选 PD、钉基座、maxForce 补丁、安装偏置。 +6. 话题对齐 `/g20/cb_left_hand_control_cmd`；状态发 `/sim/isaac/...`。 +7. 视角 `COMPARE_*` 与 MuJoCo 对齐（+X 看掌心）。 +8. SDK15–19 除 mimic 与 MuJoCo/真机语义对齐（写进 `joint_mapping.py`）。 + +--- + +## 9. 已知限制 + +1. 默认硬写下，PhysX 接触 **推不开** 被写死的 DOF；穿模缓解主要靠选择性碰撞几何，**没有** MuJoCo 式投影。 +2. 近端/中节碰撞多为 OFF：邻指中段穿模可能检不出。 +3. 凸包近似与视觉 mesh 非凸外形不一致。 +4. 仅左手 G20 USD。 +5. `dual_sim` 与 PLAN 桥参数不同，联调时不要混用入口脚本。 diff --git a/docs/G20_Isaac_技术详解.md b/docs/G20_Isaac_技术详解.md new file mode 100644 index 0000000..a0e0066 --- /dev/null +++ b/docs/G20_Isaac_技术详解.md @@ -0,0 +1,499 @@ +# G20 左手 · Isaac Sim 技术详解 + + +资产路径、URDF→USD 改动清单见 [`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md)。 +MuJoCo 对照：[`G20_MuJoCo_技术详解.md`](./G20_MuJoCo_技术详解.md)。 + +主文件： + +| 角色 | 路径 | +|------|------| +| 桥主程序 | `/home/lxqs/isaacsim/linkerhand/isaac_g20_plan_bridge.py` | +| 关节映射 | `/home/lxqs/isaacsim/linkerhand/joint_mapping.py` | +| 启动脚本 | `/home/lxqs/isaacsim/linkerhand/run_g20_plan.sh` | +| 默认 USD | `.../urdf/g20/left/linkerhand_g20_left/linkerhand_g20_left.usda` | + +--- + +## 1. 总体结构：单线程循环（与 MuJoCo 不同） + +Isaac 桥**没有**单独的 ROS daemon 线程，也**没有** `threading.Lock`。 +主循环里每个物理步先 `rclpy.spin_once`，再写关节、再 `SimulationManager.step()`。 + +```text +┌─────────────────────────────────────────────────────────────┐ +│ 模块加载期（import 顶层） │ +│ parse_args() │ +│ SimulationApp(...) # 必须最早启动 Isaac │ +│ enable_extension(ros2.bridge) │ +│ import joint_mapping / rclpy │ +├─────────────────────────────────────────────────────────────┤ +│ main() 初始化 │ +│ create_new_stage / GroundPlane / DistantLight │ +│ _load_g20(usd) → Articulation │ +│ _fix_base_to_world / set_dof_gains / 自碰 │ +│ G20PlanBridge(...) │ +│ apply_targets() 初始 snap 到 open │ +├─────────────────────────────────────────────────────────────┤ +│ 主循环（每 render 帧内跑 phys_per_render 次物理步） │ +│ for _ in range(phys_per_render): │ +│ rclpy.spin_once(node) # 处理回调 → 改 goal │ +│ bridge.apply_targets() # slew → 写 Articulation │ +│ SimulationManager.step() # PhysX 一步 │ +│ recover_if_invalid() │ +│ bridge.publish_state() │ +│ if 位置硬写: apply_targets() 再钉一次 │ +│ RenderingManager.render() │ +│ simulation_app.update() │ +└─────────────────────────────────────────────────────────────┘ +``` + +**核心状态变量（`G20PlanBridge`）：** + +| 变量 | 类型 | 含义 | +|------|------|------| +| `goal` | `dict[str, float]` | 最新目标关节角（弧度，含 mimic） | +| `applied` | `dict[str, float]` | 本步实际写入的角（slew 后） | +| `targets` | 同 `applied` | 别名，历史兼容 | +| `track_nominal` | `dict[str, float] \| None` | 主动关节目标，供 `tracking_error()` | +| `dof_index` | `dict[str, int]` | 关节名 → Articulation DOF 下标 | +| `use_physics_drive` | `bool` | True=`--pd-drive`；False=位置硬写 | + +**与 MuJoCo 的关键差异：** + +| 项 | Isaac | MuJoCo | +|----|-------|--------| +| 线程模型 | 单线程 `spin_once` + 物理循环 | ROS daemon + 主仿真线程 + Lock | +| 防穿模 | USD 选择性 collider + 自碰 | `collision_project` + `mj_forward` | +| slew 单位 | **rad / 物理步**，默认 **0**（瞬达） | **rad / 秒**，默认 **5.0** | +| 写关节 API | `set_dof_positions` / `set_dof_position_targets` | `data.qpos` + `mj_forward` | +| 物理步进 | 每 render 默认 16 次 `SimulationManager.step` | 硬写模式不 `mj_step` | + +--- + +## 2. 程序入口与加载顺序（很重要） + +入口：`./run_g20_plan.sh` → `$ISAAC_SIM_DIR/python.sh isaac_g20_plan_bridge.py`。 + +### 2.1 为何 `parse_args` + `SimulationApp` 在顶层？ + +```text +# 文件顶层执行顺序（main 之前） +parse_args() # ① 先解析，因为 SimulationApp 要 headless +SimulationApp({"headless": ...}) # ② 启动 Omniverse / PhysX 运行时 +... import isaacsim.* ... +from joint_mapping import ... +enable_extension("isaacsim.ros2.bridge") +simulation_app.update() +import rclpy ... +``` + +Isaac 要求：**先有 `SimulationApp`，才能 import 多数 isaacsim 模块**。 +因此参数解析不能放在 `main()` 里才做；顶层 `args = parse_args()` 是刻意的。 + +`_normalize_argv`：把 `-pd-drive` 这类单横杠改成 `--pd-drive`，避免被 argparse 拆成 `-p -d ...`。 + +### 2.2 `main()` 调用链 + +```text +main() +├── stage_utils.create_new_stage() +├── GroundPlane / DistantLight +├── _compare_camera_eye_target() → set_camera_view(...) +├── hand_path = _load_g20(args.g20_usd) +│ ├── define_prim("/World/G20Hand") +│ ├── XformPrim(..., positions=[[0, 0.45, 0.70]]) +│ ├── add_reference_to_stage(usd → .../hand) +│ ├── set_prim_variants(Physics=physx) +│ └── _patch_joint_max_forces(hand_path) +├── SimulationManager.set_physics_dt(1/120) +├── play → update → _fix_base_to_world → stop → play +├── art = Articulation(hand_path) +├── art.set_enabled_self_collisions(...) +├── art.set_link_enabled_gravities(...) +├── set_dof_gains / set_dof_max_efforts # 见 §5 +├── rclpy.init(); Node("isaac_g20_plan_bridge") +├── G20PlanBridge(...) +├── slew=0; apply_targets(); 恢复 slew # 初始 snap open +└── while simulation_app.is_running(): + for _ in range(phys_per_render): # 默认 16 + spin_once → apply_targets → step → publish → [再钉] + render + update +``` + +--- + +## 3. 场景加载相关函数 + +### 3.1 `_load_g20(usd_path) → hand_path` + +```python +mount = "/World/G20Hand" +stage_utils.define_prim(mount, "Xform") +XformPrim(mount, positions=[[0.0, HAND_MOUNT_Y, HAND_MOUNT_Z]], ...) +hand_path = f"{mount}/hand" +stage_utils.add_reference_to_stage(usd_path=..., path=hand_path) +prim_utils.set_prim_variants(hand_path, variants=[("Physics", "physx")]) +_patch_joint_max_forces(hand_path) +return hand_path +``` + +要点： + +1. **运行时不读 URDF**，只挂预烘焙 USD。 +2. 手装在世界坐标 `(0, 0.45, 0.70)`，离地，避免和地面穿插。 +3. Physics variant 强制 `physx`（USD 里还有 mujoco/newton 等层）。 + +### 3.2 `_patch_joint_max_forces(hand_path, max_force=8.0)` + +遍历手下面所有 `UsdPhysics.RevoluteJoint`，取 angular DriveAPI： + +```python +if maxForce < 8.0: + attr.Set(8.0) +``` + +防止某些关节 drive 力矩上限太小，PD 跟不上。 + +### 3.3 `_fix_base_to_world(hand_path)` + +钉住掌心，手不会掉下去： + +1. 在手 prim 树里找 `hand_base_link`（或 `base_link` / `palm`）RigidBody。 +2. 若已有合法 `root_joint`：改 `LocalPos0/Rot0` 对齐当前世界位姿，Body1=base，Body0=世界。 +3. 否则新建 `{hand_path}/WorldFixedJoint`（`UsdPhysics.FixedJoint`）。 + +调用时机：`play → update → fix → stop → play`，让 PhysX 在重建 articulation 前把固定关节写进 stage。 + +### 3.4 `_compare_camera_eye_target()` + +与 MuJoCo `COMPARE_*` 对齐： + +```python +target = hand_origin + COMPARE_LOOKAT_LOCAL # (0.04,-0.04,0.12) 相对手 +eye.x = target.x + distance * cos(elev) # 从 +X 看 +eye.z = target.z - distance * sin(elev) # elev=-12° 略俯视 +``` + +`set_camera_view(eye, target, "/OmniverseKit_Persp")`；play/stop 后会再设一次，防止 viewport 被重置。 + +--- + +## 4. 映射层：`joint_mapping.py` 函数说明 + +与 MuJoCo `g20_joints.py` **数值一致**，但 API 多了 `hand`/`side` 参数（同一文件还服务 O6/L20）。 + +### 4.1 Legacy 0~255 调用链 + +```text +_on_cmd_u8(msg) + vals = [255]*20 + 优先按 msg.name → L20_SDK_TO_URDF 反查 SDK 索引填 vals + 否则按下标填 vals[:20] + sdk_range_to_full_urdf_positions(vals, "G20", "left") + ├── sdk_range_to_urdf_positions(...) + │ ├── range_to_arc(vals, "G20", "left") + │ └── SDK15: thumb_mcp = arc/1.02 + │ SDK16..19: *_pip = arc/0.89 + └── 补全 dip/ip（函数内部直接写，不等价于再调 complete_*，结果相同） + set_nominal_targets(full) + └── goal = complete_g20_mimic_positions(positions) +``` + +### 4.2 `range_to_arc(values, hand, side)` + +```python +j_min, j_max, j_dir = _tables("G20", "left") # → G20_L_MIN/MAX/DIR +for i in 0..19: + if 11 <= i <= 14: continue + if j_dir[i] == -1: + arcs[i] = lerp(val, 0, 255, j_max[i], j_min[i]) + else: + arcs[i] = lerp(val, 0, 255, j_min[i], j_max[i]) # 侧摆 6..9 +``` + +### 4.3 `sdk_range_to_urdf_positions` vs `sdk_range_to_full_urdf_positions` + +| 函数 | 输出 | +|------|------| +| `sdk_range_to_urdf_positions` | 仅主动 16 关节（已做 SDK15/16~19 除 mimic） | +| `sdk_range_to_full_urdf_positions` | 主动 + 软件解析的 `thumb_ip` / `*_dip` | + +Isaac **硬写模式**写全关节（含 mimic）；**PD 模式**只写主动关节，mimic 交给 PhysX USD mimic。 + +### 4.4 `complete_g20_mimic_positions` + +与 MuJoCo 相同：`thumb_ip = thumb_mcp * 1.02`，`*_dip = *_pip * 0.89`；已有 mimic 值不覆盖。 + +### 4.5 `open_hand_command("G20")` + +返回 `G20_OPEN_CMD`（float 列表），与 MuJoCo `G20_OPEN_CMD` 同值。 + +### 4.6 PLAN 名义路径 + +```text +_on_nominal(msg) + incoming = {name: position} + 缺主动关节 → 与旧 goal 合并 + goal = complete_g20_mimic_positions(merged) + track_nominal = {主动关节: goal[...]} +``` + +注意：Isaac 回调里**直接改 `self.goal`**，没有 MuJoCo 的 `_set_goal_only` + Lock；因为主循环同线程 `spin_once`，不存在并发写 Articulation。 + +--- + +## 5. 驱动模式：`set_dof_gains` 与 `apply_targets` + +### 5.1 初始化时增益怎么设（`main` 里） + +**默认位置硬写：** + +```python +art.set_dof_gains(stiffnesses=zeros(n), dampings=zeros(n)) +``` + +零增益，避免 PD 与 `set_dof_positions` 抢控制。 + +**`--pd-drive`：** + +```python +# 主动关节 +拇指四关节: stiff/damp/effort = 10 / 3 / 6 +其余主动: stiff/damp/effort = 15 / 2.5 / 8 +art.set_dof_gains(...); art.set_dof_max_efforts(...) + +# mimic 关节强制 0 增益 —— 否则与 PhysX mimic 冲突 → Invalid PhysX transform +art.set_dof_gains(0, 0, dof_indices=mimic_idx) +``` + +`--hand-gravity` 仅在 PD 下生效：`set_link_enabled_gravities(True)`。 + +### 5.2 `apply_targets()` — 每物理步的核心 + +```python +def apply_targets(self): + self._slew_applied_toward_goal() # goal → applied + + if use_physics_drive: + name_iter = (n for n in applied if n in actuated) # 只主动 + else: + name_iter = applied.keys() # 含 mimic + + positions = [...] + if use_physics_drive: + art.set_dof_position_targets(positions, dof_indices=indices) + else: + art.set_dof_positions(positions, dof_indices=indices) + art.set_dof_velocities(zeros(...), dof_indices=indices) +``` + +| 模式 | API | 写哪些关节 | +|------|-----|------------| +| 位置硬写（默认） | `set_dof_positions` + `set_dof_velocities(0)` | 全部（含软件算好的 mimic） | +| PD | `set_dof_position_targets` | 仅主动 16 个 | + +### 5.3 `_slew_applied_toward_goal()` + +```python +max_step = self.slew_rad # 默认 0 → 每步直达 goal +margin = LIMIT_MARGIN_RAD # 0.05，离限位留边 +for name, g in goal.items(): + g = clip(g, lo+margin, hi-margin) + if max_step <= 0: + applied[name] = g + else: + applied[name] = a ± min(|g-a|, max_step) +applied = complete_g20_mimic_positions(applied) +``` + +单位是 **每物理步弧度**，不是每秒。 +物理 dt=1/120，默认每 render 16 步 → 墙钟约 16/120 ≈ 0.133 s 一帧渲染。 + +### 5.4 为何硬写后还要「再钉一次」？ + +主循环： + +```python +bridge.apply_targets() +SimulationManager.step() # PhysX 可能轻微挪动 DOF +bridge.publish_state() +if not use_pd: + bridge.apply_targets() # 再钉回 applied +``` + +接触/积分会让硬写姿态漂移；步进后再写一遍，保证跟指令贴。 + +--- + +## 6. 异常恢复与状态发布 + +### 6.1 `recover_if_invalid()` + +```python +pos = art.get_dof_positions() +if not all finite: + goal = applied = open_full + set_dof_positions(open) + return True +``` + +PD 爆炸或数值坏掉时复位张开姿态。 + +### 6.2 `publish_state()` + +每物理步调用（不是独立 50Hz 定时器）： + +```python +pos = art.get_dof_positions() +vel = art.get_dof_velocities() # 失败则填 0 +pub → /sim/isaac/g20/left/joint_state +``` + +`legacy_pub` 恒为 `None`，不会发 `/cb_left_hand_state`； +`urdf_positions_to_sdk_range` 已实现，预留反算 0~255。 + +### 6.3 `tracking_error()` + +对 `track_nominal` 里每个主动关节：`|q_sim - q_nominal|`，返回 `(max, mean)`。 + +--- + +## 7. 自碰与防穿模（Isaac 怎么做） + +**没有** MuJoCo 的 `collision_project`。手段是： + +1. **USD `instances.usda`**：选择性开碰撞（掌心 + 指尖 ON，近端/中节多数 OFF）。 +2. **`physx.usda`**：`enabledSelfCollisions = 1`。 +3. **运行时**：`art.set_enabled_self_collisions([True])`（`--no-self-collision` 关）。 + +因此： + +- 硬写时 PhysX **推不开** 被写死的 DOF → 仍可能视觉穿模。 +- PD 模式下接触力可挡一部分，但跟手变差。 +- 这与 MuJoCo「硬写前投影」是不同哲学。 + +--- + +## 8. 完整时序例子：收到半握命令后的一物理步 + +假设默认硬写、`slew_rad=0`。 + +```text +1. rclpy.spin_once + → _on_cmd_u8 → sdk_range_to_full_urdf_positions + → set_nominal_targets → goal = 半握全关节弧度 + +2. apply_targets() + → _slew_applied_toward_goal: applied = goal（瞬达） + → set_dof_positions(全部关节含 mimic) + → set_dof_velocities(0) + +3. SimulationManager.step() + → PhysX 积分（硬写下姿态应贴近刚写的值） + +4. recover_if_invalid() # 通常 False + +5. publish_state() + → 读 get_dof_positions → 发 /sim/isaac/... + +6. apply_targets() 再钉一次（硬写模式） +``` + +若 `slew_rad=0.02`：每物理步每个关节最多走 0.02 rad，需多步才到 goal。 + +--- + +## 9. `--track-test` 路径 + +```text +max_steps 默认 480（若未指定） +每 160 物理步切换一次目标： + phase 偶: open_hand_command("G20") + phase 奇: [80.0]*20 + set_nominal_targets(sdk_range_to_full_urdf_positions(...)) + +循环结束后再 settle 240 步: + apply_targets + SimulationManager.step + +tracking_error() → max <= 0.02 → PASS，否则 exit 2 +``` + +与 MuJoCo track-test（open→半握、关投影、slew=0）流程类似，但步进计数和切换方式不同。 + +--- + +## 10. 常用 Isaac / PhysX API 速查（本工程用到的） + +| API | 用途 | +|-----|------| +| `SimulationApp` | 启动 Isaac 运行时 | +| `stage_utils.add_reference_to_stage` | 挂 USD | +| `prim_utils.set_prim_variants` | 选 Physics=physx | +| `Articulation(path)` | 包装手的 DOF | +| `set_dof_positions` / `set_dof_position_targets` | 硬写 / PD 目标 | +| `set_dof_gains` / `set_dof_max_efforts` | PD 参数 | +| `set_enabled_self_collisions` | 自碰开关 | +| `get_dof_positions` / `get_dof_velocities` / `get_dof_limits` | 读状态 | +| `SimulationManager.set_physics_dt` / `step` | 物理节拍 | +| `RenderingManager.render` | 渲染一帧 | +| `set_camera_view` | 对比视角 | + +--- + +## 11. CLI 模式对照 + +| 参数 | 效果 | +|------|------| +| （默认） | 订 `/g20/cb_left_hand_control_cmd`，位置硬写，slew=0，自碰 ON | +| `--plan-nominal` | 订名义弧度话题，`use_cmd_u8=False` | +| `--legacy-sdk` | 订 `/cb_left_hand_control_cmd` | +| `--pd-drive` | `set_dof_position_targets` + 主动关节增益 | +| `--hand-gravity` | 仅 PD 下开手部重力 | +| `--no-self-collision` | 关自碰（会穿模） | +| `--slew-rad N` | 每物理步最多转 N rad | +| `--phys-per-render N` | 每渲染帧物理步数（默认 16） | +| `--track-test` | 内置跟踪验收 | + +--- + +## 12. 改代码时最常动的位置 + +| 需求 | 改哪里 | +|------|--------| +| SDK→弧度、mimic、OPEN_CMD | `joint_mapping.py` | +| 默认硬写/PD 增益、拇指特殊增益 | `isaac_g20_plan_bridge.py` 顶部常量 + `main` 里 `set_dof_gains` | +| slew / 物理步 / 安装高度 | `DEFAULT_SLEW_RAD` / `DEFAULT_PHYS_PER_RENDER` / `HAND_MOUNT_*` | +| 相机 | `COMPARE_*` + `_compare_camera_eye_target` | +| 碰撞谁开谁关 | USD `payloads/instances.usda` | +| 自碰/求解器 | USD `payloads/Physics/physx.usda` | +| 话题名 | `TOPIC_*` 或 CLI | + +--- + +## 13. 与 MuJoCo 技术详解的对照表 + +| 概念 | Isaac 函数/位置 | MuJoCo 函数/位置 | +|------|-----------------|------------------| +| 映射入口 | `joint_mapping.sdk_range_to_full_urdf_positions` | `g20_joints.sdk_range_to_full_urdf_positions` | +| 收命令 | `_on_cmd_u8` / `_on_nominal` | `_on_legacy_cmd` / `_on_nominal` | +| 更新目标 | `set_nominal_targets` / 直接改 `goal` | `_set_goal_only`（持锁） | +| 限速 | `_slew_applied_toward_goal`（rad/步） | `_slew_applied_unlocked`（rad/s） | +| 写入仿真 | `apply_targets` → `set_dof_*` | `_hard_write_pose_unlocked` → `mj_forward` | +| 防穿模 | USD collider + 自碰（无投影） | `_project_applied_no_penetration_unlocked` | +| 物理步进 | `SimulationManager.step` | 硬写不 step；PD 用 `mj_step` | +| 发状态 | 每物理步 `publish_state` | 50Hz timer `_publish_state` | +| 状态话题 | `/sim/isaac/g20/left/joint_state` | `/sim/mujoco/g20/left/joint_state` | + +--- + +## 14. 读代码建议顺序 + +1. `run_g20_plan.sh` → 确认用的是哪份 `python.sh` +2. 文件顶层：`parse_args` → `SimulationApp` → import +3. `main()` 前半：`_load_g20` → `_fix_base` → gains +4. `G20PlanBridge.__init__` + `_on_cmd_u8` +5. `apply_targets` + 主循环里「写→step→再钉」 +6. 需要改防穿模时去看 USD `instances.usda`，而不是在桥里找 project 函数 + +资产与改动清单仍见 [`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md)。 diff --git a/docs/G20_MuJoCo_实现说明.md b/docs/G20_MuJoCo_实现说明.md new file mode 100644 index 0000000..c166418 --- /dev/null +++ b/docs/G20_MuJoCo_实现说明.md @@ -0,0 +1,362 @@ +# G20 左手 · MuJoCo 仿真实现说明 + +本文记录 **MuJoCo G20 PLAN 名义弧度桥** 从 URDF 到可跑仿真的完整实现过程，以及相对原始 URDF 的全部具体改动。 + +- **函数调用与逐步逻辑**：见 [`G20_MuJoCo_技术详解.md`](./G20_MuJoCo_技术详解.md)（推荐先读此文理解代码） +- **Isaac 对照**：[`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md) + +--- + +## 1. 目标与数据流 + +### 1.1 目标 + +- 在 MuJoCo 中仿真 LinkerHand G20 **左手**。 +- 默认兼容真机 SDK 话题协议：`sensor_msgs/JointState`，`position` 为 **0~255**。 +- 可选订阅 PLAN 名义弧度（按关节名）。 +- 关节名、0~255→弧度映射、mimic、拇指限位、对比视角与 Isaac 侧对齐。 +- 默认 **运动学硬写** `qpos`（跟手紧）；用碰撞投影减轻穿模，而不是改成默认 PD。 + +### 1.2 端到端数据流 + +```text +/g20/cb_left_hand_control_cmd (默认，0~255) + 或 /retarget/g20/left/joint_target_nominal (--plan-nominal，弧度+关节名) + │ + ▼ + utils/g20_joints.py + · range_to_arc_g20_left / sdk_range_to_urdf_positions + · SDK15：thumb_mcp = arc/1.02 + · SDK16..19：*_pip = arc/0.89 + · complete_g20_mimic_positions（dip/ip） + │ + ▼ + g20_plan_bridge.py + · ROS 回调只写 goal（不碰 MjData） + · 仿真线程：slew → [collision_project] → 硬写 qpos/ctrl + mj_forward + │ + ▼ + /sim/mujoco/g20/left/joint_state @ 50 Hz + + MuJoCo viewer（可选） +``` + +--- + +## 2. 关键文件路径 + +| 角色 | 绝对路径 | +|------|----------| +| 启动脚本 | `/home/lxqs/linker_hand_mujoco_ros2/run_g20_mujoco_plan.sh` | +| 桥主程序 | `.../linker_hand_mujoco_ros2/g20_plan_bridge.py` | +| 关节映射 | `.../linker_hand_mujoco_ros2/utils/g20_joints.py` | +| Launch | `.../launch/g20_plan_bridge.launch.py` | +| URDF | `.../urdf/G20/linker_hand_g20_left/linkerhand_g20_left.urdf` | +| MJCF | `.../urdf/G20/linker_hand_g20_left/linker_hand_g20_left.xml` | +| 网格 | `.../urdf/G20/linker_hand_g20_left/meshes/*.STL`（22 个） | +| entry_point | `setup.py` → `g20_plan_bridge=linker_hand_mujoco_ros2.g20_plan_bridge:main` | + +包根目录： +`/home/lxqs/linker_hand_mujoco_ros2/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/` + +--- + +## 3. 从 URDF 到 MJCF + +### 3.1 URDF 来源 + +1. 原始模型由 **SolidWorks → URDF Exporter** 导出（URDF 头注释：Commit `1.6.0-4-g7f85cfe`）。 +2. 本仓库资产来自 retarget 工程拷贝（非运行时自动下载），与 Isaac 侧网格 **MD5 一致**（22/22）。 +3. 入库后的 URDF 文件名：`linkerhand_g20_left.urdf`。 + +### 3.2 URDF 上我们改过什么 + +相对更早的 G20 URDF（历史上限曾为 `thumb_cmc_roll` **1.4**），当前仓库 URDF **已写入**： + +| 改动项 | 具体内容 | +|--------|----------| +| `thumb_cmc_roll` 上限 | `upper="1.47"`（rad）；与映射表 `G20_L_MAX[5]=1.47`、Isaac 同步试调 | +| mimic（保留自原 URDF） | `thumb_ip` ← `thumb_mcp` × **1.02**；四指 `*_dip` ← `*_pip` × **0.89** | +| 其它关节限位 | 与 Isaac URDF 一致（见下表），未再改数值 | + +**当前 URDF 关节限位（rad，与 MJCF `range` / actuator `ctrlrange` 一致）：** + +| 关节 | lower | upper | +|------|-------|-------| +| `thumb_cmc_roll` | 0 | **1.47** | +| `thumb_cmc_yaw` | 0 | 1.57 | +| `thumb_cmc_pitch` | 0 | 0.84 | +| `thumb_mcp` | 0 | 1.26 | +| `thumb_ip` | 0 | 1.29 | +| 四指 `*_mcp_roll` | -0.23 | 0.23 | +| 四指 `*_mcp_pitch` | 0 | 1.22 | +| 四指 `*_pip` | 0 | 1.74 | +| 四指 `*_dip` | 0 | 1.55 | + +说明：MuJoCo **不读取** URDF 的 ``。mimic 在 Python 里用 `complete_g20_mimic_positions` 补全后硬写进 `qpos`。 + +### 3.3 转换步骤（URDF → MJCF） + +仓库内 **没有** 保留可一键重跑的转换脚本；实际流程是： + +1. **初转**：用 MuJoCo `MjSpec.from_file(urdf)` 编译得到骨架 MJCF（关节树、mesh、惯性）。 +2. **手改定稿** 为当前 `linker_hand_g20_left.xml`（下列改动全部是相对「纯 URDF 导入结果」的人工修改）。 + +### 3.4 MJCF 相对 URDF / 纯导入结果的全部改动清单 + +#### A. 仿真选项与默认参数 + +| 项 | 写入值 | +|----|--------| +| `compiler` | `angle="radian" meshdir="meshes" balanceinertia="true"` | +| `option` | `timestep="0.002"`，`gravity="0 0 -9.81"`，`integrator="implicitfast"` | +| 默认 joint | `damping="0.15"`，`armature="0.01"` | +| 默认 position 执行器 | `kp="200"`，`kv="8"`，`forcelimited="true"`，`forcerange="-8 8"` | + +#### B. 显示 / 碰撞分层（相对 URDF「全身 mesh 碰撞」） + +| 类型 | 改动 | +|------|------| +| `class="visual"` | `type="mesh"`，`contype="0"`，`conaffinity="0"`，`group="2"` → **只显示，不参与碰撞** | +| 碰撞体 | **同视觉 STL 的凸包 mesh**（`type="mesh"` + `class="collision"`）：掌心 + 拇指三段 + 四指近/中/远节；比 AABB 更贴外形 | +| collision 默认 | `margin="0.0005"`，`friction="1 0.5 0.01"`，`solref="0.004 1"`，`solimp="0.9 0.95 0.001"` | + +**当前碰撞体（与视觉共用同一 `meshes/*.STL`，MuJoCo 按凸包做碰撞）：** + +| body | 类型 | 说明 | +|------|------|------| +| `hand_base` | mesh 凸包 | `hand_base_link.STL` | +| `thumb_metacarpals` / `thumb_proximal` / `thumb_distal` | mesh 凸包 | 同名 STL | +| 四指 `*_proximal` / `*_middle` / `*_distal` | mesh 凸包 | 同名 STL | +| 掌骨 metacarpals、拇指 base1/base2 | 无碰撞 geom | 仅 visual；与掌心假重叠靠 `exclude` | + +说明：MuJoCo 对 mesh 碰撞使用 **凸包**，不能还原 STL 凹坑，但整体轮廓远比 AABB/手写胶囊贴近视觉；张开/半握无假碰，握拳可检出指尖↔掌心穿透。 + +#### C. ``（共 28 条） + +用途：排除 **mesh/几何本就嵌套** 造成的假碰撞（张开姿态假重叠）。 + +- 掌心 `hand_base` ↔ 拇指 base1/base2/metacarpals、四指 metacarpals/proximal +- 拇指链相邻 / 隔代相邻 +- 四指各自 metacarpals–proximal、proximal–middle、middle–distal + +未 exclude 的对（如指尖↔掌心、邻指远端）仍可产生接触，供投影使用。 + +#### D. 执行器 + +- 为 **全部 21 个 hinge**（含 5 个 mimic 关节）各加一个 ``。 +- `thumb_cmc_roll` 的 `ctrlrange="0.0 1.47"`（与 URDF 上限一致）。 +- 默认硬写模式下主要靠写 `qpos`；`--pd-drive` 时才真正用这些执行器做 `mj_step`。 + +#### E. 命名差异 + +| URDF | MJCF | +|------|------| +| `` | `` | +| link `hand_base_link` | body 名 `hand_base`（geom 仍引用 mesh `hand_base_link`） | + +--- + +## 4. 关节映射 `g20_joints.py`（相对 SDK / Isaac） + +文件：`utils/g20_joints.py`。声明与 Isaac `joint_mapping.py` 的 G20 左手表对齐。 + +### 4.1 SDK 通道 → URDF 关节名 + +| SDK 索引 | 关节名 | +|----------|--------| +| 0 | `thumb_cmc_pitch` | +| 1–4 | `index/middle/ring/pinky_mcp_pitch` | +| 5 | `thumb_cmc_roll` | +| 6–9 | `index/middle/ring/pinky_mcp_roll` | +| 10 | `thumb_cmc_yaw` | +| 11–14 | **预留**（转换时跳过） | +| 15 | `thumb_mcp`（见 4.3） | +| 16–19 | `index/middle/ring/pinky_pip`（见 4.3） | + +### 4.2 线性插值表（我们写入的数值） + +```text +G20_L_MIN = [0,0,0,0,0, 0.0, -0.23,-0.23,-0.23,-0.23, 0.0, 0×4, 0×5] +G20_L_MAX = [0.84, 1.22×4, 1.47, 0.23×4, 1.57, 0×4, 1.29, 1.55×4] +G20_L_DIR = [-1×6, +1,+1,+1,+1, -1, 0×4, -1×5] +``` + +含义： + +- `DIR == -1`：SDK `0 → MAX`，`255 → MIN`（多数弯曲通道）。 +- `DIR == +1`（仅侧摆 6..9）：SDK `0 → MIN`，`255 → MAX`（左手侧摆与 retarget/`g20_mapping` 一致）。 +- 通道 5 上限 **1.47**（相对历史 1.4 的试调，已写进 URDF/MJCF/Isaac）。 + +### 4.3 SDK 15–19 的「除 mimic」规则（与 Isaac 相同，必须写清） + +SDK 通道语义不是「直接写主动关节弧度」，而是电机/指尖弧度空间： + +| SDK | 先算出的弧度 `arc` | 写入主动关节 | +|-----|-------------------|--------------| +| 15 | 按 `G20_L_*` 插值得到 | `thumb_mcp = arc / 1.02` | +| 16–19 | 同上 | `*_pip = arc / 0.89` | + +然后再： + +- `thumb_ip = thumb_mcp * 1.02` +- `*_dip = *_pip * 0.89` + +若漏掉「除 mimic」，DIP/IP 会整体偏大，表现为关节对不上 Isaac。 + +### 4.4 张开预设 + +```text +G20_OPEN_CMD = [ + 255,255,255,255,255,255, + 193,148,105,42, # 四指侧摆 + 245, # thumb_cmc_yaw + 255,255,255,255, # 预留 + 255,255,255,255,255 # 15..19 +] +``` + +`open_hand_positions()` / `half_fist_positions()` 基于此生成测试姿态。 + +--- + +## 5. 桥节点 `g20_plan_bridge.py` 做了哪些事 + +### 5.1 话题 + +| 方向 | 默认话题 | 内容 | +|------|----------|------| +| 订阅（默认） | `/g20/cb_left_hand_control_cmd` | `position` 0~255，取前 20，不足补 255 | +| 订阅（`--plan-nominal`） | `/retarget/g20/left/joint_target_nominal` | 按关节名弧度 | +| 发布 | `/sim/mujoco/g20/left/joint_state` | 全部 hinge 的 name/position/velocity，50 Hz | + +### 5.2 默认驱动：运动学硬写 + +- **默认**（无 `--pd-drive`）：把 `applied` 写入 `data.qpos` 与对应 `ctrl`，`qvel=0`，调用 `mj_forward`；**不调用** `mj_step`。 +- **`--pd-drive`**：写 `ctrl` → `mj_step` → 用真实 `qpos` 回写 `applied`（跟手变差，接触可挡，一般不作为默认）。 + +### 5.3 速度相关默认值（相对「瞬移」做过的调参） + +| 常量 | 值 | 含义 | +|------|-----|------| +| `DEFAULT_SPEED` | `1.35` | 相对墙钟倍率 | +| `DEFAULT_SLEW_RAD_S` | `5.0` | 关节逼近 goal 的最大角速度 (rad/s)；`0` = 瞬移 | +| `sim_dt` | `0.002`（来自 MJCF） | 每步最大转角 ≈ `5.0 * 0.002 = 0.01` rad | + +### 5.4 防穿模：`collision_project`（硬写专用，默认开） + +相对早期实现，做过这些关键修正： + +| 版本问题 | 改动 | +|----------|------| +| 仅靠 STL 自碰 | 改为 visual（不碰）+ **同 STL 凸包 collision** | +| 「相对上一帧更深才算穿」 | 改为 **绝对** 判据：`min(contact.dist) < -5e-4` 即穿 | +| 关节空间整体二分 | 拇指撞掌心会拖死四指 → 改为 **按主动关节、Δq 从大到小** 逐关节接受或二分 | +| 仍穿模 | 回退 `_last_safe_pose` | + +开关： + +- 默认：开（且仅在硬写模式生效） +- `--no-collision-project`：关 +- `--track-test`：强制关投影，且 `slew=0`，方便验收跟手误差 ≤ 0.02 rad + +常量：`PENETRATION_TOL_M = 5e-4`，`COLLISION_PROJECT_ITERS = 12`。 + +### 5.5 线程安全（修过闪退） + +- `threading.Lock` 保护 `MjData`。 +- ROS 回调 **只** 更新 `goal` / `track_nominal`（`_set_goal_only`），**禁止**在回调里碰 `MjData`。 +- 主循环持锁调用 `step_simulation_unlocked()`；发 state 时持锁读 `qpos/qvel`。 + +### 5.6 对比视角（与 Isaac 对齐） + +| 常量 | 值 | +|------|-----| +| `COMPARE_LOOKAT` | `(0.04, -0.04, 0.12)` | +| `COMPARE_DISTANCE` | `0.50` | +| `COMPARE_AZIMUTH` | `180.0`（从 +X 看向掌心） | +| `COMPARE_ELEVATION` | `-12.0` | + +手模型放在世界原点；Isaac 手有安装偏置，但 lookat 本地坐标与 elevation/distance 相同。 + +### 5.7 启动时校验 + +- 用 `G20_ALL_JOINT_NAMES` 检查 MJCF 是否缺关节；缺则 **直接退出**。 +- 缺 position actuator 也会报错退出。 + +### 5.8 其它明确改动 + +- **不** import 真机包 `linker_hand_ros2_sdk`（只兼容同话题协议）。 +- **不** 写死 `ROS_DOMAIN_ID`（启动脚本里曾有过，已去掉）。 +- 退出用 `os._exit`，规避 rclpy/mujoco 析构 abort。 + +--- + +## 6. 如何启动 + +```bash +cd /home/lxqs/linker_hand_mujoco_ros2 +./run_g20_mujoco_plan.sh +``` + +脚本会：`source /opt/ros/jazzy` → 可选 `install/setup.bash` → 把 `venv` site-packages 与包源码加入 `PYTHONPATH` → +`python3 -m linker_hand_mujoco_ros2.g20_plan_bridge`。 + +常用参数： + +```bash +./run_g20_mujoco_plan.sh --plan-nominal +./run_g20_mujoco_plan.sh --pd-drive +./run_g20_mujoco_plan.sh --no-collision-project +./run_g20_mujoco_plan.sh --speed 0.5 --slew-rad-s 3 +./run_g20_mujoco_plan.sh --headless --track-test --max-steps 800 +``` + +Launch（无参数透传，调参请用 shell）： + +```bash +ros2 launch linker_hand_mujoco_ros2 g20_plan_bridge.launch.py +``` + +测试半握示例： + +```bash +ros2 topic pub -r 20 /g20/cb_left_hand_control_cmd sensor_msgs/msg/JointState \ + "{position: [80,80,80,80,80,255,193,148,105,42,245,255,255,255,255,80,80,80,80,80]}" +``` + +--- + +## 7. 已知限制（实现层面） + +1. 硬写 + 投影 **不是** 真接触动力学；碰到凸包后关节会停在接触边界，不会「挤过去」。 +2. 碰撞是 **凸包**，凹进去的视觉细节不会产生接触；比 AABB 贴，但仍可能与非凸视觉有细微差别。 +3. 大量 `exclude` 关掉了掌心–近端假碰；真实可碰区域依赖开启碰撞的那些 link。 +4. mimic 只在软件层保证；物理引擎内无 URDF mimic 约束。 +5. 仅左手 G20；右手未做。 + +--- + +## 8. 改动 chronologically（便于对照 Git / 联调） + +1. 拷贝 G20 URDF + meshes，生成带 position actuator 的 MJCF。 +2. 实现 `g20_joints.py`（对齐 Isaac；含 SDK15–19 除 mimic）。 +3. 实现桥：名校验、订 nominal / legacy、发 `/sim/mujoco/...`。 +4. 修回调碰 `MjData` 导致的闪退 → 回调只改 goal。 +5. 话题改为 `/g20/cb_left_hand_control_cmd`；去掉脚本里 DOMAIN。 +6. 调 `speed` / `slew` 到当前默认。 +7. 拇指 `max=1.47` 写入映射 + URDF/MJCF。 +8. 视角 `COMPARE_*` 与 Isaac 对齐。 +9. 默认保持硬写；加碰撞投影；后改为绝对穿透 + **mesh 凸包碰撞** + 按关节投影。 + +--- + +## 9. 与 Isaac 的对照摘要 + +| 项 | MuJoCo（本文） | Isaac | +|----|----------------|-------| +| 模型格式 | MJCF | 预烘焙 USD（非运行时 URDF 导入） | +| 状态话题 | `/sim/mujoco/g20/left/joint_state` | `/sim/isaac/g20/left/joint_state` | +| 映射表 | `g20_joints.py` | `joint_mapping.py`（G20 左手数值一致） | +| 默认驱动 | 硬写 qpos | 硬写 `set_dof_positions` | +| 防穿模 | mesh 凸包 + `collision_project` | USD 选择性 collider + PhysX 自碰 | +| slew | 默认 5 rad/s | 默认 0（每物理步瞬达） | diff --git a/docs/G20_MuJoCo_技术详解.md b/docs/G20_MuJoCo_技术详解.md new file mode 100644 index 0000000..2cf3375 --- /dev/null +++ b/docs/G20_MuJoCo_技术详解.md @@ -0,0 +1,457 @@ +# G20 左手 · MuJoCo 技术详解 + +本文面向**读代码、改逻辑、排查问题**：按「程序从哪进、函数怎么调、数据怎么流」写清楚实现细节。 +资产路径、URDF/MJCF 改动清单见 [`G20_MuJoCo_实现说明.md`](./G20_MuJoCo_实现说明.md)。 + +--- + +## 1. 总体结构：两个线程、三块逻辑 + +```text +┌─────────────────────────────────────────────────────────────┐ +│ ROS 线程 (daemon) │ +│ rclpy.spin_once() │ +│ → _on_legacy_cmd() / _on_nominal() │ +│ → set_nominal_targets() → _set_goal_only() │ +│ → 只改 goal / track_nominal（持锁） │ +│ → 绝不碰 model / data │ +├─────────────────────────────────────────────────────────────┤ +│ 主线程（仿真循环） │ +│ with lock: step_simulation_unlocked() │ +│ → _slew_applied_unlocked() # goal → applied │ +│ → _project_applied_...() # 防穿模（默认开） │ +│ → _hard_write_pose_unlocked() # applied → qpos │ +│ → mujoco.mj_forward() # 更新运动学+接触 │ +│ viewer.sync() / _pace() │ +├─────────────────────────────────────────────────────────────┤ +│ 定时器线程 (50Hz) │ +│ _publish_state() → /sim/mujoco/g20/left/joint_state │ +└─────────────────────────────────────────────────────────────┘ +``` + +**核心状态变量（`G20PlanBridge` 内）：** + +| 变量 | 类型 | 含义 | +|------|------|------| +| `goal` | `dict[str, float]` | 最新目标关节角（弧度，含 mimic） | +| `applied` | `dict[str, float]` | 本步实际要写入仿真的角（slew + 投影后） | +| `_last_safe_pose` | `dict[str, float]` | 上一次「无穿透」的 applied，投影失败时回退 | +| `track_nominal` | `dict[str, float] \| None` | 仅主动关节，供 `tracking_error()` 验收 | + +**为什么分 `goal` 和 `applied`？** +`goal` 可以瞬间跳到新命令；`applied` 按 `slew_rad_s` 逐步逼近，且碰撞投影可能让 `applied` 停在比 `goal` 更「开」的位置。 + +--- + +## 2. 程序入口：`main()` 调用链 + +入口：`python3 -m linker_hand_mujoco_ros2.g20_plan_bridge`（或 `run_g20_mujoco_plan.sh`）。 + +```text +main() +├── parse_args() # CLI 参数 +├── mujoco.MjModel.from_xml_path(xml) # 加载 MJCF +├── mujoco.MjData(model) +├── mujoco.mj_forward(model, data) # 初始化前向 +├── validate_model_joint_names(names) # 关节名必须齐全 +├── rclpy.init() +├── G20PlanBridge.__init__(...) # 建桥、订/发话题、初始 open 姿态 +├── threading.Thread(spin_ros) # ROS 回调线程 +└── while running: + with bridge.lock: + bridge.step_simulation_unlocked() + viewer.sync() / _pace() +``` + +### 2.1 `main()` 里与模式相关的赋值 + +```python +use_legacy = args.legacy_sdk and not args.plan_nominal # 默认订 0~255 +slew_rad_s = 0.0 if args.track_test else args.slew_rad_s +collision_project = (not args.no_collision_project) and (not args.track_test) +use_pd_drive = args.pd_drive and not args.track_test +``` + +| 模式 | `hard_kinematic` | `collision_project` | `slew_rad_s` | +|------|------------------|---------------------|--------------| +| 默认 | True | True | 5.0 | +| `--pd-drive` | False | False（仅硬写模式有效） | 5.0 | +| `--no-collision-project` | True | False | 5.0 | +| `--track-test` | True | False | 0（瞬达） | + +--- + +## 3. 初始化：`G20PlanBridge.__init__` + +```text +G20PlanBridge.__init__ +├── build_joint_maps(model) +│ → joint_qpos[name] # 关节名 → qpos 下标 +│ → joint_dof[name] # 关节名 → qvel 下标 +│ → joint_lo/hi[name] # MJCF range +│ → act_id[name] # 关节名 → actuator 下标 +├── validate_model_joint_names(...) +├── open_full = open_hand_positions() # 见 §4 +├── goal = applied = open_full +├── _set_goal_only(open_full) +├── slew_rad_s 临时置 0 → _slew_applied_unlocked() → _hard_write_applied_unlocked() +├── _last_safe_pose = applied +├── create_subscription(_on_legacy_cmd 或 _on_nominal) +├── create_publisher(sim_state) +└── create_timer(50Hz, _publish_state) +``` + +### 3.1 `build_joint_maps(model)` + +遍历 `model.njnt`，只保留 **hinge**（`mjJNT_HINGE`）： + +```python +name = mujoco.mj_id2name(model, mjOBJ_JOINT, i) +joint_qpos[name] = model.jnt_qposadr[i] +joint_dof[name] = model.jnt_dofadr[i] +joint_lo/hi = model.jnt_range[i] +``` + +遍历 `model.nu` 找 position actuator：优先匹配 `_pos` 命名。 + +--- + +## 4. 映射层：`g20_joints.py` 函数说明 + +外部命令（0~255 或弧度）最终都要变成 **`dict[关节名, 弧度]`**，且 mimic 关节要补全。 + +### 4.1 调用关系（legacy SDK 路径） + +```text +_on_legacy_cmd(msg) + vals = msg.position[:20] # 不足补 255 + sdk_range_to_full_urdf_positions(vals) + ├── sdk_range_to_urdf_positions(vals) + │ ├── range_to_arc_g20_left(vals) # 20 维弧度 + │ └── 按 L20_SDK_TO_URDF 填主动关节 + │ SDK15 → thumb_mcp = arc/1.02 + │ SDK16..19 → *_pip = arc/0.89 + └── complete_g20_mimic_positions(...) + thumb_ip = thumb_mcp * 1.02 + index_dip = index_pip * 0.89 + ... + set_nominal_targets(full) + └── _set_goal_only(full) +``` + +### 4.2 `range_to_arc_g20_left(values)` — 单通道怎么算 + +对每个 SDK 索引 `i`（跳过 11~14）： + +```python +val = clamp(values[i], 0, 255) +if G20_L_DIR[i] == -1: + arc = lerp(val, 0, 255, G20_L_MAX[i], G20_L_MIN[i]) # 0→MAX, 255→MIN +else: + arc = lerp(val, 0, 255, G20_L_MIN[i], G20_L_MAX[i]) # 侧摆 6..9 +``` + +**例子：** `index_mcp_pitch` 是 SDK 1，`DIR=-1`，`MAX=1.22`，`MIN=0` +→ SDK `0` 得 `1.22` rad（弯到底），SDK `255` 得 `0`（伸直）。 + +### 4.3 `sdk_range_to_urdf_positions` — SDK15/16~19 特殊处理 + +```python +# SDK 15：电机语义是「指尖弧度空间」 +out["thumb_mcp"] = arc / 1.02 + +# SDK 16..19：电机语义是「DIP 弧度空间」，写入的是 pip +out["index_pip"] = arc / 0.89 # 同理 middle/ring/pinky +``` + +漏掉除法会导致仿真关节比真机/Isaac 偏大。 + +### 4.4 `complete_g20_mimic_positions(positions)` + +```python +for mimic, (master, ratio) in G20_MIMIC_OF.items(): + if mimic not in out and master in out: + out[mimic] = out[master] * ratio +``` + +若调用方已给出 `index_dip`，则**不覆盖**。 + +### 4.5 PLAN 名义弧度路径 + +```text +_on_nominal(msg) + incoming = {msg.name[i]: msg.position[i]} + set_nominal_targets(merged) + └── _set_goal_only(merged) + complete_g20_mimic_positions(merged) # 补 dip/ip +``` + +--- + +## 5. 仿真一步：`step_simulation_unlocked()` + +**调用方必须已持有 `self.lock`。** + +### 5.1 默认模式（运动学硬写 + 碰撞投影） + +```python +def step_simulation_unlocked(self): + prev = dict(self.applied) # ① 记下上一帧姿态 + self._slew_applied_unlocked() # ② goal → applied（限速） + if self.hard_kinematic: + if self.collision_project: + self._project_applied_no_penetration_unlocked(prev) # ③ + else: + self._hard_write_applied_unlocked() # ③' + return # ④ 不调用 mj_step + # --pd-drive 分支见 §5.3 +``` + +### 5.2 `_slew_applied_unlocked()` — 限速逼近 + +```python +max_step = slew_rad_s * sim_dt # 默认 5.0 * 0.002 = 0.01 rad/步 +for name, target in self.goal.items(): + cur = applied[name] + delta = target - cur + if abs(delta) <= max_step: + applied[name] = target + else: + applied[name] = cur + sign(delta) * max_step + applied[name] = clip(applied[name], joint_lo, joint_hi) +``` + +`slew_rad_s == 0` 时 `max_step is None`，直接 `applied = goal`。 + +### 5.3 `_hard_write_pose_unlocked(pose)` — 写入 MuJoCo + +对每个关节名： + +```python +self.data.qpos[joint_qpos[name]] = val +self.data.qvel[joint_dof[name]] = 0.0 +self.data.ctrl[act_id[name]] = val # 同步执行器目标 +self.data.qvel[:] = 0.0 +mujoco.mj_forward(self.model, self.data) +``` + +**`mj_forward` 做什么：** 根据当前 `qpos` 更新 body 位姿、碰撞几何位置，并填充 `data.contact`（`ncon`、`contact[i].dist`）。 +**不调用 `mj_step`**：没有积分、没有力矩求解，纯运动学。 + +### 5.4 `_project_applied_no_penetration_unlocked(prev)` — 防穿模（重点） + +**目的：** `applied` 朝 `goal` 走了一步后，若 mesh 凸包穿透，则缩小步进，且**按关节**处理，避免拇指撞掌心时四指一起被卡住。 + +**穿透判定：** + +```python +def _worst_penetration_unlocked(self): + worst = 0.0 + for i in range(data.ncon): + worst = min(worst, data.contact[i].dist) + return worst # 负值 = 重叠深度（米） + +def _is_penetrating_unlocked(self): + return _worst_penetration_unlocked() < -5e-4 # PENETRATION_TOL_M +``` + +`contact[i].dist`：两碰撞体表面距离；**负**表示已经穿进去。 + +**算法步骤：** + +```text +cand = complete_g20_mimic_positions(applied) # slew 后的候选 +_hard_write_pose_unlocked(cand) +if not penetrating: + applied = cand; _last_safe_pose = cand; return + +# 有穿透：从安全起点 base 开始 +_hard_write_pose_unlocked(prev) +if penetrating(prev): + base = complete_g20_mimic_positions(_last_safe_pose) +else: + base = complete_g20_mimic_positions(prev) + +# 按 |cand[name]-base[name]| 从大到小排序主动关节 +for name in order: + target = cand[name] + trial = complete_g20_mimic_positions({**base, name: target}) + _hard_write_pose_unlocked(trial) + if not penetrating: + base = trial # 该关节可走到位 + continue + # 否则在 [cur, target] 上二分 12 次 + for _ in range(12): + mid = (lo+hi)/2 + trial = complete_g20_mimic_positions({**base, name: mid}) + _hard_write_pose_unlocked(trial) + if penetrating: hi = mid + else: lo = mid; best = trial + base = best + +applied = complete_g20_mimic_positions(base) +_hard_write_pose_unlocked(applied) +if still penetrating: + applied = _last_safe_pose; 硬写回退 +else: + _last_safe_pose = applied +``` + +**注意：** 每次试姿态都要 `complete_g20_mimic_positions`，因为改 `thumb_mcp` 会带动 `thumb_ip`。 + +### 5.5 `--pd-drive` 分支 + +```python +self._ctrl_from_applied_unlocked() # data.ctrl[act_id] = applied +mujoco.mj_step(self.model, self.data) # PhysX 式积分+接触力 +self._sync_applied_from_qpos_unlocked() # applied 跟真实 qpos +``` + +此时靠 MuJoCo 执行器 + 接触求解，**不用** `collision_project`。 + +--- + +## 6. ROS 回调与发布 + +### 6.1 Legacy 命令 + +```python +def _on_legacy_cmd(self, msg): + vals = [float(x) for x in msg.position[:20]] + while len(vals) < 20: vals.append(255.0) + full = sdk_range_to_full_urdf_positions(vals) + self.set_nominal_targets(full) # → _set_goal_only +``` + +### 6.2 状态发布（50Hz） + +```python +def _publish_state(self): + with self.lock: + for name in ordered_names: + pos.append(data.qpos[joint_qpos[name]]) + vel.append(data.qvel[joint_dof[name]]) + pub.publish(JointState(...)) +``` + +读 `qpos` 也持锁，避免与仿真步并发写 `MjData`。 + +--- + +## 7. MJCF 碰撞：如何配合 `mj_forward` + +### 7.1 双层 geom + +每个要显示的 link 上通常有两个 geom： + +```xml + + +``` + +- **visual**：`contype=0 conaffinity=0`，只渲染。 +- **collision**：`contype=1 conaffinity=1`，MuJoCo 对 STL 自动生成**凸包**做碰撞。 + +有碰撞的 body：**掌心** + **拇指三段** + **四指近/中/远节**（共 16 个 collision geom）。 +掌骨 `*_metacarpals`、拇指 `base1/base2` **无** collision geom。 + +### 7.2 `contact/exclude`（28 条） + +排除「几何嵌套」导致的假接触，例如： + +```xml + + +``` + +张开手时这些对不应产生 `ncon`；握拳时**指尖↔掌心**、**邻指远端**仍可接触，供投影使用。 + +### 7.3 碰撞检测在代码里何时发生 + +每次 `_hard_write_pose_unlocked` 末尾调用 `mujoco.mj_forward` 后： + +- `data.ncon`：接触对数量 +- `data.contact[i].dist`：距离（负=穿透） + +投影逻辑只读这些字段，**不**施加接触力。 + +--- + +## 8. 完整时序例子：收到半握命令后的一帧 + +假设上一帧 `applied` 已是张开，`goal` 刚被 `_on_legacy_cmd` 设为半握弧度。 + +```text +1. 主线程 step_simulation_unlocked() +2. prev = applied（张开） +3. _slew_applied_unlocked() + index_pip: 0.0 → min(0.01, 目标-0) = 0.01 … 各关节类似 +4. _project_applied_no_penetration_unlocked(prev) + cand = complete(applied) + _hard_write_pose_unlocked(cand) → mj_forward → 检查 ncon + 若无穿透：applied=cand，结束 + 若有穿透：按关节二分缩小，更新 applied +5. （③ 里已硬写）viewer 显示新姿态 +6. 定时器 _publish_state 读出 qpos 发布 +``` + +ROS 回调可能在步骤 3 与 4 之间再次更新 `goal`；因持锁，要么整步完成后再处理新消息，要么下一步再 slew。 + +--- + +## 9. 自检：`--track-test` 函数路径 + +```text +main() + bridge.set_nominal_targets(open_hand_positions()) + loop step=0..199: + step_simulation_unlocked() # slew=0，无投影，瞬到 open + step==200: + bridge.set_nominal_targets(half_fist_positions()) + step==600: + tracking_error() → max|q_sim - q_nominal| 主动关节 + max <= 0.02 → PASS +``` + +`half_fist_positions()`：复制 `G20_OPEN_CMD`，把通道 `0..4, 15..19` 设为 `80`，再 `sdk_range_to_full_urdf_positions`。 + +--- + +## 10. 常用 MuJoCo API 速查（本工程用到的） + +| API | 用途 | +|-----|------| +| `MjModel.from_xml_path(path)` | 加载 MJCF | +| `mj_forward(model, data)` | 运动学前向 + 碰撞检测 | +| `mj_step(model, data)` | 动力学步进（仅 `--pd-drive`） | +| `mj_id2name(model, mjOBJ_JOINT, i)` | 关节名 | +| `data.qpos[adr]` / `data.qvel[adr]` | 广义坐标 | +| `data.ncon` / `data.contact[i].dist` | 接触对与穿透深度 | +| `viewer.launch_passive` + `cam.lookat/distance/azimuth/elevation` | 对比视角 | + +--- + +## 11. 改代码时最常动的位置 + +| 需求 | 改哪里 | +|------|--------| +| SDK→弧度映射、张开/半握预设 | `utils/g20_joints.py` | +| 限速、穿透阈值、投影迭代次数 | `g20_plan_bridge.py` 顶部常量 | +| 默认驱动/投影开关 | `G20PlanBridge.__init__` 或 CLI | +| 碰撞形状、exclude | `linker_hand_g20_left.xml` | +| 话题名 | `TOPIC_*` 常量或 CLI | +| 对比相机 | `COMPARE_*` + `main()` 里 `viewer.cam.*` | + +--- + +## 12. 与 Isaac 桥的对应关系（读代码对照用） + +| MuJoCo | Isaac (`isaac_g20_plan_bridge.py`) | +|--------|--------------------------------------| +| `sdk_range_to_full_urdf_positions` | `joint_mapping.sdk_range_to_full_urdf_positions` | +| `_hard_write_pose` / `set_dof_positions` | 默认都是位置硬写 | +| `collision_project` + `mj_forward` | **无**；靠 USD 选择性 collider | +| `slew_rad_s`（rad/s） | `slew_rad`（rad/物理步，默认 0） | +| `/sim/mujoco/g20/left/joint_state` | `/sim/isaac/g20/left/joint_state` | + +Isaac 对照见 [`G20_Isaac_技术详解.md`](./G20_Isaac_技术详解.md) / [`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md)。 diff --git a/run_g20_mujoco_plan.sh b/run_g20_mujoco_plan.sh new file mode 100755 index 0000000..944ae56 --- /dev/null +++ b/run_g20_mujoco_plan.sh @@ -0,0 +1,28 @@ +#!/usr/bin/env bash +# MuJoCo G20 左手仿真桥 +# 默认订阅 /g20/cb_left_hand_control_cmd（0~255） +# 发布 /sim/mujoco/g20/left/joint_state +# 默认 1.35× 墙钟 + 关节限速；可用 --speed / --slew-rad-s 再调 +set -eo pipefail +ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$ROOT" + +set +u +# shellcheck disable=SC1091 +source /opt/ros/jazzy/setup.bash +if [[ -f "$ROOT/install/setup.bash" ]]; then + # shellcheck disable=SC1091 + source "$ROOT/install/setup.bash" +fi +set -u + +VENV_SITE="$ROOT/venv/lib/python3.12/site-packages" +if [[ -d "$VENV_SITE" ]]; then + export PYTHONPATH="$VENV_SITE${PYTHONPATH:+:$PYTHONPATH}" +fi + +# 源码优先（含 urdf 资产） +PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2" +export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}" + +exec python3 -m linker_hand_mujoco_ros2.g20_plan_bridge "$@" diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/g20_plan_bridge.launch.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/g20_plan_bridge.launch.py new file mode 100644 index 0000000..143959b --- /dev/null +++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/g20_plan_bridge.launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """启动 MuJoCo G20 桥（默认订 /g20/cb_left_hand_control_cmd）。 + + 更多参数请用根目录: ./run_g20_mujoco_plan.sh [--speed 0.5] [--plan-nominal] + """ + return LaunchDescription([ + Node( + package="linker_hand_mujoco_ros2", + executable="g20_plan_bridge", + name="g20_mujoco_plan_bridge", + output="screen", + ), + ]) diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/g20_plan_bridge.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/g20_plan_bridge.py new file mode 100644 index 0000000..79a8582 --- /dev/null +++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/g20_plan_bridge.py @@ -0,0 +1,677 @@ +#!/usr/bin/env python3 +# SPDX-License-Identifier: Apache-2.0 +"""MuJoCo G20 左手仿真桥。 + +默认： + 订阅 /g20/cb_left_hand_control_cmd JointState(position 0~255) + 发布 /sim/mujoco/g20/left/joint_state JointState(实际仿真 DOF) + +可选 ``--plan-nominal``：订 /retarget/g20/left/joint_target_nominal（弧度按名）。 +默认按墙钟实时步进（``--speed`` 可调；``--no-realtime`` 尽快跑）。 +""" + +from __future__ import annotations + +import argparse +import os +import signal +import sys +import threading +import time +from pathlib import Path + +import numpy as np + +from linker_hand_mujoco_ros2.utils.g20_joints import ( + complete_g20_mimic_positions, + g20_actuated_joint_names, + g20_required_joint_names, + half_fist_positions, + open_hand_positions, + sdk_range_to_full_urdf_positions, +) + +PKG_DIR = Path(__file__).resolve().parent +DEFAULT_XML = PKG_DIR / "urdf/G20/linker_hand_g20_left/linker_hand_g20_left.xml" + +TOPIC_NOMINAL = "/retarget/g20/left/joint_target_nominal" +TOPIC_SIM_STATE = "/sim/mujoco/g20/left/joint_state" +TOPIC_LEGACY_CMD = "/g20/cb_left_hand_control_cmd" + +TRACK_TOL_RAD = 0.02 +WARN_INTERVAL_S = 2.0 +STATE_HZ = 50.0 +# 相对墙钟倍率；1.0=实时，>1 更快 +DEFAULT_SPEED = 1.35 +# 每秒最大关节角速度（rad/s）；0=瞬移到目标 +DEFAULT_SLEW_RAD_S = 5.0 +# 运动学防穿模：绝对重叠深度超过该值（米）则二分回退（勿用相对判据，否则会逐步钻入） +PENETRATION_TOL_M = 5e-4 +COLLISION_PROJECT_ITERS = 12 + +# 与 Isaac G20 共用的对比视角：正对掌心（法向约 -X）、手指朝上(+Z) +# lookat = 手几何中心（本地）；相机从 +X 看向 -X +COMPARE_LOOKAT = (0.04, -0.04, 0.12) +COMPARE_DISTANCE = 0.50 +COMPARE_AZIMUTH = 180.0 # MuJoCo：180 = 从 +X 看向掌心 +COMPARE_ELEVATION = -12.0 + + +def parse_args() -> argparse.Namespace: + p = argparse.ArgumentParser(description="MuJoCo G20 left-hand ROS2 bridge") + p.add_argument("--xml", type=Path, default=DEFAULT_XML) + p.add_argument("--headless", action="store_true", help="无 MuJoCo viewer") + p.add_argument("--max-steps", type=int, default=0, help=">0 时跑满步数退出") + p.add_argument( + "--plan-nominal", + action="store_true", + help="订 PLAN 名义弧度话题 /retarget/g20/left/joint_target_nominal", + ) + p.add_argument( + "--legacy-sdk", + action="store_true", + default=True, + help="订 /g20/cb_left_hand_control_cmd（0~255，默认开启）", + ) + p.add_argument( + "--no-legacy-sdk", + action="store_false", + dest="legacy_sdk", + help="关闭 legacy，改用 --plan-nominal / --nominal-topic", + ) + p.add_argument( + "--track-test", + action="store_true", + help="内置 open→半握 nominal，验收主动关节跟踪 ≤0.02rad", + ) + p.add_argument( + "--pd-drive", + action="store_true", + help="改用执行器+自碰（可减轻穿模，跟手不如默认运动学）", + ) + p.add_argument( + "--no-collision-project", + action="store_true", + help="关闭运动学硬写的碰撞投影（允许穿模）", + ) + p.add_argument( + "--speed", + type=float, + default=DEFAULT_SPEED, + help=f"相对实时倍率（默认 {DEFAULT_SPEED}；1.0=墙钟实时）", + ) + p.add_argument( + "--slew-rad-s", + type=float, + default=DEFAULT_SLEW_RAD_S, + help=f"关节逼近目标的最大角速度 rad/s（默认 {DEFAULT_SLEW_RAD_S}；0=瞬移）", + ) + p.add_argument( + "--no-realtime", + action="store_true", + help="不按墙钟节拍，尽快步进（自检/benchmark 用）", + ) + p.add_argument("--nominal-topic", default=TOPIC_NOMINAL) + p.add_argument("--cmd-topic", default=TOPIC_LEGACY_CMD, help="legacy SDK 控制话题") + p.add_argument("--sim-state-topic", default=TOPIC_SIM_STATE) + return p.parse_args() + + +def validate_model_joint_names(joint_names: set[str]) -> None: + required = g20_required_joint_names() + missing = sorted(required - joint_names) + if missing: + raise RuntimeError( + "G20 MJCF 关节名与 PLAN/URDF 规范不匹配，缺失: " + + ", ".join(missing) + + f" | have={sorted(joint_names)}" + ) + + +def build_joint_maps(model): + import mujoco + + joint_qpos: dict[str, int] = {} + joint_dof: dict[str, int] = {} + joint_lo: dict[str, float] = {} + joint_hi: dict[str, float] = {} + for i in range(model.njnt): + if model.jnt_type[i] != int(mujoco.mjtJoint.mjJNT_HINGE): + continue + name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) + if not name: + continue + joint_qpos[name] = int(model.jnt_qposadr[i]) + joint_dof[name] = int(model.jnt_dofadr[i]) + joint_lo[name] = float(model.jnt_range[i, 0]) + joint_hi[name] = float(model.jnt_range[i, 1]) + + act_id: dict[str, int] = {} + for i in range(model.nu): + aname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) or "" + # prefer "_pos" + if aname.endswith("_pos"): + jname = aname[: -len("_pos")] + if jname in joint_qpos: + act_id[jname] = i + continue + # fallback: actuator target joint + # model.actuator_trnid[i,0] is joint id for joint transmission + jid = int(model.actuator_trnid[i, 0]) + if 0 <= jid < model.njnt: + jname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, jid) + if jname and jname not in act_id: + act_id[jname] = i + return joint_qpos, joint_dof, joint_lo, joint_hi, act_id + + +class G20PlanBridge: + def __init__( + self, + node, + model, + data, + *, + legacy_sdk: bool, + nominal_topic: str, + cmd_topic: str, + sim_state_topic: str, + use_pd_drive: bool = False, + collision_project: bool = True, + slew_rad_s: float = DEFAULT_SLEW_RAD_S, + sim_dt: float = 0.002, + ): + from sensor_msgs.msg import JointState + + self.node = node + self.model = model + self.data = data + self.legacy_sdk = legacy_sdk + # 默认运动学硬写（原模式）；--pd-drive 才走执行器+接触 + self.hard_kinematic = not bool(use_pd_drive) + self.collision_project = bool(collision_project) and self.hard_kinematic + self.slew_rad_s = max(0.0, float(slew_rad_s)) + self.sim_dt = float(sim_dt) + self.lock = threading.Lock() + + ( + self.joint_qpos, + self.joint_dof, + self.joint_lo, + self.joint_hi, + self.act_id, + ) = build_joint_maps(model) + validate_model_joint_names(set(self.joint_qpos.keys())) + + missing_act = sorted(set(self.joint_qpos) - set(self.act_id)) + if missing_act: + raise RuntimeError(f"MJCF 缺少 position actuator: {missing_act}") + + self.actuated = g20_actuated_joint_names() + self.ordered_names = sorted(self.joint_qpos.keys()) + + open_full = open_hand_positions() + self.goal: dict[str, float] = dict(open_full) + self.applied: dict[str, float] = dict(open_full) + self.track_nominal: dict[str, float] | None = None + self._last_warn_missing = 0.0 + self._last_warn_unknown = 0.0 + self._set_goal_only(open_full) + # 初始瞬移到 open + old_slew = self.slew_rad_s + self.slew_rad_s = 0.0 + self._slew_applied_unlocked() + self._hard_write_applied_unlocked() + self._last_safe_pose: dict[str, float] = dict(self.applied) + self.slew_rad_s = old_slew + + if legacy_sdk: + self.sub = node.create_subscription( + JointState, cmd_topic, self._on_legacy_cmd, 10 + ) + node.get_logger().info(f"G20: sub {cmd_topic} (SDK 0~255)") + else: + self.sub = node.create_subscription( + JointState, nominal_topic, self._on_nominal, 10 + ) + node.get_logger().info(f"G20 PLAN: sub {nominal_topic} (rad by name)") + + self.pub = node.create_publisher(JointState, sim_state_topic, 10) + self._state_timer = node.create_timer(1.0 / STATE_HZ, self._publish_state) + mode = "kinematic-qpos" if self.hard_kinematic else "actuator+contact" + proj = "on" if self.collision_project else "off" + node.get_logger().info( + f"G20: pub {sim_state_topic}, joints={len(self.ordered_names)}, " + f"actuated={len(self.actuated)}, drive={mode}, " + f"collision_project={proj}, slew={self.slew_rad_s:.3f} rad/s" + ) + + def _clamp_name(self, name: str, val: float) -> float: + lo = self.joint_lo.get(name, -10.0) + hi = self.joint_hi.get(name, 10.0) + return float(np.clip(val, lo, hi)) + + def _set_goal_only(self, positions: dict[str, float]) -> None: + """仅更新 goal（线程安全）。禁止在 ROS 回调里碰 MjData。""" + full = complete_g20_mimic_positions(positions) + with self.lock: + self.goal = full + self.track_nominal = { + n: float(full[n]) for n in self.actuated if n in full + } + + def _slew_applied_unlocked(self) -> None: + max_step = ( + self.slew_rad_s * self.sim_dt if self.slew_rad_s > 0.0 else None + ) + for name, target in self.goal.items(): + if name not in self.joint_qpos: + continue + target = self._clamp_name(name, target) + if max_step is None: + val = target + else: + cur = float(self.applied.get(name, target)) + delta = target - cur + if abs(delta) <= max_step: + val = target + else: + val = cur + max_step * (1.0 if delta > 0 else -1.0) + val = self._clamp_name(name, val) + self.applied[name] = val + + def _hard_write_pose_unlocked(self, pose: dict[str, float]) -> None: + import mujoco + + for name, val in pose.items(): + if name not in self.joint_qpos: + continue + aid = self.act_id.get(name) + if aid is not None: + self.data.ctrl[aid] = val + self.data.qpos[self.joint_qpos[name]] = val + self.data.qvel[self.joint_dof[name]] = 0.0 + self.data.qvel[:] = 0.0 + mujoco.mj_forward(self.model, self.data) + + def _hard_write_applied_unlocked(self) -> None: + self._hard_write_pose_unlocked(self.applied) + + def _worst_penetration_unlocked(self) -> float: + """最严重重叠深度（负=穿透，0=无重叠）。""" + worst = 0.0 + for i in range(int(self.data.ncon)): + dist = float(self.data.contact[i].dist) + if dist < worst: + worst = dist + return worst + + def _is_penetrating_unlocked(self) -> bool: + """绝对穿透判定（相对上一帧会漏掉每步 None: + """按主动关节分别投影：某关节撞到时不拖死其它手指（仍硬写）。""" + cand = complete_g20_mimic_positions(dict(self.applied)) + self._hard_write_pose_unlocked(cand) + if not self._is_penetrating_unlocked(): + self.applied = cand + self._last_safe_pose = dict(self.applied) + return + + self._hard_write_pose_unlocked(prev) + if self._is_penetrating_unlocked(): + base = complete_g20_mimic_positions(dict(self._last_safe_pose)) + else: + base = complete_g20_mimic_positions(dict(prev)) + + # 变化大的关节优先尝试完整步进，减少“拇指撞掌心把四指一起卡住” + order = sorted( + self.actuated, + key=lambda n: -abs(float(cand.get(n, 0.0)) - float(base.get(n, 0.0))), + ) + for name in order: + if name not in cand or name not in self.joint_qpos: + continue + target = float(cand[name]) + cur = float(base.get(name, target)) + if abs(target - cur) < 1e-12: + continue + trial = complete_g20_mimic_positions({**base, name: target}) + self._hard_write_pose_unlocked(trial) + if not self._is_penetrating_unlocked(): + base = trial + continue + lo, hi = cur, target + best = dict(base) + for _ in range(COLLISION_PROJECT_ITERS): + mid = 0.5 * (lo + hi) + trial = complete_g20_mimic_positions({**base, name: mid}) + self._hard_write_pose_unlocked(trial) + if self._is_penetrating_unlocked(): + hi = mid + else: + lo = mid + best = trial + base = best + + self.applied = complete_g20_mimic_positions(base) + self._hard_write_pose_unlocked(self.applied) + if self._is_penetrating_unlocked(): + self.applied = dict(self._last_safe_pose) + self._hard_write_pose_unlocked(self.applied) + else: + self._last_safe_pose = dict(self.applied) + + def _ctrl_from_applied_unlocked(self) -> None: + for name, val in self.applied.items(): + aid = self.act_id.get(name) + if aid is not None: + self.data.ctrl[aid] = val + + def _sync_applied_from_qpos_unlocked(self) -> None: + """接触挡住后，以真实 qpos 为下一次 slew 起点，避免硬顶穿模。""" + for name, qadr in self.joint_qpos.items(): + self.applied[name] = float(self.data.qpos[qadr]) + + def step_simulation_unlocked(self) -> None: + """调用方须已持有 self.lock。""" + import mujoco + + prev = dict(self.applied) + self._slew_applied_unlocked() + if self.hard_kinematic: + if self.collision_project: + self._project_applied_no_penetration_unlocked(prev) + else: + self._hard_write_applied_unlocked() + return + self._ctrl_from_applied_unlocked() + mujoco.mj_step(self.model, self.data) + self._sync_applied_from_qpos_unlocked() + + def set_nominal_targets(self, positions: dict[str, float]) -> None: + self._set_goal_only(positions) + + def _on_nominal(self, msg) -> None: + if not msg.name: + self._warn_missing("empty name[]") + return + n = min(len(msg.name), len(msg.position)) + incoming = {str(msg.name[i]): float(msg.position[i]) for i in range(n)} + unknown = [k for k in incoming if k not in self.joint_qpos] + if unknown: + self._warn_unknown(unknown) + missing_act = sorted(self.actuated - set(incoming.keys())) + if missing_act: + self._warn_missing(f"missing actuated: {missing_act}") + with self.lock: + merged = dict(self.goal) + for k, v in incoming.items(): + if k in self.joint_qpos: + merged[k] = v + self.set_nominal_targets(merged) + else: + merged = {k: v for k, v in incoming.items() if k in self.joint_qpos} + self.set_nominal_targets(merged) + + def _on_legacy_cmd(self, msg) -> None: + vals = [float(x) for x in msg.position[:20]] + while len(vals) < 20: + vals.append(255.0) + full = sdk_range_to_full_urdf_positions(vals) + self.set_nominal_targets(full) + + def _warn_missing(self, detail: str) -> None: + now = time.monotonic() + if now - self._last_warn_missing < WARN_INTERVAL_S: + return + self._last_warn_missing = now + self.node.get_logger().warn( + f"nominal cmd incomplete, holding last targets ({detail})" + ) + + def _warn_unknown(self, names: list[str]) -> None: + now = time.monotonic() + if now - self._last_warn_unknown < WARN_INTERVAL_S: + return + self._last_warn_unknown = now + self.node.get_logger().warn(f"unknown joint names ignored: {names[:8]}") + + def _publish_state(self) -> None: + from sensor_msgs.msg import JointState + from std_msgs.msg import Header + + msg = JointState() + msg.header = Header() + msg.header.stamp = self.node.get_clock().now().to_msg() + names = [] + pos = [] + vel = [] + with self.lock: + for name in self.ordered_names: + names.append(name) + pos.append(float(self.data.qpos[self.joint_qpos[name]])) + vel.append(float(self.data.qvel[self.joint_dof[name]])) + msg.name = names + msg.position = pos + msg.velocity = vel + self.pub.publish(msg) + + def tracking_error(self) -> tuple[float, float]: + """返回主动关节 |q_sim - q_nominal| 的 (max, mean)。""" + if not self.track_nominal: + return 0.0, 0.0 + errs = [] + with self.lock: + for name, nom in self.track_nominal.items(): + q = float(self.data.qpos[self.joint_qpos[name]]) + errs.append(abs(q - float(nom))) + if not errs: + return 0.0, 0.0 + return float(max(errs)), float(sum(errs) / len(errs)) + + +def main(argv: list[str] | None = None) -> int: + # argparse 读 sys.argv；保留签名兼容 entry_point + _ = argv + args = parse_args() + + xml_path = args.xml.expanduser().resolve() + if not xml_path.is_file(): + print(f"[error] MJCF not found: {xml_path}", file=sys.stderr) + return 1 + + import mujoco + import mujoco.viewer + + model = mujoco.MjModel.from_xml_path(str(xml_path)) + data = mujoco.MjData(model) + model.opt.disableflags = 0 + data.qpos[:] = 0 + data.qvel[:] = 0 + mujoco.mj_forward(model, data) + + # Pre-validate before ROS init so failures are loud + names = set() + for i in range(model.njnt): + if model.jnt_type[i] != int(mujoco.mjtJoint.mjJNT_HINGE): + continue + n = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) + if n: + names.add(n) + try: + validate_model_joint_names(names) + except RuntimeError as e: + print(f"[error] {e}", file=sys.stderr) + return 1 + + import rclpy + from rclpy.node import Node + + # --plan-nominal 优先于默认 legacy + use_legacy = bool(args.legacy_sdk) and not bool(args.plan_nominal) + use_realtime = (not args.no_realtime) and (not args.track_test) + speed = max(1e-6, float(args.speed)) + dt = float(model.opt.timestep) + # track-test 需瞬达；默认运动学，仅 --pd-drive 开接触驱动 + slew_rad_s = 0.0 if args.track_test else float(args.slew_rad_s) + + rclpy.init() + node = Node("g20_mujoco_plan_bridge") + bridge = G20PlanBridge( + node, + model, + data, + legacy_sdk=use_legacy, + nominal_topic=args.nominal_topic, + cmd_topic=args.cmd_topic, + sim_state_topic=args.sim_state_topic, + use_pd_drive=bool(args.pd_drive) and not bool(args.track_test), + collision_project=(not bool(args.no_collision_project)) + and (not bool(args.track_test)), + slew_rad_s=slew_rad_s, + sim_dt=dt, + ) + node.get_logger().info( + f"timing: realtime={use_realtime} speed={speed:.3f} dt={dt:.4f}s" + ) + + running = True + + def _stop(*_): + nonlocal running + running = False + + signal.signal(signal.SIGINT, _stop) + signal.signal(signal.SIGTERM, _stop) + + def spin_ros(): + while running and rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0.01) + + ros_thread = threading.Thread(target=spin_ros, daemon=True) + ros_thread.start() + + track_phase = 0 + track_switch_step = 200 + if args.track_test: + bridge.set_nominal_targets(open_hand_positions()) + node.get_logger().info("track-test: phase0 open") + + step = 0 + exit_code = 0 + viewer = None + wall0 = time.perf_counter() + sim_time = 0.0 + + def _pace() -> None: + nonlocal sim_time + sim_time += dt + if not use_realtime: + return + target = wall0 + sim_time / speed + # 短延时用忙等，避免 time.sleep 粒度导致跑超实时 + while True: + delay = target - time.perf_counter() + if delay <= 0.0: + break + if delay > 0.002: + time.sleep(delay * 0.5) + else: + # busy-wait 尾部 + while time.perf_counter() < target: + pass + break + + def _maybe_track() -> bool: + """更新 track-test；返回 True 表示应结束。""" + nonlocal track_phase, exit_code + if not args.track_test: + return False + if step == track_switch_step: + bridge.set_nominal_targets(half_fist_positions()) + track_phase = 1 + node.get_logger().info("track-test: phase1 half-fist") + if track_phase == 1 and step >= track_switch_step + 400: + mx, mean = bridge.tracking_error() + node.get_logger().info( + f"track-test settle: max={mx:.4f} mean={mean:.4f} tol={TRACK_TOL_RAD}" + ) + if mx > TRACK_TOL_RAD: + node.get_logger().error("track-test FAIL") + exit_code = 2 + else: + node.get_logger().info("track-test PASS") + return True + return False + + try: + if args.headless: + while running: + with bridge.lock: + bridge.step_simulation_unlocked() + step += 1 + _pace() + if _maybe_track(): + break + if args.max_steps > 0 and step >= args.max_steps: + if args.track_test and track_phase == 1: + mx, mean = bridge.tracking_error() + node.get_logger().info( + f"track-test @max-steps: max={mx:.4f} mean={mean:.4f}" + ) + if mx > TRACK_TOL_RAD: + exit_code = 2 + break + else: + viewer = mujoco.viewer.launch_passive(model, data) + # 初始对比视角：正对掌心（与 Isaac 同 lookat/距离） + viewer.cam.lookat[:] = COMPARE_LOOKAT + viewer.cam.distance = COMPARE_DISTANCE + viewer.cam.azimuth = COMPARE_AZIMUTH + viewer.cam.elevation = COMPARE_ELEVATION + while running and viewer.is_running(): + with bridge.lock: + bridge.step_simulation_unlocked() + viewer.sync() + step += 1 + _pace() + if _maybe_track(): + break + if args.max_steps > 0 and step >= args.max_steps: + break + finally: + running = False + if viewer is not None: + try: + viewer.close() + except Exception: + pass + try: + bridge._state_timer.cancel() + except Exception: + pass + try: + ros_thread.join(timeout=1.0) + except Exception: + pass + try: + node.destroy_node() + except Exception: + pass + try: + if rclpy.ok(): + rclpy.shutdown() + except Exception: + pass + + print(f"[done] steps={step} exit={exit_code}") + # rclpy/mujoco 析构顺序偶发 abort；直接退出避免污染退出码 + os._exit(exit_code) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linker_hand_g20_left.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linker_hand_g20_left.xml new file mode 100644 index 0000000..29837a8 --- /dev/null +++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linker_hand_g20_left.xml @@ -0,0 +1,228 @@ + + + diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linkerhand_g20_left.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linkerhand_g20_left.urdf new file mode 100644 index 0000000..e65aebe --- /dev/null +++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linkerhand_g20_left.urdf @@ -0,0 +1,1285 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/hand_base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/hand_base_link.STL new file mode 100644 index 0000000..706606c Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/hand_base_link.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_distal.STL new file mode 100644 index 0000000..281a53c Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_distal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_metacarpals.STL new file mode 100644 index 0000000..608b860 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_metacarpals.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_middle.STL new file mode 100644 index 0000000..9f00fc4 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_middle.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_proximal.STL new file mode 100644 index 0000000..4a4473d Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/index_proximal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_distal.STL new file mode 100644 index 0000000..37d5121 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_distal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_metacarpals.STL new file mode 100644 index 0000000..96674b1 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_metacarpals.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_middle.STL new file mode 100644 index 0000000..3b17190 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_middle.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_proximal.STL new file mode 100644 index 0000000..80b553d Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/middle_proximal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_distal.STL new file mode 100644 index 0000000..8e2e782 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_distal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_metacarpals.STL new file mode 100644 index 0000000..05a4d03 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_metacarpals.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_middle.STL new file mode 100644 index 0000000..afab601 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_middle.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_proximal.STL new file mode 100644 index 0000000..446789a Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/pinky_proximal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_distal.STL new file mode 100644 index 0000000..6915e85 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_distal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_metacarpals.STL new file mode 100644 index 0000000..f502969 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_metacarpals.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_middle.STL new file mode 100644 index 0000000..88b6ef5 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_middle.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_proximal.STL new file mode 100644 index 0000000..5a06a97 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/ring_proximal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_distal.STL new file mode 100644 index 0000000..dc10991 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_distal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals.STL new file mode 100644 index 0000000..156e9b3 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals_base1.STL new file mode 100644 index 0000000..16e1059 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals_base1.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals_base2.STL new file mode 100644 index 0000000..cf5fced Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_metacarpals_base2.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_proximal.STL new file mode 100644 index 0000000..b657d19 Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/meshes/thumb_proximal.STL differ diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/g20_joints.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/g20_joints.py new file mode 100644 index 0000000..dbac6b6 --- /dev/null +++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/g20_joints.py @@ -0,0 +1,139 @@ +# SPDX-License-Identifier: Apache-2.0 +"""G20 关节名 / mimic / SDK0~255→弧度（与 Isaac joint_mapping 对齐）。""" + +from __future__ import annotations + +# SDK 索引 → URDF 关节名（不含前缀） +L20_SDK_TO_URDF: dict[int, str] = { + 0: "thumb_cmc_pitch", + 1: "index_mcp_pitch", + 2: "middle_mcp_pitch", + 3: "ring_mcp_pitch", + 4: "pinky_mcp_pitch", + 5: "thumb_cmc_roll", + 6: "index_mcp_roll", + 7: "middle_mcp_roll", + 8: "ring_mcp_roll", + 9: "pinky_mcp_roll", + 10: "thumb_cmc_yaw", + 15: "thumb_mcp", + 16: "index_pip", + 17: "middle_pip", + 18: "ring_pip", + 19: "pinky_pip", +} + +DIP_MIMIC_MULTIPLIER = 0.89 +THUMB_IP_MIMIC_MULTIPLIER = 1.02 + +G20_ACTUATED_JOINT_NAMES: tuple[str, ...] = tuple( + sorted({name for name in L20_SDK_TO_URDF.values()}) +) +G20_MIMIC_JOINT_NAMES: tuple[str, ...] = ( + "thumb_ip", + "index_dip", + "middle_dip", + "ring_dip", + "pinky_dip", +) +G20_ALL_JOINT_NAMES: tuple[str, ...] = tuple( + sorted(set(G20_ACTUATED_JOINT_NAMES) | set(G20_MIMIC_JOINT_NAMES)) +) + +G20_MIMIC_OF: dict[str, tuple[str, float]] = { + "thumb_ip": ("thumb_mcp", THUMB_IP_MIMIC_MULTIPLIER), + "index_dip": ("index_pip", DIP_MIMIC_MULTIPLIER), + "middle_dip": ("middle_pip", DIP_MIMIC_MULTIPLIER), + "ring_dip": ("ring_pip", DIP_MIMIC_MULTIPLIER), + "pinky_dip": ("pinky_pip", DIP_MIMIC_MULTIPLIER), +} + +# 左手 G20 限位（与 Isaac / URDF 一致）；SDK 索引语义同 L20 +G20_L_MIN = [0, 0, 0, 0, 0, 0.0, -0.23, -0.23, -0.23, -0.23, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +# 通道5 thumb_cmc_roll：试调 max=1.47（原 URDF/Isaac 为 1.4） +G20_L_MAX = [0.84, 1.22, 1.22, 1.22, 1.22, 1.47, 0.23, 0.23, 0.23, 0.23, 1.57, 0, 0, 0, 0, 1.29, 1.55, 1.55, 1.55, 1.55] +# 四指侧摆 (6..9)：SDK0→下限、255→上限 +G20_L_DIR = [-1, -1, -1, -1, -1, -1, 1, 1, 1, 1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] + +G20_OPEN_CMD = [ + 255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255 +] + + +def g20_required_joint_names() -> frozenset[str]: + return frozenset(G20_ALL_JOINT_NAMES) + + +def g20_actuated_joint_names() -> frozenset[str]: + return frozenset(G20_ACTUATED_JOINT_NAMES) + + +def complete_g20_mimic_positions(positions: dict[str, float]) -> dict[str, float]: + """补全缺失的 mimic 关节；已给出的 mimic 值优先保留。""" + out = dict(positions) + for mimic, (master, ratio) in G20_MIMIC_OF.items(): + if mimic in out: + continue + if master in out: + out[mimic] = float(out[master]) * float(ratio) + return out + + +def scale_value(original_value: float, a_min: float, a_max: float, b_min: float, b_max: float) -> float: + if a_max == a_min: + return b_min + return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min + + +def clamp(value: float, low: float, high: float) -> float: + return max(low, min(high, value)) + + +def range_to_arc_g20_left(values: list[float]) -> list[float]: + """SDK 0~255 → 20 维弧度（预留通道 11~14 保持 0）。""" + arcs = [0.0] * 20 + for i in range(20): + if 11 <= i <= 14: + continue + val = clamp(values[i] if i < len(values) else 0.0, 0.0, 255.0) + if G20_L_DIR[i] == -1: + arcs[i] = scale_value(val, 0.0, 255.0, G20_L_MAX[i], G20_L_MIN[i]) + else: + arcs[i] = scale_value(val, 0.0, 255.0, G20_L_MIN[i], G20_L_MAX[i]) + return arcs + + +def sdk_range_to_urdf_positions(values: list[float]) -> dict[str, float]: + """20 维 SDK → URDF 主动关节弧度（与 Isaac joint_mapping 一致）。 + + SDK 15 控制指尖弧度空间，写入 thumb_mcp = arc/1.02； + SDK 16..19 控制 DIP 电机弧度，写入 *_pip = arc/0.89。 + """ + arcs = range_to_arc_g20_left(values) + out: dict[str, float] = {} + for sdk_idx, short_name in L20_SDK_TO_URDF.items(): + arc = float(arcs[sdk_idx]) + if sdk_idx == 15: + out["thumb_mcp"] = arc / THUMB_IP_MIMIC_MULTIPLIER + elif sdk_idx in (16, 17, 18, 19): + out[short_name] = arc / DIP_MIMIC_MULTIPLIER + else: + out[short_name] = arc + return out + + +def sdk_range_to_full_urdf_positions(values: list[float]) -> dict[str, float]: + """主动 + mimic，供运动学写入（对齐 Isaac sdk_range_to_full_urdf_positions）。""" + return complete_g20_mimic_positions(sdk_range_to_urdf_positions(values)) + + +def open_hand_positions() -> dict[str, float]: + return sdk_range_to_full_urdf_positions(list(G20_OPEN_CMD)) + + +def half_fist_positions() -> dict[str, float]: + """半握：主动弯曲通道约中间值，侧摆保持张开。""" + cmd = list(G20_OPEN_CMD) + for i in (0, 1, 2, 3, 4, 15, 16, 17, 18, 19): + cmd[i] = 80.0 + return sdk_range_to_full_urdf_positions(cmd) diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.py index 652c296..57a0d35 100644 --- a/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.py +++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.py @@ -14,6 +14,15 @@ setup( (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), ('share/' + package_name, ['package.xml']), ], + package_data={ + package_name: [ + 'urdf/**/*.xml', + 'urdf/**/*.urdf', + 'urdf/**/*.STL', + 'urdf/**/*.stl', + ], + }, + include_package_data=True, install_requires=['setuptools'], zip_safe=True, maintainer='linkerhand', @@ -25,6 +34,7 @@ setup( 'console_scripts': [ 'linker_hand_mujoco_ros2_node=linker_hand_mujoco_ros2.linker_hand_mujoco_ros2:main', 'hand_curve_recorder=linker_hand_mujoco_ros2.hand_curve_recorder:main', + 'g20_plan_bridge=linker_hand_mujoco_ros2.g20_plan_bridge:main', ], }, )