feat(lekiwi): release V0.10.1 初步集成 LeKiwi，优化碰撞模型

集成通用机器人数值接口、本机控制桥、LeRobot 插件和统一键盘遥操作。采用离线 CoACD 全臂碰撞配方 revision 4、局部装配区切分与结构自接触，限制直接关节位姿写入并保留安全看门狗。同步版本号、变更记录、来源许可证和兼容性验证。
2026-09-20 14:42:30 +08:00
parent da4d59c31a
commit 3ad29356c9
103 changed files with 23055 additions and 280 deletions
@@ -0,0 +1,117 @@
+"""Offline, deterministic convex parts from the pinned CAD (requires numpy/scipy).
+
+Runtime imports the checked-in JSON only; no extra browser/Python teleop dependency.
+Do not use one hull for the entire gripper: that would fill its open jaw space.
+"""
+
+import argparse
+import hashlib
+import json
+import struct
+from pathlib import Path
+
+import numpy as np
+from scipy.spatial import ConvexHull
+
+ROOT = Path(__file__).resolve().parents[2]
+# Cuts in the original STL's millimeters, with 1 mm overlap at each seam.
+# Each plane keeps sign * coordinate[axis] <= limit.
+PARTS = [
+    ("fixed_palm", "Wrist_Roll_08c-v1", [(2, 1, 39.0)]),
+    ("fixed_finger", "Wrist_Roll_08c-v1", [(2, -1, -38.0)]),
+    ("moving_hinge", "Moving_Jaw_08d-v1", [(1, -1, 22.0)]),
+    ("moving_finger", "Moving_Jaw_08d-v1", [(1, 1, -21.0), (1, -1, 60.0)]),
+    ("moving_tip", "Moving_Jaw_08d-v1", [(1, 1, -59.0)]),
+]
+# Bound to prepare_assets.py's pinned source; never silently regenerate for another CAD.
+SOURCE_HASHES = {
+    "Wrist_Roll_08c-v1.stl": "87507f73f485c2cacb3dc83924a712069fbd68f72b82d4a31a2cfe7f58c9e4c9",
+    "Moving_Jaw_08d-v1.stl": "71caabee267376791210950b3b4e2f7968d9b57b92ed3f57711c03f2b5666912",
+}
+
+
+def clip_polygon(polygon, axis, sign, limit):
+    result = []
+    for previous, current in zip(polygon[-1:] + polygon[:-1], polygon, strict=True):
+        a = sign * previous[axis] - limit
+        b = sign * current[axis] - limit
+        if (a <= 0) != (b <= 0):
+            result.append(previous + (current - previous) * (a / (a - b)))
+        if b <= 0:
+            result.append(current)
+    return result
+
+
+def generate(source, *, definitions=PARTS, source_hashes=SOURCE_HASHES, revision=2):
+    """Shared CAD-frame convex clipping; callers bind their own source hashes/cuts."""
+    parts = []
+    for name, mesh, planes in definitions:
+        raw = (source / (mesh + ".stl")).read_bytes()
+        digest = hashlib.sha256(raw).hexdigest()
+        if digest != source_hashes[mesh + ".stl"]:
+            raise ValueError(f"不支持的碰撞体 CAD：{mesh}.stl ({digest})")
+        count = struct.unpack_from("<I", raw, 80)[0]
+        if len(raw) != 84 + count * 50:
+            raise ValueError(f"无效的二进制 STL：{mesh}")
+        records = np.frombuffer(
+            raw,
+            offset=84,
+            dtype=np.dtype(
+                [
+                    ("normal", "<f4", (3,)),
+                    ("vertex", "<f4", (3, 3)),
+                    ("attribute", "<u2"),
+                ]
+            ),
+        )
+        points = []
+        for triangle in records["vertex"]:
+            polygon = [point.astype(float) for point in triangle]
+            for axis, sign, limit in planes:
+                if polygon:
+                    polygon = clip_polygon(polygon, axis, sign, limit)
+            points.extend(polygon)
+        # Clip triangles, not just vertices, so long faces crossing cuts leave no holes.
+        points = np.unique(np.round(points, 3), axis=0)  # 1 micrometer grid in CAD mm.
+        vertices = points[ConvexHull(points).vertices] / 1000  # MJCF meters.
+        vertices = vertices[np.lexsort(vertices.T[::-1])]
+        parts.append(
+            {
+                "name": name,
+                "visual": mesh + "_visual",
+                "vertices": " ".join(f"{value:.6f}" for value in vertices.flat),
+            }
+        )
+    return {
+        "revision": revision,
+        "source": {
+            "repository": "https://github.com/SIGRobotics-UIUC/LeKiwi",
+            "revision": "efa608d7ee5a495a4803b1d28cd0c955b4f1e033",
+            "license": "Apache-2.0",
+        },
+        "sources": source_hashes,
+        "parts": parts,
+    }
+
+
+def main():
+    parser = argparse.ArgumentParser(description="从固定版本 STL 重建 LeKiwi 分段夹爪碰撞体")
+    parser.add_argument("--source", type=Path, default=ROOT / "build/lekiwi/URDF/meshes")
+    parser.add_argument(
+        "--output", type=Path, default=ROOT / "robot_profiles/lekiwi-gripper-collision.json"
+    )
+    parser.add_argument("--check", action="store_true", help="只验证已签入的碰撞数据可重建")
+    args = parser.parse_args()
+    result = generate(args.source)
+    if args.check:
+        if json.loads(args.output.read_text()) != result:
+            raise ValueError("夹爪碰撞数据与固定 CAD/生成器不一致")
+    else:
+        args.output.parent.mkdir(parents=True, exist_ok=True)
+        args.output.write_text(json.dumps(result, indent=2) + "\n")
+    status = "验证通过" if args.check else args.output
+    print(f"LeKiwi 夹爪碰撞体：{len(result['parts'])} 个分段凸包，{status}")
+
+
+if __name__ == "__main__":
+    main()