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