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

集成通用机器人数值接口、本机控制桥、LeRobot 插件和统一键盘遥操作。采用离线 CoACD 全臂碰撞配方 revision 4、局部装配区切分与结构自接触，限制直接关节位姿写入并保留安全看门狗。同步版本号、变更记录、来源许可证和兼容性验证。

examples/lekiwi/generate_full_collisions.py

@@ -0,0 +1,494 @@
+"""Offline collision-from-visuals cooking for the complete LeKiwi arm subtree.
+
+Run in build/venvs/collision, NOT the training or LeRobot environment. CoACD is
+never imported by the browser/bridge. Each convex part is a separate MJCF mesh.
+"""
+
+import argparse
+import hashlib
+import importlib.metadata
+import json
+import os
+import xml.etree.ElementTree as ET
+from pathlib import Path
+
+# Fix the numerical execution environment before importing numpy/CoACD.
+os.environ["OMP_NUM_THREADS"] = "4"
+os.environ["OPENBLAS_NUM_THREADS"] = "1"
+
+ROOT = Path(__file__).resolve().parents[2]
+CONFIG = ROOT / "robot_profiles/lekiwi-collision-source.json"
+VERSIONS = {"coacd": "1.0.14", "trimesh": "4.12.2", "numpy": "2.1.3", "scipy": "1.17.0"}
+PARAMETERS = {
+    "threshold": 0.003,
+    "real_metric": True,
+    "preprocess_mode": "auto",
+    "preprocess_resolution": 60,
+    "resolution": 1000,
+    "mcts_nodes": 12,
+    "mcts_iterations": 60,
+    "mcts_max_depth": 3,
+    "decimate": True,
+    "max_ch_vertex": 96,
+    "seed": 42,
+}
+
+
+def digest(data):
+    return hashlib.sha256(data).hexdigest()
+
+
+def visuals(source, config):
+    """Select every visual in the subtree, including welded motors/accessories."""
+    urdf = (source / "LeKiwi.urdf").read_bytes()
+    if digest(urdf) != config["urdfSha256"]:
+        raise ValueError("URDF 与固定版本不符")
+    robot = ET.fromstring(urdf)
+    parents = {
+        j.find("child").get("link"): j.find("parent").get("link") for j in robot.findall("joint")
+    }
+    result = []
+    for link in robot.findall("link"):
+        ancestor = link.get("name")
+        visited = set()
+        while ancestor in parents and ancestor != config["rootLink"]:
+            if ancestor in visited:
+                raise ValueError("URDF 运动链存在环")
+            visited.add(ancestor)
+            ancestor = parents[ancestor]
+        if ancestor != config["rootLink"]:
+            continue
+        for visual in link.findall("visual"):
+            mesh = visual.find("geometry/mesh")
+            if mesh is None:
+                raise ValueError("当前固定资产只支持 STL 视觉网格，不能静默遗漏几何")
+            filename = mesh.get("filename")
+            path = (source / filename).resolve()
+            if not path.is_relative_to(source.resolve()) or filename not in config["meshes"]:
+                raise ValueError(f"未批准的源网格：{filename}")
+            sha = digest(path.read_bytes())
+            if sha != config["meshes"][filename]:
+                raise ValueError(f"源网格 SHA-256 不匹配：{filename}")
+            scale = [float(x) for x in mesh.get("scale", "1 1 1").split()]
+            if len(scale) != 3 or not all(0 < x < float("inf") for x in scale):
+                raise ValueError("无效的视觉网格缩放")
+            result.append(
+                {
+                    "link": link.get("name"),
+                    "visual": visual.get("name"),
+                    "mesh": filename,
+                    "sha256": sha,
+                    "scale": scale,
+                }
+            )
+    if not result or {v["mesh"] for v in result} != set(config["meshes"]):
+        raise ValueError("整臂视觉覆盖清单不完整")
+    return result
+
+
+def swept_bounds(vertices, axis):
+    """Conservative axial/radial intervals for a full revolution, not pose sampling."""
+    import numpy as np
+    from scipy.spatial import ConvexHull
+
+    axial = vertices @ axis
+    reference = np.eye(3)[int(np.argmin(np.abs(axis)))]
+    x = np.cross(axis, reference)
+    x /= np.linalg.norm(x)
+    y = np.cross(axis, x)
+    projected = vertices @ np.column_stack([x, y])
+    hull = ConvexHull(projected)
+    radial_min = 0.0
+    if hull.equations[:, 2].max() > 0:
+        a = projected[hull.vertices]
+        b = np.roll(a, -1, axis=0)
+        edge = b - a
+        t = np.clip(-(a * edge).sum(axis=1) / (edge * edge).sum(axis=1), 0, 1)
+        radial_min = float(np.linalg.norm(a + t[:, None] * edge, axis=1).min())
+    return (
+        float(axial.min()),
+        float(axial.max()),
+        radial_min,
+        float(np.linalg.norm(projected, axis=1).max()),
+    )
+
+
+def intervals_overlap(a, b, margin=0.0001):
+    # 0.1 mm conservative allowance for serialized/compiled frame precision.
+    return (
+        max(a[0], b[0]) <= min(a[1], b[1]) + margin and max(a[2], b[2]) <= min(a[3], b[3]) + margin
+    )
+
+
+def assembly_frames(source, config, coverage):
+    """URDF joint/visual frames for both geometric partitioning and pair policy."""
+    import numpy as np
+    from scipy.spatial.transform import Rotation
+
+    robot = ET.parse(source / "LeKiwi.urdf").getroot()
+    joints = {j.get("name"): j for j in robot.findall("joint")}
+    parents = {j.find("child").get("link"): j for j in joints.values()}
+    links = {link.get("name"): link for link in robot.findall("link")}
+    poses = {}
+
+    def origin(node):
+        result = np.eye(4)
+        if node is not None:
+            result[:3, 3] = [float(x) for x in node.get("xyz", "0 0 0").split()]
+            result[:3, :3] = Rotation.from_euler(
+                "xyz", [float(x) for x in node.get("rpy", "0 0 0").split()]
+            ).as_matrix()
+        return result
+
+    def pose(link):
+        if link not in poses:
+            j = parents.get(link)
+            poses[link] = (
+                np.eye(4)
+                if j is None
+                else pose(j.find("parent").get("link")) @ origin(j.find("origin"))
+            )
+        return poses[link]
+
+    def owner(link):
+        if link == config["rootLink"]:
+            return "base"
+        j = parents[link]
+        return owner(j.find("parent").get("link")) if j.get("type") == "fixed" else j.get("name")
+
+    by_visual = {}
+    for item in coverage:
+        item["weldJoint"] = owner(item["link"])
+        visual = next(
+            v for v in links[item["link"]].findall("visual") if v.get("name") == item["visual"]
+        )
+        by_visual[item["visual"]] = (
+            item["weldJoint"],
+            pose(item["link"]) @ origin(visual.find("origin")),
+        )
+    result = []
+    for name, core in config["assemblyCores"].items():
+        if not (0 < core["radiusM"] <= 0.025 and 0 < core["axialHalfExtentM"] <= 0.06):
+            raise ValueError("装配配合区域超出已审核尺寸预算")
+        j = joints[name]
+        parent = owner(j.find("parent").get("link"))
+        transform = pose(j.find("child").get("link"))
+        axis = transform[:3, :3] @ np.array([float(x) for x in j.find("axis").get("xyz").split()])
+        axis /= np.linalg.norm(axis)
+        result.append((name, parent, transform, axis, core))
+    return by_visual, result
+
+
+def joint_policy(source, config, parts, coverage):
+    """Bound exceptions by joint-local geometry, never by a whole body pair."""
+    import numpy as np
+
+    by_visual, frames = assembly_frames(source, config, coverage)
+    result = []
+    for name, parent, transform, axis, core in frames:
+        members = []
+        groups = {parent: {}, name: {}}
+        for part in parts:
+            group, visual_pose = by_visual[part["visual"]]
+            if group not in (parent, name):
+                continue
+            vertices = np.fromstring(part["vertices"], sep=" ").reshape(-1, 3)
+            offsets = vertices @ visual_pose[:3, :3].T + visual_pose[:3, 3] - transform[:3, 3]
+            bounds = swept_bounds(offsets, axis)
+            groups[group][part["name"]] = bounds
+            if (
+                bounds[3] <= core["radiusM"]
+                and max(abs(bounds[0]), abs(bounds[1])) <= core["axialHalfExtentM"]
+            ):
+                members.append(part["name"])
+        core_set = set(members)
+        pairs = []
+        filtered = pruned = 0
+        for a, bound_a in groups[parent].items():
+            for b, bound_b in groups[name].items():
+                if a in core_set or b in core_set:
+                    filtered += 1
+                elif not intervals_overlap(bound_a, bound_b):
+                    pruned += 1
+                else:
+                    pairs.append([a, b])
+        if not pairs:
+            raise ValueError(f"不能关闭整对相邻连杆碰撞：{name}")
+        result.append(
+            {
+                "joint": name,
+                "parent": parent,
+                **core,
+                "coreParts": members,
+                "pairs": pairs,
+                "coreFilteredPairs": filtered,
+                "sweptPrunedPairs": pruned,
+            }
+        )
+    return result
+
+
+def clip_convex(vertices, normal, offset):
+    """Intersect a convex hull with a half-space, retaining all crossing edges."""
+    import numpy as np
+    from scipy.spatial import ConvexHull, QhullError
+
+    signed = vertices @ normal - offset
+    if signed.max() <= 1e-10:
+        return vertices
+    if signed.min() >= -1e-10:
+        return None
+    hull = ConvexHull(vertices)
+    result = list(vertices[signed <= 0])
+    for triangle in hull.simplices:
+        for i in range(3):
+            a, b = triangle[i], triangle[(i + 1) % 3]
+            if signed[a] * signed[b] < 0:
+                result.append(
+                    vertices[a] + (vertices[b] - vertices[a]) * signed[a] / (signed[a] - signed[b])
+                )
+    points = np.unique(np.asarray(result), axis=0)
+    if len(points) < 4:
+        return None
+    try:
+        # Remove edge/triangulation interpolation points BEFORE rounding;
+        # rounding collinear points first invents tiny zigzag facet vertices.
+        points = np.unique(np.round(points[ConvexHull(points).vertices], 6), axis=0)
+        if len(points) < 4:
+            return None
+        hull = ConvexHull(points)
+    except QhullError:
+        return None  # sub-micrometre cutting sliver collapsed by serialization
+    if hull.volume <= 1e-15:
+        return None
+    return points[hull.vertices]
+
+
+def split_core(vertices, planes):
+    """Partition, not delete: core and every outside fragment retain external contact."""
+    result = []
+    pending = vertices
+    for normal, offset in planes:
+        outside = clip_convex(pending, -normal, -offset)
+        if outside is not None:
+            result.append(outside)
+        pending = clip_convex(pending, normal, offset)
+        if pending is None:
+            return result
+    result.append(pending)
+    return result
+
+
+def partition_assembly_cores(source, config, parts, coverage):
+    """Split straddling hulls so a bearing contact cannot lock a structural hull.
+
+    A 16-sided inscribed prism stays inside each reviewed cylinder. The 2um
+    inset keeps rounding from reclassifying its core as structural. No region
+    is removed; only whole resulting core parts receive local mating exceptions.
+    """
+    import numpy as np
+    from scipy.spatial import ConvexHull
+
+    by_visual, frames = assembly_frames(source, config, coverage)
+    shapes = [
+        (part["visual"], np.fromstring(part["vertices"], sep=" ").reshape(-1, 3)) for part in parts
+    ]
+    for name, parent, transform, axis, core in frames:
+        x = np.cross(axis, np.eye(3)[int(np.argmin(np.abs(axis)))])
+        x /= np.linalg.norm(x)
+        y = np.cross(axis, x)
+        normals = [np.cos(t) * x + np.sin(t) * y for t in np.arange(16) * (2 * np.pi / 16)] + [
+            axis,
+            -axis,
+        ]
+        distances = [(core["radiusM"] - 0.000002) * np.cos(np.pi / 16)] * 16 + [
+            core["axialHalfExtentM"] - 0.000002
+        ] * 2
+        divided = []
+        for visual, vertices in shapes:
+            group, pose = by_visual[visual]
+            if group not in (parent, name):
+                divided.append((visual, vertices))
+                continue
+            origin = pose[:3, 3] - transform[:3, 3]
+            bounds = swept_bounds(vertices @ pose[:3, :3].T + origin, axis)
+            if (
+                (
+                    bounds[3] <= core["radiusM"]
+                    and max(abs(bounds[0]), abs(bounds[1])) <= core["axialHalfExtentM"]
+                )
+                or bounds[2] > core["radiusM"]
+                or bounds[0] > core["axialHalfExtentM"]
+                or bounds[1] < -core["axialHalfExtentM"]
+            ):
+                divided.append((visual, vertices))
+                continue
+            planes = [
+                (pose[:3, :3].T @ n, distance - n @ origin)
+                for n, distance in zip(normals, distances, strict=True)
+            ]
+            pieces = split_core(vertices, planes)
+            original = ConvexHull(vertices)
+            volume = sum(ConvexHull(piece).volume for piece in pieces)
+            if abs(volume - original.volume) > max(original.volume * 0.01, original.area * 0.00001):
+                raise ValueError(f"配合区切分体积误差超出微米量化预算：{visual}")
+            divided.extend((visual, piece) for piece in pieces)
+        shapes = divided
+    result = []
+    for item in coverage:
+        pieces = [v for visual, v in shapes if visual == item["visual"]]
+        pieces.sort(key=lambda v: tuple(v.mean(axis=0)))
+        item["coacdParts"] = item["parts"]
+        item["parts"] = len(pieces)
+        if not 1 <= len(pieces) <= 256:
+            raise ValueError("装配区域分割超出预算")
+        for i, vertices in enumerate(pieces):
+            if len(vertices) > 256:
+                raise ValueError(f"装配分割凸包顶点超出预算：{item['visual']} / {len(vertices)}")
+            result.append(
+                {
+                    "name": f"{item['link']}__h{i:03d}",
+                    "visual": item["visual"],
+                    "vertices": " ".join(
+                        f"{value:.6f}" for point in sorted(map(tuple, vertices)) for value in point
+                    ),
+                }
+            )
+    return result
+
+
+def cook(source, config, cache):
+    import coacd
+    import numpy as np
+    import trimesh
+    from scipy.spatial import ConvexHull
+
+    for name, version in VERSIONS.items():
+        if importlib.metadata.version(name) != version:
+            raise ValueError(f"请使用离线固定依赖：{name}=={version}")
+    coacd.set_log_level("info")
+    parts = []
+    coverage = []
+    for visual in visuals(source, config):
+        key = digest(
+            json.dumps(
+                {
+                    "source": visual["sha256"],
+                    "scale": visual["scale"],
+                    "parameters": PARAMETERS,
+                    "versions": VERSIONS,
+                    "threads": 4,
+                },
+                sort_keys=True,
+            ).encode()
+        )
+        cached = cache / f"{key}.json"
+        mesh = trimesh.load(source / visual["mesh"], force="mesh")
+        mesh.apply_scale(visual["scale"])
+        if cached.exists():
+            hulls = json.loads(cached.read_text())
+        else:
+            print(f"生成 {visual['mesh']}", flush=True)
+            raw = coacd.run_coacd(coacd.Mesh(mesh.vertices, mesh.faces), **PARAMETERS)
+            hulls = [sorted(np.round(v, 6).tolist()) for v, _ in raw]
+            hulls.sort(key=lambda vertices: tuple(np.mean(vertices, axis=0)))
+            cache.mkdir(parents=True, exist_ok=True)
+            temporary = cached.with_suffix(".tmp")
+            temporary.write_text(json.dumps(hulls))
+            temporary.replace(cached)
+        if not 1 <= len(hulls) <= 128:
+            raise ValueError(f"凸包数量超出预算：{visual['mesh']}")
+        # An explicit sampled coverage diagnostic, not a certified Hausdorff bound.
+        samples = np.concatenate([mesh.vertices, mesh.triangles_center])[::3]
+        outside = np.full(len(samples), np.inf)
+        for index, vertices in enumerate(hulls):
+            v = np.asarray(vertices)
+            if (
+                v.ndim != 2
+                or v.shape[1] != 3
+                or not 4 <= len(v) <= PARAMETERS["max_ch_vertex"]
+                or not np.isfinite(v).all()
+            ):
+                raise ValueError("凸包顶点损坏或超出预算")
+            if (v.min(axis=0) < mesh.bounds[0] - 0.002).any() or (
+                v.max(axis=0) > mesh.bounds[1] + 0.002
+            ).any():
+                raise ValueError("凸包超出源网格边界预算")
+            hull = ConvexHull(v)
+            if hull.volume <= 1e-15:
+                raise ValueError("退化凸包")
+            signed_planes = (samples @ hull.equations[:, :3].T + hull.equations[:, 3]).max(axis=1)
+            outside = np.minimum(outside, signed_planes)
+            parts.append(
+                {
+                    "name": f"{visual['link']}__h{index:03d}",
+                    "visual": visual["visual"],
+                    "vertices": " ".join(f"{x:.6f}" for x in v.ravel()),
+                }
+            )
+        if outside.max() > 0.0015:
+            raise ValueError(f"源表面采样覆盖不合格：{visual['mesh']}")
+        coverage.append(
+            {
+                **visual,
+                "parts": len(hulls),
+                "samples": len(samples),
+                "coacdSampleOutsidePlanesMaxM": float(max(0, outside.max())),
+                "sourceWatertight": bool(mesh.is_watertight),
+                "cacheKey": key,
+            }
+        )
+        print(
+            f"完成 {visual['link']}: {len(hulls)} hulls; sampled plane gap={outside.max():.6g} m",
+            flush=True,
+        )
+    parts = partition_assembly_cores(source, config, parts, coverage)
+    policy = joint_policy(source, config, parts, coverage)
+    geometry_sha = digest(json.dumps(parts, sort_keys=True, separators=(",", ":")).encode())
+    physics = {"timestep": 0.001, "solref": [0.002, 1], "solimp": [0.95, 0.99, 0.001]}
+    recipe_sha = digest(
+        json.dumps(
+            {"geometry": geometry_sha, "policy": policy, "physics": physics},
+            sort_keys=True,
+            separators=(",", ":"),
+        ).encode()
+    )
+    return {
+        "revision": 4,
+        "geometrySha256": geometry_sha,
+        "recipeSha256": recipe_sha,
+        "physics": physics,
+        "jointPolicy": policy,
+        "source": config,
+        "generator": {
+            "versions": VERSIONS,
+            "parameters": PARAMETERS,
+            "threads": 4,
+            "corePartition": {"sides": 16, "insetM": 0.000002},
+        },
+        "coverage": coverage,
+        "parts": parts,
+    }
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--source", type=Path, default=ROOT / "build/lekiwi/URDF")
+    parser.add_argument("--cache", type=Path, default=ROOT / "build/collision-cache")
+    parser.add_argument(
+        "--output", type=Path, default=ROOT / "robot_profiles/lekiwi-full-collision.json"
+    )
+    parser.add_argument("--check", action="store_true")
+    args = parser.parse_args()
+    result = cook(args.source, json.loads(CONFIG.read_text()), args.cache)
+    if args.check:
+        if json.loads(args.output.read_text()) != result:
+            raise ValueError("已生成碰撞数据与源文件/算法参数不一致")
+    else:
+        args.output.parent.mkdir(parents=True, exist_ok=True)
+        args.output.write_text(json.dumps(result, indent=2) + "\n")
+    print(f"整臂碰撞覆盖 {len(result['coverage'])} 个视觉网格 / {len(result['parts'])} 个凸包")
+
+
+if __name__ == "__main__":
+    main()