Add G20 left-hand MuJoCo PLAN bridge, assets, and docs.
Bring in kinematic hard-write control with mesh-convex collision projection, SDK mapping aligned to Isaac, launch/run scripts, and implementation/tech writeups.
This commit is contained in:
@@ -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 `<position>` + `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 桥参数不同，联调时不要混用入口脚本。
|
||||
@@ -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)。
|
||||
@@ -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>`。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. `<contact><exclude>`（共 28 条）
|
||||
|
||||
用途：排除 **mesh/几何本就嵌套** 造成的假碰撞（张开姿态假重叠）。
|
||||
|
||||
- 掌心 `hand_base` ↔ 拇指 base1/base2/metacarpals、四指 metacarpals/proximal
|
||||
- 拇指链相邻 / 隔代相邻
|
||||
- 四指各自 metacarpals–proximal、proximal–middle、middle–distal
|
||||
|
||||
未 exclude 的对（如指尖↔掌心、邻指远端）仍可产生接触，供投影使用。
|
||||
|
||||
#### D. 执行器
|
||||
|
||||
- 为 **全部 21 个 hinge**（含 5 个 mimic 关节）各加一个 `<position name="*_pos" joint="...">`。
|
||||
- `thumb_cmc_roll` 的 `ctrlrange="0.0 1.47"`（与 URDF 上限一致）。
|
||||
- 默认硬写模式下主要靠写 `qpos`；`--pd-drive` 时才真正用这些执行器做 `mj_step`。
|
||||
|
||||
#### E. 命名差异
|
||||
|
||||
| URDF | MJCF |
|
||||
|------|------|
|
||||
| `<robot name="...">` | `<mujoco model="linker_hand_g20_left">` |
|
||||
| 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（每物理步瞬达） |
|
||||
@@ -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：优先匹配 `<joint>_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
|
||||
<geom class="visual" mesh="index_proximal"/> <!-- contype=0，不碰 -->
|
||||
<geom class="collision" type="mesh" mesh="index_proximal"/> <!-- 凸包碰撞 -->
|
||||
```
|
||||
|
||||
- **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
|
||||
<exclude body1="hand_base" body2="index_proximal"/>
|
||||
<exclude body1="index_proximal" body2="index_middle"/>
|
||||
```
|
||||
|
||||
张开手时这些对不应产生 `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)。
|
||||
Executable
+28
@@ -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 "$@"
|
||||
@@ -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",
|
||||
),
|
||||
])
|
||||
+677
@@ -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 "<joint>_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:
|
||||
"""绝对穿透判定（相对上一帧会漏掉每步 <tol 的缓慢钻入）。"""
|
||||
return self._worst_penetration_unlocked() < -PENETRATION_TOL_M
|
||||
|
||||
def _project_applied_no_penetration_unlocked(
|
||||
self, prev: dict[str, float]
|
||||
) -> 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())
|
||||
+228
@@ -0,0 +1,228 @@
|
||||
<mujoco model="linker_hand_g20_left">
|
||||
<compiler angle="radian" meshdir="meshes" balanceinertia="true"/>
|
||||
<option timestep="0.002" gravity="0 0 -9.81" integrator="implicitfast" />
|
||||
<default>
|
||||
<joint damping="0.15" armature="0.01" />
|
||||
<position kp="200" kv="8" forcelimited="true" forcerange="-8 8" />
|
||||
<!-- visual: 仅显示；collision: 同 mesh 的凸包（MuJoCo 默认），比 AABB 更贴视觉外形 -->
|
||||
<default class="visual">
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="2" rgba="1 1 1 1"/>
|
||||
</default>
|
||||
<default class="collision">
|
||||
<geom contype="1" conaffinity="1" condim="3" group="3" rgba="0 1 0 0"
|
||||
friction="1 0.5 0.01" solref="0.004 1" solimp="0.9 0.95 0.001" margin="0.0005"/>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
|
||||
<asset>
|
||||
<mesh name="hand_base_link" file="hand_base_link.STL"/>
|
||||
<mesh name="thumb_metacarpals_base1" file="thumb_metacarpals_base1.STL"/>
|
||||
<mesh name="thumb_metacarpals_base2" file="thumb_metacarpals_base2.STL"/>
|
||||
<mesh name="thumb_metacarpals" file="thumb_metacarpals.STL"/>
|
||||
<mesh name="thumb_proximal" file="thumb_proximal.STL"/>
|
||||
<mesh name="thumb_distal" file="thumb_distal.STL"/>
|
||||
<mesh name="index_metacarpals" file="index_metacarpals.STL"/>
|
||||
<mesh name="index_proximal" file="index_proximal.STL"/>
|
||||
<mesh name="index_middle" file="index_middle.STL"/>
|
||||
<mesh name="index_distal" file="index_distal.STL"/>
|
||||
<mesh name="middle_metacarpals" file="middle_metacarpals.STL"/>
|
||||
<mesh name="middle_proximal" file="middle_proximal.STL"/>
|
||||
<mesh name="middle_middle" file="middle_middle.STL"/>
|
||||
<mesh name="middle_distal" file="middle_distal.STL"/>
|
||||
<mesh name="ring_metacarpals" file="ring_metacarpals.STL"/>
|
||||
<mesh name="ring_proximal" file="ring_proximal.STL"/>
|
||||
<mesh name="ring_middle" file="ring_middle.STL"/>
|
||||
<mesh name="ring_distal" file="ring_distal.STL"/>
|
||||
<mesh name="pinky_metacarpals" file="pinky_metacarpals.STL"/>
|
||||
<mesh name="pinky_proximal" file="pinky_proximal.STL"/>
|
||||
<mesh name="pinky_middle" file="pinky_middle.STL"/>
|
||||
<mesh name="pinky_distal" file="pinky_distal.STL"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<body name="hand_base" pos="0 0 0">
|
||||
<geom class="visual" mesh="hand_base_link"/>
|
||||
<geom class="collision" type="mesh" mesh="hand_base_link"/>
|
||||
</body>
|
||||
<body name="thumb_metacarpals_base1" pos="0.01916 -0.01 0.0436">
|
||||
<inertial pos="0.0115155 0.00398069 0.000272888" quat="0.982721 -0.180506 -0.00245546 0.0408729" mass="0.0160352" diaginertia="4.05464e-06 2.4729e-06 2.02979e-06"/>
|
||||
<joint name="thumb_cmc_roll" pos="0 0 0" axis="-1 0 0" range="0 1.47" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="thumb_metacarpals_base1"/>
|
||||
<body name="thumb_metacarpals_base2" pos="0.019845 0 0" quat="0.926488 0.376324 0 0">
|
||||
<inertial pos="0.00286715 -0.00615727 0.00472605" quat="0.751727 0.570385 0.0649353 -0.32458" mass="0.00183265" diaginertia="1.63035e-07 1.63035e-07 1.63035e-07"/>
|
||||
<joint name="thumb_cmc_yaw" pos="0 0 0" axis="0 0 1" range="0 1.57" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="thumb_metacarpals_base2"/>
|
||||
<body name="thumb_metacarpals" pos="0.0088218 -0.024245 0.0005" quat="0.49419 0.415774 -0.291116 0.705804">
|
||||
<inertial pos="0.00670658 8.60759e-05 0.0331413" quat="0.705208 -0.00142811 -0.0031017 0.708992" mass="0.028716" diaginertia="1.35288e-05 1.13359e-05 3.37772e-06"/>
|
||||
<joint name="thumb_cmc_pitch" pos="0 0 0" axis="0 1 0" range="0 0.84" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="thumb_metacarpals"/>
|
||||
<geom class="collision" type="mesh" mesh="thumb_metacarpals"/>
|
||||
<body name="thumb_proximal" pos="0.0127018 0 0.0627256">
|
||||
<inertial pos="-0.00268734 0.000568245 0.0118502" quat="0.705679 -0.0691221 -0.0785141 0.700767" mass="0.00549016" diaginertia="6.2328e-07 4.89863e-07 2.23372e-07"/>
|
||||
<joint name="thumb_mcp" pos="0 0 0" axis="0 1 0" range="0 1.26" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="thumb_proximal"/>
|
||||
<geom class="collision" type="mesh" mesh="thumb_proximal"/>
|
||||
<body name="thumb_distal" pos="-0.00483652 0 0.0342226">
|
||||
<inertial pos="-0.000710022 -0.00267576 0.0124763" quat="0.561846 0.213938 0.139751 0.786785" mass="0.00382047" diaginertia="2.69897e-07 2.52035e-07 8.26387e-08"/>
|
||||
<joint name="thumb_ip" pos="0 0 0" axis="0 1 0" range="0 1.29" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="thumb_distal"/>
|
||||
<geom class="collision" type="mesh" mesh="thumb_distal"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="index_metacarpals" pos="0.0038 -0.03525 0.1481">
|
||||
<inertial pos="-0.007943 -2.93674e-07 0.00220106" quat="0.599922 0.599922 -0.374291 0.374291" mass="0.000895516" diaginertia="4.67031e-08 3.17351e-08 2.85435e-08"/>
|
||||
<joint name="index_mcp_roll" pos="0 0 0" axis="1 0 0" range="-0.23 0.23" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="index_metacarpals"/>
|
||||
<body name="index_proximal" pos="-0.0021302 0 0">
|
||||
<inertial pos="-0.00114185 3.47316e-05 0.0228928" quat="0.706728 0.0167051 0.0169055 0.707086" mass="0.00450921" diaginertia="6.74196e-07 6.67877e-07 1.34555e-07"/>
|
||||
<joint name="index_mcp_pitch" pos="0 0 0" axis="0 1 0" range="0 1.22" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="index_proximal"/>
|
||||
<geom class="collision" type="mesh" mesh="index_proximal"/>
|
||||
<body name="index_middle" pos="-0.0041946 0 0.044819">
|
||||
<inertial pos="-0.000206992 2.60617e-06 0.0167065" quat="0.99852 7.15582e-05 0.054381 -3.89717e-06" mass="0.00237827" diaginertia="2.31148e-07 1.76761e-07 9.46702e-08"/>
|
||||
<joint name="index_pip" pos="0 0 0" axis="0 1 0" range="0 1.74" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="index_middle"/>
|
||||
<geom class="collision" type="mesh" mesh="index_middle"/>
|
||||
<body name="index_distal" pos="0.0028245 0 0.031696">
|
||||
<inertial pos="-0.00816247 2.27059e-05 0.00811807" quat="0.942747 0.00109864 0.333492 0.00313565" mass="0.0033417" diaginertia="2.07631e-07 1.78094e-07 8.6082e-08"/>
|
||||
<joint name="index_dip" pos="0 0 0" axis="0 1 0" range="0 1.55" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="index_distal"/>
|
||||
<geom class="collision" type="mesh" mesh="index_distal"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="middle_metacarpals" pos="0.0038 -0.01175 0.1526">
|
||||
<inertial pos="-0.007943 -2.93007e-07 0.00220106" quat="0.599922 0.599922 -0.374291 0.374291" mass="0.000895517" diaginertia="4.67031e-08 3.17351e-08 2.85436e-08"/>
|
||||
<joint name="middle_mcp_roll" pos="0 0 0" axis="1 0 0" range="-0.23 0.23" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="middle_metacarpals"/>
|
||||
<body name="middle_proximal" pos="-0.0021302 0 0">
|
||||
<inertial pos="-0.00114185 3.47316e-05 0.0228928" quat="0.706728 0.0167051 0.0169055 0.707086" mass="0.00450921" diaginertia="6.74196e-07 6.67877e-07 1.34555e-07"/>
|
||||
<joint name="middle_mcp_pitch" pos="0 0 0" axis="0 1 0" range="0 1.22" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="middle_proximal"/>
|
||||
<geom class="collision" type="mesh" mesh="middle_proximal"/>
|
||||
<body name="middle_middle" pos="-0.00419458 0 0.0448194">
|
||||
<inertial pos="-0.000206992 2.60625e-06 0.0167065" quat="0.99852 7.15718e-05 0.054381 -3.89791e-06" mass="0.00237828" diaginertia="2.31148e-07 1.76761e-07 9.46702e-08"/>
|
||||
<joint name="middle_pip" pos="0 0 0" axis="0 1 0" range="0 1.74" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="middle_middle"/>
|
||||
<geom class="collision" type="mesh" mesh="middle_middle"/>
|
||||
<body name="middle_distal" pos="0.0028245 0 0.031696">
|
||||
<inertial pos="-0.00816247 2.27059e-05 0.00811807" quat="0.942747 0.00109865 0.333492 0.00313567" mass="0.0033417" diaginertia="2.07631e-07 1.78094e-07 8.6082e-08"/>
|
||||
<joint name="middle_dip" pos="0 0 0" axis="0 1 0" range="0 1.55" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="middle_distal"/>
|
||||
<geom class="collision" type="mesh" mesh="middle_distal"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="ring_metacarpals" pos="0.0038 0.01175 0.1481">
|
||||
<inertial pos="-0.007943 -2.94513e-07 0.00220106" quat="0.599922 0.599922 -0.374291 0.374291" mass="0.000895517" diaginertia="4.67031e-08 3.17351e-08 2.85436e-08"/>
|
||||
<joint name="ring_mcp_roll" pos="0 0 0" axis="1 0 0" range="-0.23 0.23" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="ring_metacarpals"/>
|
||||
<body name="ring_proximal" pos="-0.0021302 0 0">
|
||||
<inertial pos="-0.00114185 3.47316e-05 0.0228928" quat="0.706728 0.0167051 0.0169055 0.707086" mass="0.00450921" diaginertia="6.74196e-07 6.67877e-07 1.34555e-07"/>
|
||||
<joint name="ring_mcp_pitch" pos="0 0 0" axis="0 1 0" range="0 1.22" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="ring_proximal"/>
|
||||
<geom class="collision" type="mesh" mesh="ring_proximal"/>
|
||||
<body name="ring_middle" pos="-0.00419458 0 0.0448194">
|
||||
<inertial pos="-0.000206993 2.60747e-06 0.0167065" quat="0.99852 7.16159e-05 0.054381 -3.90032e-06" mass="0.00237827" diaginertia="2.31148e-07 1.76761e-07 9.46701e-08"/>
|
||||
<joint name="ring_pip" pos="0 0 0" axis="0 1 0" range="0 1.74" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="ring_middle"/>
|
||||
<geom class="collision" type="mesh" mesh="ring_middle"/>
|
||||
<body name="ring_distal" pos="0.0028245 0 0.031696">
|
||||
<inertial pos="-0.00816247 2.27057e-05 0.00811807" quat="0.942747 0.00109865 0.333492 0.00313572" mass="0.0033417" diaginertia="2.07631e-07 1.78094e-07 8.6082e-08"/>
|
||||
<joint name="ring_dip" pos="0 0 0" axis="0 1 0" range="0 1.55" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="ring_distal"/>
|
||||
<geom class="collision" type="mesh" mesh="ring_distal"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="pinky_metacarpals" pos="0.0038 0.03525 0.1436">
|
||||
<inertial pos="-0.007943 -2.93007e-07 0.00220106" quat="0.599922 0.599922 -0.374291 0.374291" mass="0.000895517" diaginertia="4.67031e-08 3.17351e-08 2.85436e-08"/>
|
||||
<joint name="pinky_mcp_roll" pos="0 0 0" axis="1 0 0" range="-0.23 0.23" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="pinky_metacarpals"/>
|
||||
<body name="pinky_proximal" pos="-0.0021302 0 0">
|
||||
<inertial pos="-0.00114185 3.47316e-05 0.0228928" quat="0.706728 0.0167051 0.0169055 0.707086" mass="0.00450921" diaginertia="6.74196e-07 6.67877e-07 1.34555e-07"/>
|
||||
<joint name="pinky_mcp_pitch" pos="0 0 0" axis="0 1 0" range="0 1.22" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="pinky_proximal"/>
|
||||
<geom class="collision" type="mesh" mesh="pinky_proximal"/>
|
||||
<body name="pinky_middle" pos="-0.0041946 0 0.044819">
|
||||
<inertial pos="-0.000206993 2.60749e-06 0.0167065" quat="0.99852 7.16152e-05 0.054381 -3.90028e-06" mass="0.00237827" diaginertia="2.31148e-07 1.76761e-07 9.46701e-08"/>
|
||||
<joint name="pinky_pip" pos="0 0 0" axis="0 1 0" range="0 1.74" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="pinky_middle"/>
|
||||
<geom class="collision" type="mesh" mesh="pinky_middle"/>
|
||||
<body name="pinky_distal" pos="0.0028245 0 0.031696">
|
||||
<inertial pos="-0.00816247 2.27054e-05 0.00811807" quat="0.942747 0.00109863 0.333492 0.00313569" mass="0.0033417" diaginertia="2.07631e-07 1.78094e-07 8.6082e-08"/>
|
||||
<joint name="pinky_dip" pos="0 0 0" axis="0 1 0" range="0 1.55" actuatorfrcrange="-100 100"/>
|
||||
<geom class="visual" mesh="pinky_distal"/>
|
||||
<geom class="collision" type="mesh" mesh="pinky_distal"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<contact>
|
||||
<!-- 掌心与各指根/近端：mesh 本就嵌套，排除假碰撞 -->
|
||||
<exclude body1="hand_base" body2="thumb_metacarpals_base1"/>
|
||||
<exclude body1="hand_base" body2="thumb_metacarpals_base2"/>
|
||||
<exclude body1="hand_base" body2="thumb_metacarpals"/>
|
||||
<exclude body1="hand_base" body2="index_metacarpals"/>
|
||||
<exclude body1="hand_base" body2="index_proximal"/>
|
||||
<exclude body1="hand_base" body2="middle_metacarpals"/>
|
||||
<exclude body1="hand_base" body2="middle_proximal"/>
|
||||
<exclude body1="hand_base" body2="ring_metacarpals"/>
|
||||
<exclude body1="hand_base" body2="ring_proximal"/>
|
||||
<exclude body1="hand_base" body2="pinky_metacarpals"/>
|
||||
<exclude body1="hand_base" body2="pinky_proximal"/>
|
||||
<!-- 拇指链相邻 / 隔代假重叠 -->
|
||||
<exclude body1="thumb_metacarpals_base1" body2="thumb_metacarpals_base2"/>
|
||||
<exclude body1="thumb_metacarpals_base1" body2="thumb_metacarpals"/>
|
||||
<exclude body1="thumb_metacarpals_base2" body2="thumb_metacarpals"/>
|
||||
<exclude body1="thumb_metacarpals" body2="thumb_proximal"/>
|
||||
<exclude body1="thumb_proximal" body2="thumb_distal"/>
|
||||
<!-- 食指链 -->
|
||||
<exclude body1="index_metacarpals" body2="index_proximal"/>
|
||||
<exclude body1="index_proximal" body2="index_middle"/>
|
||||
<exclude body1="index_middle" body2="index_distal"/>
|
||||
<!-- 中指链 -->
|
||||
<exclude body1="middle_metacarpals" body2="middle_proximal"/>
|
||||
<exclude body1="middle_proximal" body2="middle_middle"/>
|
||||
<exclude body1="middle_middle" body2="middle_distal"/>
|
||||
<!-- 无名指链 -->
|
||||
<exclude body1="ring_metacarpals" body2="ring_proximal"/>
|
||||
<exclude body1="ring_proximal" body2="ring_middle"/>
|
||||
<exclude body1="ring_middle" body2="ring_distal"/>
|
||||
<!-- 小指链 -->
|
||||
<exclude body1="pinky_metacarpals" body2="pinky_proximal"/>
|
||||
<exclude body1="pinky_proximal" body2="pinky_middle"/>
|
||||
<exclude body1="pinky_middle" body2="pinky_distal"/>
|
||||
</contact>
|
||||
<actuator>
|
||||
<position name="thumb_cmc_roll_pos" joint="thumb_cmc_roll" kp="200" kv="8" ctrlrange="0.0 1.47" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="thumb_cmc_yaw_pos" joint="thumb_cmc_yaw" kp="200" kv="8" ctrlrange="0.0 1.57" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="thumb_cmc_pitch_pos" joint="thumb_cmc_pitch" kp="200" kv="8" ctrlrange="0.0 0.84" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="thumb_mcp_pos" joint="thumb_mcp" kp="200" kv="8" ctrlrange="0.0 1.26" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="thumb_ip_pos" joint="thumb_ip" kp="200" kv="8" ctrlrange="0.0 1.29" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="index_mcp_roll_pos" joint="index_mcp_roll" kp="200" kv="8" ctrlrange="-0.23 0.23" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="index_mcp_pitch_pos" joint="index_mcp_pitch" kp="200" kv="8" ctrlrange="0.0 1.22" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="index_pip_pos" joint="index_pip" kp="200" kv="8" ctrlrange="0.0 1.74" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="index_dip_pos" joint="index_dip" kp="200" kv="8" ctrlrange="0.0 1.55" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="middle_mcp_roll_pos" joint="middle_mcp_roll" kp="200" kv="8" ctrlrange="-0.23 0.23" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="middle_mcp_pitch_pos" joint="middle_mcp_pitch" kp="200" kv="8" ctrlrange="0.0 1.22" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="middle_pip_pos" joint="middle_pip" kp="200" kv="8" ctrlrange="0.0 1.74" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="middle_dip_pos" joint="middle_dip" kp="200" kv="8" ctrlrange="0.0 1.55" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="ring_mcp_roll_pos" joint="ring_mcp_roll" kp="200" kv="8" ctrlrange="-0.23 0.23" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="ring_mcp_pitch_pos" joint="ring_mcp_pitch" kp="200" kv="8" ctrlrange="0.0 1.22" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="ring_pip_pos" joint="ring_pip" kp="200" kv="8" ctrlrange="0.0 1.74" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="ring_dip_pos" joint="ring_dip" kp="200" kv="8" ctrlrange="0.0 1.55" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="pinky_mcp_roll_pos" joint="pinky_mcp_roll" kp="200" kv="8" ctrlrange="-0.23 0.23" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="pinky_mcp_pitch_pos" joint="pinky_mcp_pitch" kp="200" kv="8" ctrlrange="0.0 1.22" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="pinky_pip_pos" joint="pinky_pip" kp="200" kv="8" ctrlrange="0.0 1.74" forcelimited="true" forcerange="-8 8" />
|
||||
<position name="pinky_dip_pos" joint="pinky_dip" kp="200" kv="8" ctrlrange="0.0 1.55" forcelimited="true" forcerange="-8 8" />
|
||||
</actuator>
|
||||
</mujoco>
|
||||
+1285
File diff suppressed because it is too large
Load Diff
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
+139
@@ -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)
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user