DECISION: Gate LH20 delivery on reproducible geometry and export checks
DECISION: Require matching model hashes, explicit check scopes and retained failure evidence before packaging the digital prototype. WHY: The 2026-11-16 request covers both appearance and internals. A plausible render or successful API call cannot establish assembly geometry or manipulation behavior. The evidence policy is recorded in design-docs/lh20-tendonless-prototype.md; it follows the independent-module and bounded-control decisions in this series. ALTERNATIVE CONSIDERED AND REJECTED: Capsule collision checks alone missed local frame and cover interference. Ignoring those pairs was rejected; measured local reliefs are applied with topology/material-removal guards and then rechecked. The installed FreeCAD feature translator rejects some workplane operations, so the FCStd output is explicitly a BREP snapshot, with feature history retained in notebooks and the native package. WHAT CHANGES: Add independent OCP validity and exact interference inspection, deterministic opposition search, load/Jacobian/simulation checks, actual driven-finger comparison, source-CAD rendering, STEP/FreeCAD and URDF/MJCF exports, and a hash-indexed delivery ZIP. Exchange comparison uses matching fixed-order volume integration, preserves the native adaptive estimate, and independently checks boundary vertex locations. Generated assets are deliberately not committed. Checked: All six final whole-hand sampled external-interference checks and four standalone module checks pass. Independent driven-finger comparison covers 201 placements (maximum matrix error 2.75e-8). MuJoCo loads 20 positions, velocities and actuators and remains finite for 200 steps; finite-difference Jacobian and eight random FK configurations match. FreeCAD reimports 1011 valid solids; matching-method relative volume difference is 5.19e-6 and maximum bidirectional vertex distance is 4.16e-8 mm. Ruff undefined-name/syntax checks and compileall pass. One of 36 static force target cases remains over budget and is reported. Sampled checks, vertex agreement and short simulation do not certify full motion, whole surfaces, manufacturing tolerances or physical performance. Co-Authored-By: Codex <noreply@openai.com>
This commit is contained in:
@@ -0,0 +1,94 @@
|
||||
# LH20:以 WUJI Hand 2 为参考的无腱绳灵巧手
|
||||
|
||||
面向精细操作与柔顺抓取的原创参数化工程布局原型。五指、20 个独立转动轴;电机经过两级行星传动驱动关节,允许内部电线,没有腱绳/拉索传动。
|
||||
|
||||
**当前为 R0.1 数字原型,尚不能直接下单加工或宣称实现了柔顺抓取。** 已建立外形、承力框架、真实齿形、轴/轴孔、轴承、外盖与接触垫;电机、编码器和 PCB 的部分内容为明确标注的安装空间。检查覆盖展开、四组对指与一个中间状态,实际结果见工程记录;完整运动包络与结构强度仍需验证。
|
||||
|
||||
## 查看结果
|
||||
|
||||
- `out/lh20_right_hand_overview.png`:掌面、背面、内部传动布局,均由实际 CAD 渲染。
|
||||
- `out/joint_A_overview.png`:独立关节完整、拆壳和轴向分解图。
|
||||
- `out/lh20_right_hand_opposition_index_overview.png`:食指对指状态的 CAD 几何投影。
|
||||
- `out/lh20_right_hand.scadpkg`:SimpleCAD 原生装配包。
|
||||
- `out/joint_A.scadpkg`、`out/joint_B.scadpkg`、`out/joint_C.scadpkg`:可单独打开的三档关节原生模型。
|
||||
- `out/lh20_right_hand.step`:跨 CAD 软件的实体装配。
|
||||
- `out/lh20_right_hand.FCStd`:FreeCAD BREP 装配快照。特征历史保存在源笔记本和 `.scadpkg`;它不是完整翻译后的 FreeCAD 原生特征树。
|
||||
- `out/lh20.xml`、`out/lh20.urdf`、`out/meshes/`:20 轴 MuJoCo/URDF 模型,外观网格来自同一 CAD。
|
||||
- `out/engineering_review.md`:自动汇总的检查结果、质量估算和未闭合项。
|
||||
|
||||
生成物不提交到 Git。源文件、确定性种子与构建脚本均在本目录。
|
||||
|
||||
## 参数与模块
|
||||
|
||||
| 档位 | 数量 | 模块直径 × 轴向宽度 | 两级齿数:太阳/行星/齿圈 | 总减速比 |
|
||||
|---|---:|---:|---:|---:|
|
||||
| A | 6 | 24 × 22.2 mm | 12 / 24 / 60 | 36:1 |
|
||||
| B | 9 | 21 × 21.4 mm | 12 / 18 / 48 | 25:1 |
|
||||
| C | 5 | 18 × 20 mm | 18 / 18 / 54 | 16:1 |
|
||||
|
||||
25° 压力角、齿宽、模数、名义侧隙和三行星等分条件在 `parameters.py`、`parts/drivetrain.py` 中定义。非自锁齿轮构型有利于研究反驱,但没有测量摩擦和传动效率之前,不宣称已经具备合格的反驱性能。
|
||||
|
||||
`hand.py` → 五个 `finger.py` → 二十个 `joint_module.py` 构成装配层级。所有零件有独立身份、材料和命名连接坐标;外部 20 个主动轴与内部齿轮的从动运动分开描述。参数化零件按掌部、指节和传动分模块存放于 `parts/`。
|
||||
|
||||
内部齿轮角度按行星传动的 Willis 关系给定;这不是齿面接触动力学求解。标准轴承的装配身份按关节区分,允许各轴同时使用不同角度,避免多个从动定义共享同一身份却指向不同状态。
|
||||
|
||||
## 重建
|
||||
|
||||
以下命令从仓库根目录运行。依赖版本记录在 `requirements.txt`;当前已验证 macOS arm64 + Python 3.12 + SimpleCADAPI 2.3.0。
|
||||
|
||||
```sh
|
||||
uv venv --python 3.12 .venv
|
||||
uv pip install --python .venv/bin/python -r examples/lh20_hand/requirements.txt
|
||||
.venv/bin/python examples/lh20_hand/build.py hand --no-cache
|
||||
.venv/bin/python examples/lh20_hand/verify/opposition.py
|
||||
.venv/bin/python examples/lh20_hand/export_sim.py
|
||||
.venv/bin/python examples/lh20_hand/export_model.py --fcstd
|
||||
.venv/bin/python examples/lh20_hand/render.py
|
||||
```
|
||||
|
||||
`--fcstd` 使用本机 `/Applications/FreeCAD.app`;其他平台可省略该参数,直接导入 STEP。SimpleCADAPI 当前安装版本的 FreeCAD 原生图翻译器不支持本例的部分工作平面操作,故这里明确导出 BREP 快照。
|
||||
|
||||
编辑 `parameters.py` 后重新构建;`hand.py` 中的五组角度为度。可在项目编辑器中打开 `hand.py`,或生成某个实际受驱动装配姿态:
|
||||
|
||||
```sh
|
||||
.venv/bin/sca edit examples/lh20_hand/hand.py
|
||||
.venv/bin/python examples/lh20_hand/build.py hand --pose opposition_index --no-cache
|
||||
```
|
||||
|
||||
## 验证
|
||||
|
||||
```sh
|
||||
.venv/bin/python examples/lh20_hand/verify/geometry.py --collisions --external-only
|
||||
for pose in opposition_index opposition_middle opposition_ring opposition_little opposition_index@0.5; do
|
||||
.venv/bin/python examples/lh20_hand/verify/geometry.py --collisions --external-only --pose "$pose"
|
||||
done
|
||||
.venv/bin/python examples/lh20_hand/verify/engineering.py
|
||||
.venv/bin/python examples/lh20_hand/verify/pose_projection.py
|
||||
.venv/bin/python -m pytest examples/lh20_hand/verify/test_control.py -q
|
||||
for size in A B C; do
|
||||
.venv/bin/python examples/lh20_hand/build.py joint --size "$size" --no-cache
|
||||
.venv/bin/python examples/lh20_hand/verify/geometry.py --name "joint_$size" --collisions
|
||||
done
|
||||
.venv/bin/python examples/lh20_hand/build.py joint --size A --angle 17.3 --no-cache
|
||||
.venv/bin/python examples/lh20_hand/verify/geometry.py --name joint_A_17p3 --collisions
|
||||
.venv/bin/python examples/lh20_hand/render.py --name joint_A
|
||||
.venv/bin/python examples/lh20_hand/render.py --pose opposition_index
|
||||
.venv/bin/python examples/lh20_hand/release.py
|
||||
```
|
||||
|
||||
`release.py` 检查原生包、检查记录、导出文件和图像摘要是否对应,满足检查条件后生成 `out/LH20_R01_delivery.zip`。上面的完整交付流程包含本机 FreeCAD 快照导出;已有虚拟环境时无需再次运行 `uv venv`。
|
||||
|
||||
`--external-only` 检查零件与其他模块之间的干涉;每类模块内部另用 `--name joint_A/B/C --collisions` 检查。包络加速前先用 BREP 布尔差验证所有模块零件确实在包络内。未执行的检查不会标记为通过;发现干涉时程序以非零状态退出并保留零件对和重叠体积。
|
||||
|
||||
额外检查包括:A 档 17.3° 实际装配角度下的齿轮干涉、CAD 与独立运动学坐标一致性、雅可比有限差分、MuJoCo 20 轴加载与短时数值运行。它们均不代替实物测试或全运动空间验证。
|
||||
|
||||
碰撞修订数据位于 `clearance_reliefs.json`。`verify/design_reliefs.py` 将已测得的局部碰撞转换为可审阅的避让特征,并更新参数摘要以使缓存失效;修订后仍必须重建并重新运行独立检查。构建会拒绝过大的材料去除或结构分裂。
|
||||
|
||||
## 接下来必须闭合的工程项目
|
||||
|
||||
1. 完善指根叉架与外盖的连续运动包络,验证整段路径及极限组合,完成最小壁厚、刚度/应力分析。
|
||||
2. 确认三档定制电机、电流与热性能;完成齿轮寿命、背隙、支承刚度、轴承寿命和轴向固定。
|
||||
3. 完成紧固件、压配/滑配公差、实际电线或 FPC 回环、驱动和触觉电路。
|
||||
4. 先制作单关节与单指台架,再扩展整手。测试堵转温升、反驱力矩、接触力标定、循环寿命与控制稳定性。
|
||||
|
||||
设计依据及决策见 `DESIGN.md`;控制与布线方案见 `CONTROL.md`;供应商电机输入条件见 `MOTOR_RFQ.md`。参考 WUJI 的构型与开放开发思路,不把未公开的内部结构当成已知,也不把本模型称为 WUJI 复刻品。
|
||||
@@ -0,0 +1,127 @@
|
||||
"""Export the canonical, validated package; geometry source remains the notebooks."""
|
||||
|
||||
import argparse, json, subprocess, hashlib
|
||||
from parameters import OUT
|
||||
from simplecadapi.exporter.step import export_product_package_to_step
|
||||
|
||||
|
||||
def source_metrics(leaves):
|
||||
"""Use the same fixed-order integral as FreeCAD and retain native estimates.
|
||||
|
||||
Comparing FreeCAD's fixed-order integral with the native SDK's adaptive
|
||||
integral conflates exchange error with quadrature on trimmed loft surfaces.
|
||||
Independent vertex-location checks supplement volume and solid counts.
|
||||
"""
|
||||
import numpy as np
|
||||
from simplecadapi.verify import to_ocp
|
||||
from OCP.BRepGProp import BRepGProp
|
||||
from OCP.GProp import GProp_GProps
|
||||
from OCP.BRep import BRep_Tool
|
||||
from OCP.TopExp import TopExp_Explorer
|
||||
from OCP.TopAbs import TopAbs_VERTEX
|
||||
from OCP.TopoDS import TopoDS
|
||||
|
||||
unique = {p.part_id: p for _, p, _ in leaves}
|
||||
volumes, native, points = {}, {}, {}
|
||||
for key, part in unique.items():
|
||||
shape = to_ocp(part.body)
|
||||
props = GProp_GProps()
|
||||
BRepGProp.VolumeProperties_s(shape, props)
|
||||
volumes[key] = props.Mass()
|
||||
native[key] = part.body.get_volume()
|
||||
explorer = TopExp_Explorer(shape, TopAbs_VERTEX)
|
||||
vertices = []
|
||||
while explorer.More():
|
||||
p = BRep_Tool.Pnt_s(TopoDS.Vertex_s(explorer.Current()))
|
||||
vertices.append((p.X(), p.Y(), p.Z()))
|
||||
explorer.Next()
|
||||
points[key] = np.unique(np.array(vertices), axis=0)
|
||||
return (
|
||||
sum(volumes[p.part_id] for _, p, _ in leaves),
|
||||
sum(native[p.part_id] for _, p, _ in leaves),
|
||||
np.concatenate(
|
||||
[points[p.part_id] @ m[:3, :3].T + m[:3, 3] for _, p, m in leaves]
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def main(name, fcstd):
|
||||
package = OUT / f"{name}.scadpkg"
|
||||
report = export_product_package_to_step(
|
||||
data=package, output_path=OUT / f"{name}.step"
|
||||
)
|
||||
print(
|
||||
"STEP", report.output_path, "occurrences", report.occurrence_count, flush=True
|
||||
)
|
||||
if fcstd:
|
||||
from model_io import read_model, flatten
|
||||
import numpy as np
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
leaves = flatten(read_model(name))
|
||||
source_volume, native_volume, vertices = source_metrics(leaves)
|
||||
# The installed 2.3.0 native-graph translator rejects workplane/sketch
|
||||
# operations. Preserve the STEP assembly as a BREP inspection document;
|
||||
# editable feature history remains in the notebook and .scadpkg.
|
||||
script = OUT / "import_freecad.py"
|
||||
vertex_path = OUT / "freecad_exchange_vertices.json"
|
||||
script.write_text(
|
||||
"import sys,json\n"
|
||||
"sys.path.insert(0,'/Applications/FreeCAD.app/Contents/Resources/lib')\n"
|
||||
"import FreeCAD,Part,Import\n"
|
||||
f"doc=FreeCAD.newDocument({name!r})\n"
|
||||
f"Import.insert({str(OUT / f'{name}.step')!r},doc.Name)\n"
|
||||
"doc.recompute()\n"
|
||||
f"doc.saveAs({str(OUT / f'{name}.FCStd')!r})\n"
|
||||
'features=[o for o in doc.Objects if o.TypeId=="Part::Feature" and not o.Shape.isNull()]\n'
|
||||
'roots=[o for o in doc.RootObjects if hasattr(o,"Shape") and not o.Shape.isNull()]\n'
|
||||
"solid_count=sum(len(o.Shape.Solids) for o in roots)\n"
|
||||
"volume=sum(o.Shape.Volume for o in roots)\n"
|
||||
"vertices=[[v.Point.x,v.Point.y,v.Point.z] for o in roots for v in o.Shape.Vertexes]\n"
|
||||
f"open({str(vertex_path)!r},'w').write(json.dumps(vertices))\n"
|
||||
f"error=abs(volume-{source_volume!r})/max(1.,{source_volume!r})\n"
|
||||
f'facts=dict(format="BREP assembly snapshot; source feature history remains in .scadpkg",solid_features=len(features),root_solids=solid_count,source_occurrences={len(leaves)},volume_mm3=volume,source_volume_mm3={source_volume!r},relative_volume_error=error,valid=all(o.Shape.isValid() for o in features))\n'
|
||||
f'open({str(OUT / f"{name}_freecad_check.json")!r},"w").write(json.dumps(facts,indent=2))\n'
|
||||
f'assert facts["valid"] and solid_count=={len(leaves)} and error<1e-5, facts\n'
|
||||
'print("FreeCAD snapshot saved",len(features),flush=True)\n'
|
||||
)
|
||||
subprocess.run(
|
||||
["/Applications/FreeCAD.app/Contents/Resources/bin/python", str(script)],
|
||||
check=True,
|
||||
)
|
||||
check_path = OUT / f"{name}_freecad_check.json"
|
||||
facts = json.loads(check_path.read_text())
|
||||
imported_vertices = np.array(json.loads(vertex_path.read_text()))
|
||||
vertex_error = max(
|
||||
cKDTree(imported_vertices).query(vertices)[0].max(),
|
||||
cKDTree(vertices).query(imported_vertices)[0].max(),
|
||||
)
|
||||
facts.update(
|
||||
volume_method="Both sides use OCCT fixed-order integration; native adaptive estimate recorded separately",
|
||||
source_native_adaptive_volume_mm3=native_volume,
|
||||
native_adaptive_vs_freecad_fixed_relative_difference=abs(
|
||||
facts["volume_mm3"] - native_volume
|
||||
)
|
||||
/ native_volume,
|
||||
maximum_bidirectional_vertex_distance_mm=float(vertex_error),
|
||||
vertex_check_scope="All boundary vertices, not a full surface-distance proof",
|
||||
)
|
||||
facts["canonical_package_sha256"] = hashlib.sha256(
|
||||
package.read_bytes()
|
||||
).hexdigest()
|
||||
for suffix in ("step", "FCStd"):
|
||||
facts[f"{suffix}_sha256"] = hashlib.sha256(
|
||||
(OUT / f"{name}.{suffix}").read_bytes()
|
||||
).hexdigest()
|
||||
check_path.write_text(json.dumps(facts, indent=2))
|
||||
assert vertex_error < 1e-4, facts
|
||||
vertex_path.unlink()
|
||||
print("FCSTD BREP inspection snapshot", OUT / f"{name}.FCStd", flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--name", default="lh20_right_hand")
|
||||
ap.add_argument("--fcstd", action="store_true")
|
||||
a = ap.parse_args()
|
||||
main(a.name, a.fcstd)
|
||||
@@ -0,0 +1,306 @@
|
||||
"""20-axis URDF/MuJoCo model with visuals derived from the same CAD package.
|
||||
|
||||
This is a kinematic/control scaffold. The reduced collision shapes, inertia,
|
||||
friction and motor masses are assumptions, not measured actuator properties.
|
||||
"""
|
||||
|
||||
import json, re, hashlib
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
from simplecadapi.exporter._surface_mesh import tessellate_solid
|
||||
from parameters import FINGERS, SIZES, OUT
|
||||
from kinematics import finger_frames, AXES, transform
|
||||
from model_io import read_model, flatten
|
||||
|
||||
|
||||
def text(a):
|
||||
return " ".join(f"{v:.9g}" for v in a)
|
||||
|
||||
|
||||
def sub(parent, tag, **attrs):
|
||||
return ET.SubElement(parent, tag, {k: str(v) for k, v in attrs.items()})
|
||||
|
||||
|
||||
def quat(r):
|
||||
x, y, z, w = Rotation.from_matrix(r).as_quat()
|
||||
return text([w, x, y, z])
|
||||
|
||||
|
||||
def save(root, path):
|
||||
ET.indent(root, space=" ")
|
||||
ET.ElementTree(root).write(path, encoding="utf-8", xml_declaration=True)
|
||||
|
||||
|
||||
def export_meshes():
|
||||
leaves = flatten(read_model())
|
||||
frames = {"palm": np.eye(4)}
|
||||
for f in FINGERS:
|
||||
for k, fr in enumerate(finger_frames(f)[0]):
|
||||
frames[f"{f.name}_{k}"] = fr["moving"]
|
||||
meshdir = OUT / "meshes"
|
||||
meshdir.mkdir(exist_ok=True)
|
||||
groups = {}
|
||||
cache = {}
|
||||
for path, p, m in leaves:
|
||||
tokens = path.strip("/").split("/")
|
||||
link = "palm"
|
||||
if tokens[0] in [f.name for f in FINGERS]:
|
||||
finger = tokens[0]
|
||||
cid = tokens[1]
|
||||
k = int(re.search(r"\d+", cid).group())
|
||||
owner = (
|
||||
k
|
||||
if not cid.startswith("joint") or tokens[-1] == "output_flange"
|
||||
else k - 1
|
||||
)
|
||||
link = f"{finger}_{owner}" if owner >= 0 else "palm"
|
||||
mat = p.material.material_id if p.material else "aluminium"
|
||||
key = (link, mat)
|
||||
if p.part_id not in cache:
|
||||
cache[p.part_id] = tessellate_solid(
|
||||
p.body,
|
||||
linear_deflection=0.25,
|
||||
angular_deflection_degrees=22,
|
||||
relative=False,
|
||||
)
|
||||
v, t = cache[p.part_id]
|
||||
matrix = np.linalg.inv(frames[link]) @ m
|
||||
v = (np.asarray(v) @ matrix[:3, :3].T + matrix[:3, 3]) / 1000
|
||||
groups.setdefault(key, []).append((v, np.asarray(t)))
|
||||
result = {k: [] for k in frames}
|
||||
for (link, mat), blocks in groups.items():
|
||||
name = f"{link}_{mat}"
|
||||
lines = []
|
||||
offset = 0
|
||||
for v, t in blocks:
|
||||
lines.extend("v " + text(p) for p in v)
|
||||
lines.extend("f " + text(tri + offset + 1) for tri in t)
|
||||
offset += len(v)
|
||||
(meshdir / f"{name}.obj").write_text("\n".join(lines) + "\n")
|
||||
result[link].append((name, mat))
|
||||
return result
|
||||
|
||||
|
||||
def main():
|
||||
from parameters import MATERIALS
|
||||
|
||||
meshes = export_meshes()
|
||||
urdf = ET.Element("robot", name="LH20_R01")
|
||||
mj = ET.Element("mujoco", model="LH20_R01")
|
||||
sub(mj, "compiler", angle="radian", meshdir="meshes", inertiafromgeom="false")
|
||||
sub(mj, "option", timestep=".001", gravity="0 0 -9.81", integrator="implicitfast")
|
||||
asset = sub(mj, "asset")
|
||||
world = sub(mj, "worldbody")
|
||||
act = sub(mj, "actuator")
|
||||
exclude = sub(mj, "contact")
|
||||
for mat, (_, color) in MATERIALS.items():
|
||||
sub(asset, "material", name=mat, rgba=text([*color, 1]))
|
||||
ma = sub(urdf, "material", name=mat)
|
||||
sub(ma, "color", rgba=text([*color, 1]))
|
||||
for entries in meshes.values():
|
||||
for name, mat in entries:
|
||||
sub(asset, "mesh", name=name, file=f"{name}.obj")
|
||||
palm = sub(world, "body", name="palm")
|
||||
sub(
|
||||
palm, "inertial", pos="0 .035 0", mass=".30", diaginertia=".00025 .00018 .00030"
|
||||
)
|
||||
|
||||
def visuals(ulink, body, name):
|
||||
for mesh, mat in meshes[name]:
|
||||
vis = sub(ulink, "visual")
|
||||
geom = sub(vis, "geometry")
|
||||
sub(geom, "mesh", filename=f"meshes/{mesh}.obj")
|
||||
sub(vis, "material", name=mat)
|
||||
sub(
|
||||
body,
|
||||
"geom",
|
||||
name=mesh,
|
||||
type="mesh",
|
||||
mesh=mesh,
|
||||
material=mat,
|
||||
contype="0",
|
||||
conaffinity="0",
|
||||
group="1",
|
||||
)
|
||||
|
||||
pl = sub(urdf, "link", name="palm")
|
||||
visuals(pl, palm, "palm")
|
||||
inert = sub(pl, "inertial")
|
||||
sub(inert, "origin", xyz="0 .035 0")
|
||||
sub(inert, "mass", value=".30")
|
||||
sub(
|
||||
inert,
|
||||
"inertia",
|
||||
ixx=".00025",
|
||||
iyy=".00018",
|
||||
izz=".00030",
|
||||
ixy="0",
|
||||
ixz="0",
|
||||
iyz="0",
|
||||
)
|
||||
pcol = sub(pl, "collision")
|
||||
sub(pcol, "origin", xyz="0 .035 0")
|
||||
sub(sub(pcol, "geometry"), "box", size=".09 .085 .03")
|
||||
sub(
|
||||
palm,
|
||||
"geom",
|
||||
name="palm_collision",
|
||||
type="box",
|
||||
pos="0 .035 0",
|
||||
size=".045 .0425 .015",
|
||||
group="3",
|
||||
rgba=".5 .7 .9 .2",
|
||||
)
|
||||
metadata = []
|
||||
for f in FINGERS:
|
||||
parent = palm
|
||||
parent_name = "palm"
|
||||
for k in range(4):
|
||||
name = f"{f.name}_{k}"
|
||||
joint = f"{name}_joint"
|
||||
size = SIZES[f.sizes[k]]
|
||||
length = f.lengths[k] / 1000
|
||||
pre = Rotation.from_euler(
|
||||
"xyz", f.pre_rotations[k], degrees=True
|
||||
).as_matrix()
|
||||
if k == 0:
|
||||
origin = np.array(f.base) / 1000
|
||||
rot = (
|
||||
Rotation.from_euler("xyz", f.base_rpy, degrees=True).as_matrix()
|
||||
@ pre
|
||||
)
|
||||
else:
|
||||
origin = np.array([0, f.lengths[k - 1] / 1000, 0])
|
||||
rot = pre
|
||||
body = sub(parent, "body", name=name, pos=text(origin), quat=quat(rot))
|
||||
axis = AXES[f.axes[k]]
|
||||
lo, hi = np.deg2rad(f.limits[k])
|
||||
sub(
|
||||
body,
|
||||
"joint",
|
||||
name=joint,
|
||||
type="hinge",
|
||||
axis=text(axis),
|
||||
range=text([lo, hi]),
|
||||
damping=".015",
|
||||
frictionloss=".005",
|
||||
armature=".000001",
|
||||
)
|
||||
# A transparent, explicit seed inertia; replace with measured values.
|
||||
mass = 0.025 if k < 3 else 0.015
|
||||
radius = 0.012
|
||||
inertia = (
|
||||
mass * (3 * radius**2 + length**2) / 12,
|
||||
mass * radius**2 / 2,
|
||||
mass * (3 * radius**2 + length**2) / 12,
|
||||
)
|
||||
sub(
|
||||
body,
|
||||
"inertial",
|
||||
pos=text([0, length / 2, 0]),
|
||||
mass=mass,
|
||||
diaginertia=text(inertia),
|
||||
)
|
||||
ul = sub(urdf, "link", name=name)
|
||||
visuals(ul, body, name)
|
||||
inert = sub(ul, "inertial")
|
||||
sub(inert, "origin", xyz=text([0, length / 2, 0]))
|
||||
sub(inert, "mass", value=mass)
|
||||
sub(
|
||||
inert,
|
||||
"inertia",
|
||||
ixx=inertia[0],
|
||||
iyy=inertia[1],
|
||||
izz=inertia[2],
|
||||
ixy="0",
|
||||
ixz="0",
|
||||
iyz="0",
|
||||
)
|
||||
# Collision capsules are coarse envelopes, not detailed CAD contact.
|
||||
sub(
|
||||
body,
|
||||
"geom",
|
||||
name=f"{name}_collision",
|
||||
type="capsule",
|
||||
fromto=text([0, 0.010, 0, 0, max(0.011, length - 0.005), 0]),
|
||||
size=".010",
|
||||
group="3",
|
||||
rgba=".5 .7 .9 .2",
|
||||
)
|
||||
col = sub(ul, "collision")
|
||||
sub(col, "origin", xyz=text([0, length / 2, 0]))
|
||||
sub(sub(col, "geometry"), "box", size=text([0.022, length, 0.022]))
|
||||
uj = sub(urdf, "joint", name=joint, type="revolute")
|
||||
sub(uj, "parent", link=parent_name)
|
||||
sub(uj, "child", link=name)
|
||||
sub(
|
||||
uj,
|
||||
"origin",
|
||||
xyz=text(origin),
|
||||
rpy=text(Rotation.from_matrix(rot).as_euler("xyz")),
|
||||
)
|
||||
sub(uj, "axis", xyz=text(axis))
|
||||
sub(
|
||||
uj, "limit", lower=lo, upper=hi, effort=size.peak_target, velocity="2.0"
|
||||
)
|
||||
sub(uj, "dynamics", damping=".015", friction=".005")
|
||||
sub(
|
||||
act,
|
||||
"motor",
|
||||
name=f"{name}_torque",
|
||||
joint=joint,
|
||||
gear="1",
|
||||
ctrllimited="true",
|
||||
ctrlrange=text([-size.peak_target, size.peak_target]),
|
||||
)
|
||||
sub(exclude, "exclude", body1=parent_name, body2=name)
|
||||
metadata.append(
|
||||
dict(
|
||||
joint=joint,
|
||||
size=f.sizes[k],
|
||||
ratio=size.ratio,
|
||||
mass_seed_kg=mass,
|
||||
peak_torque_target_Nm=size.peak_target,
|
||||
)
|
||||
)
|
||||
parent = body
|
||||
parent_name = name
|
||||
keys = sub(mj, "keyframe")
|
||||
sub(keys, "key", name="open", qpos=text(np.zeros(20)))
|
||||
opposition = json.loads((OUT / "opposition.json").read_text())
|
||||
for row in opposition:
|
||||
pose = np.zeros(20)
|
||||
i = [f.name for f in FINGERS].index(row["finger"])
|
||||
pose[i * 4 : i * 4 + 4] = np.deg2rad(row["finger_deg"])
|
||||
pose[16:20] = np.deg2rad(row["thumb_deg"])
|
||||
sub(keys, "key", name="opposition_" + row["finger"], qpos=text(pose))
|
||||
save(urdf, OUT / "lh20.urdf")
|
||||
save(mj, OUT / "lh20.xml")
|
||||
(OUT / "simulation_assumptions.json").write_text(
|
||||
json.dumps(
|
||||
{
|
||||
"canonical_package_sha256": hashlib.sha256(
|
||||
(OUT / "lh20_right_hand.scadpkg").read_bytes()
|
||||
).hexdigest(),
|
||||
"status": "kinematic/control scaffold, not validated dynamics",
|
||||
"units": "SI",
|
||||
"mesh_origin": "actual LH20 canonical CAD package; gears held rigid within each link in reduced model",
|
||||
"mass_inertia": "seed values, not CAD mass estimate or measured mass",
|
||||
"collision": "coarse capsules/boxes; adjacent links excluded; detailed interference is checked separately with OCP",
|
||||
"friction_armature": "uncalibrated seed values",
|
||||
"torque": "design targets, not supplier ratings",
|
||||
"joints": metadata,
|
||||
},
|
||||
indent=2,
|
||||
)
|
||||
)
|
||||
print(
|
||||
"Exported LH20 URDF, MJCF, CAD visual meshes and explicit assumption record",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,221 @@
|
||||
"""Build an evidence-indexed handoff from the latest, matching CAD package."""
|
||||
|
||||
from pathlib import Path
|
||||
from collections import Counter
|
||||
import hashlib, json, zipfile
|
||||
from parameters import HERE, OUT, FINGERS, SIZES
|
||||
from model_io import read_model, flatten
|
||||
|
||||
|
||||
def read(name):
|
||||
return json.loads((OUT / name).read_text())
|
||||
|
||||
|
||||
def sha(path):
|
||||
return hashlib.sha256(path.read_bytes()).hexdigest()
|
||||
|
||||
|
||||
def main():
|
||||
package = OUT / "lh20_right_hand.scadpkg"
|
||||
digest = sha(package)
|
||||
neutral = read("lh20_right_hand_assembly_clearance.json")
|
||||
assert neutral["package_sha256"] == digest, "Neutral audit is stale"
|
||||
assert (
|
||||
neutral["geometry_pass"]
|
||||
and neutral["constraints_pass"]
|
||||
and neutral["interference_pass"]
|
||||
), "Neutral assembly must pass before packaging"
|
||||
leaves = flatten(read_model())
|
||||
counts = Counter(p.part_id for _, p, _ in leaves)
|
||||
parts = {p.part_id: p for _, p, _ in leaves}
|
||||
facts = {row["part"]: row for row in neutral["geometry"]}
|
||||
bom = []
|
||||
for pid, p in sorted(parts.items()):
|
||||
density = p.material.density if p.material else None
|
||||
v = facts[pid]["volume_mm3"]
|
||||
count = counts[pid]
|
||||
bom.append(
|
||||
dict(
|
||||
part_id=pid,
|
||||
label=p.name,
|
||||
instances=count,
|
||||
material=p.material.material_id if p.material else None,
|
||||
volume_each_mm3=v,
|
||||
density_kg_m3=density,
|
||||
total_model_mass_g=None
|
||||
if density is None
|
||||
else count * v * density / 1e6,
|
||||
selection_status="reserved space / unselected"
|
||||
if density is None
|
||||
else "modeled geometry; manufacturing specification pending",
|
||||
)
|
||||
)
|
||||
(OUT / "cad_bom.json").write_text(json.dumps(bom, indent=2, ensure_ascii=False))
|
||||
engineering = read("engineering_checks.json")
|
||||
projection = read("pose_projection_check.json")
|
||||
fcstd = read("lh20_right_hand_freecad_check.json")
|
||||
assert (
|
||||
engineering["canonical_package_sha256"] == digest
|
||||
and projection["canonical_package_sha256"] == digest
|
||||
), "Engineering evidence is stale"
|
||||
assert engineering["simulation_xml_sha256"] == sha(OUT / "lh20.xml"), (
|
||||
"Simulation evidence is stale"
|
||||
)
|
||||
assert read("simulation_assumptions.json")["canonical_package_sha256"] == digest
|
||||
assert fcstd["canonical_package_sha256"] == digest and fcstd["valid"]
|
||||
assert fcstd["maximum_bidirectional_vertex_distance_mm"] < 1e-4
|
||||
assert (
|
||||
fcstd["root_solids"] == neutral["occurrences"]
|
||||
and fcstd["relative_volume_error"] < 1e-5
|
||||
)
|
||||
for suffix in ("step", "FCStd"):
|
||||
assert fcstd[f"{suffix}_sha256"] == sha(OUT / f"lh20_right_hand.{suffix}")
|
||||
for name in ("lh20_right_hand", "joint_A"):
|
||||
rendering = read(f"{name}_overview.json")
|
||||
assert rendering["canonical_package_sha256"] == sha(OUT / f"{name}.scadpkg")
|
||||
assert rendering["image_sha256"] == sha(OUT / f"{name}_overview.png")
|
||||
posed_image = "lh20_right_hand_opposition_index_overview"
|
||||
rendering = read(posed_image + ".json")
|
||||
assert rendering["canonical_package_sha256"] == digest
|
||||
assert rendering["image_sha256"] == sha(OUT / (posed_image + ".png"))
|
||||
checks = engineering["checks"]
|
||||
assert checks["jacobian_max_error_m_per_rad"] < 1e-8
|
||||
assert checks["cad_neutral_transform_max_error"] < 1e-5
|
||||
assert checks["mujoco_fk_max_error_mm_or_rotation_entry"] < 1e-4
|
||||
assert checks["active_dofs"] == [20, 20, 20] and checks["mujoco_200_steps_finite"]
|
||||
assert projection["maximum_matrix_entry_error"] < 1e-5
|
||||
opposition = read("opposition.json")
|
||||
pose_lines = []
|
||||
pose_files = []
|
||||
for pose in [
|
||||
"opposition_index",
|
||||
"opposition_middle",
|
||||
"opposition_ring",
|
||||
"opposition_little",
|
||||
"opposition_index@0.5",
|
||||
]:
|
||||
filename = f"lh20_right_hand_{pose}_assembly_clearance.json"
|
||||
r = read(filename)
|
||||
pose_files.append(filename)
|
||||
assert r["package_sha256"] == digest, f"{pose} audit is stale"
|
||||
assert r["geometry_pass"] and r["interference_pass"], (
|
||||
f"{pose} still needs clearance revisions"
|
||||
)
|
||||
status = (
|
||||
"通过"
|
||||
if r["interference_pass"]
|
||||
else f"未通过:{len(r['interferences'])} 对干涉"
|
||||
)
|
||||
pose_lines.append(f"| {pose} | {status} | {r['unique_boolean_pairs']} |")
|
||||
modules = []
|
||||
for name in ["joint_A", "joint_B", "joint_C", "joint_A_17p3"]:
|
||||
r = read(name + "_geometry.json")
|
||||
assert (
|
||||
r["interference_pass"] and r["geometry_pass"] and r["constraints_pass"]
|
||||
), name
|
||||
assert r["package_sha256"] == sha(OUT / f"{name}.scadpkg"), (
|
||||
name + " audit is stale"
|
||||
)
|
||||
modules.append(name)
|
||||
size = " × ".join(f"{v:.1f}" for v in neutral["size_mm"])
|
||||
mass = neutral["assigned_material_mass_kg"] * 1000
|
||||
max_gap = max(row["gap_error_mm"] for row in opposition)
|
||||
rows = "\n".join(pose_lines)
|
||||
e = engineering["checks"]
|
||||
review = f"""# LH20 R0.1 工程交付记录
|
||||
|
||||
这是一份可重新构建的无腱绳灵巧手数字原型。20 个主动转动轴、五个可替换手指、三档双级行星模块、分节骨架/外盖/指腹,以及编码器和指尖触觉安装空间已建立。当前还不能作为完整制造图或实机性能证明。
|
||||
|
||||
## 本次实际检查
|
||||
|
||||
- 原生装配包 SHA-256:`{digest}`。
|
||||
- {neutral["occurrences"]} 个零件实例、{neutral["unique_parts"]} 种零件定义;全部实体有效且体积为正,基准装配约束解算通过。
|
||||
- 基准姿态的模块间精确 BREP 干涉检查通过,阈值为重叠体积 0.01 mm³;模块包络包含关系先经布尔差验证。
|
||||
- A/B/C 三档模块内部干涉检查通过;A 档另检查了 17.3° 实际驱动角度。
|
||||
- CAD 与独立运动学的最大矩阵分量误差:{e["cad_neutral_transform_max_error"]:.3g};雅可比有限差分误差:{e["jacobian_max_error_m_per_rad"]:.3g} m/rad。
|
||||
- MuJoCo 的 q/nv/nu:{e["active_dofs"]},短时数值测试结果:{e["mujoco_200_steps_finite"]}。这不是动态抓取成功或控制稳定性证明。
|
||||
- 受驱动食指原生装配与独立姿态投影比较了 {projection["leaf_placements_compared"]} 个零件放置,最大矩阵分量误差 {projection["maximum_matrix_entry_error"]:.3g}。
|
||||
- STEP 在 FreeCAD 重读后共有 {fcstd["root_solids"]} 个实体,实体数与源模型一致;使用一致的积分方式,体积相对误差 {fcstd["relative_volume_error"]:.3g};边界顶点双向最近距离最大 {fcstd["maximum_bidirectional_vertex_distance_mm"]:.3g} mm,所有叶实体有效。顶点比较不是完整曲面距离证明。
|
||||
|
||||
## 代表性运动姿态
|
||||
|
||||
| 姿态 | 精确几何结果 | 精确布尔对数量 |
|
||||
|---|---|---:|
|
||||
{rows}
|
||||
|
||||
四组对指的点/法向逆解最大接触点误差 {max_gap:.4f} mm。对指终点及一个中间状态只是有限样本,**不能据此宣称全关节范围、整段路径或任意物体抓取都无干涉**。
|
||||
|
||||
上述坐标、雅可比和逆解误差仅描述软件数值一致性,不代表加工精度、定位精度或真实接触力精度。
|
||||
|
||||
发现的碰撞没有被忽略:`clearance_reliefs.json` 保存逐零件的测量来源、局部避让尺寸与 0.4 mm 名义余量。构建时限制去除体积和脱落碎片,并再次检查闭合实体。局部修订仍需要最小壁厚、疲劳和刚度验证。历史失败报告保存在 `history/`。
|
||||
|
||||
## 尺寸、质量和力预算
|
||||
|
||||
- 当前展开姿态外包络(X×Y×Z):{size} mm,包含伸出的拇指与腕盘;不是掌部本体尺寸。
|
||||
- 按已指定材料密度计算约 {mass:.1f} g。**没有计入真实电机、传感器、磁环、电线、紧固件及完整电子装配质量**,不能称为整手重量。
|
||||
- 暂定连续 3 N / 短时 8 N 是设计目标。完全伸直的中指在 8 N 法向载荷下需要约 0.92 N·m,超过 A 档 0.90 N·m 的当前预算。示例控制采用姿态相关的力矩限幅和 20% 预算余量。
|
||||
- MR104 / 3.2 mm 支承间距改善了初始 MR84 方案的侧向弯矩筛查;目录静额定载荷不能直接作为整关节的许用侧向载荷。
|
||||
|
||||
## 交付物与边界
|
||||
|
||||
`lh20_right_hand.scadpkg` 是原生模型;`.step` 可供其他 CAD 检查;`.FCStd` 为 BREP 装配快照,特征历史保留在源文件和原生包。`lh20.xml` / `lh20.urdf` 使用同源 CAD 外观,碰撞与惯量为明确标注的简化模型。`cad_bom.json` 是几何实例清单,采购轴承应按 40 套计,不应把示意滚珠逐个当成采购零件。
|
||||
|
||||
尚需闭合:定制电机与热性能、齿轮强度/寿命与背隙、轴承装配/寿命、轴向固定及紧固件、公差栈、完整运动间隙与刚度、真实布线、驱动/触觉电路、标定和实机控制。下一阶段应先进行单关节与单指台架验证,再扩展整手。
|
||||
"""
|
||||
(OUT / "engineering_review.md").write_text(review)
|
||||
files = [
|
||||
"lh20_right_hand.scadpkg",
|
||||
"lh20_right_hand.step",
|
||||
"lh20_right_hand.FCStd",
|
||||
"lh20_right_hand_overview.png",
|
||||
"joint_A_overview.png",
|
||||
"lh20_right_hand_overview.json",
|
||||
"joint_A_overview.json",
|
||||
posed_image + ".png",
|
||||
posed_image + ".json",
|
||||
"lh20.xml",
|
||||
"lh20.urdf",
|
||||
"engineering_review.md",
|
||||
"cad_bom.json",
|
||||
"engineering_checks.json",
|
||||
"pose_projection_check.json",
|
||||
"simulation_assumptions.json",
|
||||
"opposition.json",
|
||||
"initial_interference_audit.json",
|
||||
"thumb_zero_reference.json",
|
||||
"lh20_right_hand_assembly_clearance.json",
|
||||
"lh20_right_hand_freecad_check.json",
|
||||
*pose_files,
|
||||
]
|
||||
files.extend(name + "_geometry.json" for name in modules)
|
||||
files.extend(name + ".scadpkg" for name in modules)
|
||||
files.extend(str(p.relative_to(OUT)) for p in (OUT / "history").glob("*.json"))
|
||||
files.extend(str(p.relative_to(OUT)) for p in (OUT / "meshes").glob("*.obj"))
|
||||
manifest = {
|
||||
"prototype_status": "digital engineering prototype; physical performance and manufacturing not qualified",
|
||||
"canonical_package_sha256": digest,
|
||||
"files": {name: sha(OUT / name) for name in files},
|
||||
"source_files": {
|
||||
str(p.relative_to(HERE)): sha(p)
|
||||
for p in HERE.rglob("*")
|
||||
if p.is_file()
|
||||
and (p.suffix in (".py", ".md", ".json", ".txt") or p.name == ".gitignore")
|
||||
and "out" not in p.relative_to(HERE).parts
|
||||
and "__pycache__" not in p.parts
|
||||
and ".pytest_cache" not in p.parts
|
||||
and "__marimo__" not in p.parts
|
||||
},
|
||||
}
|
||||
(OUT / "release_manifest.json").write_text(json.dumps(manifest, indent=2))
|
||||
files.append("release_manifest.json")
|
||||
target = OUT / "LH20_R01_delivery.zip"
|
||||
with zipfile.ZipFile(target, "w", zipfile.ZIP_DEFLATED, compresslevel=6) as archive:
|
||||
for name in manifest["source_files"]:
|
||||
archive.write(HERE / name, f"lh20_hand/{name}")
|
||||
for name in files:
|
||||
archive.write(OUT / name, f"lh20_hand/out/{name}")
|
||||
print("Handoff", target, "bytes", target.stat().st_size, flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,124 @@
|
||||
"""Depth-tested studio images of actual CAD, using the SDK native renderer.
|
||||
|
||||
The native rendering adapter is isolated here; it does not create CAD features.
|
||||
"""
|
||||
|
||||
import argparse, hashlib, json
|
||||
import numpy as np
|
||||
from PIL import Image, ImageDraw, ImageFont
|
||||
import simplecadapi as scad
|
||||
from simplecadapi._cpp import _native
|
||||
from model_io import read_model, flatten_pose
|
||||
from parameters import OUT
|
||||
from parts.common import rigid_shape
|
||||
|
||||
|
||||
def panel(leaves, view, hidden=(), explode=False):
|
||||
scene = _native.RenderScene()
|
||||
for path, p, m in leaves:
|
||||
if any(h in path or h in p.part_id for h in hidden):
|
||||
continue
|
||||
matrix = m.copy()
|
||||
if explode:
|
||||
matrix[2, 3] += matrix[2, 3] * 2.0
|
||||
shape = rigid_shape(p.body, matrix)
|
||||
color = tuple(p.material.color)[:3] if p.material else (0.6, 0.65, 0.7)
|
||||
scene.add_shape(
|
||||
shape,
|
||||
color=color,
|
||||
studio_material=True,
|
||||
linear_deflection=0.12,
|
||||
angular_deflection=0.22,
|
||||
edges=False,
|
||||
ink=False,
|
||||
)
|
||||
frame = _native.render_scene(
|
||||
scene,
|
||||
900,
|
||||
1120,
|
||||
supersample=2,
|
||||
style="studio",
|
||||
views=[(*view, "")],
|
||||
view_up=(0, 1, 0),
|
||||
edge_width_scale=0.0,
|
||||
show_axes=False,
|
||||
background_top=(0.945, 0.95, 0.96),
|
||||
background_bottom=(0.88, 0.90, 0.92),
|
||||
)
|
||||
return Image.fromarray(np.asarray(frame.rgba)[:, :, :3].copy())
|
||||
|
||||
|
||||
def font(size):
|
||||
return ImageFont.truetype("/System/Library/Fonts/Helvetica.ttc", size)
|
||||
|
||||
|
||||
def main(name, pose=None):
|
||||
leaves = flatten_pose(read_model(name), pose)
|
||||
is_joint = name.startswith("joint")
|
||||
if is_joint:
|
||||
views = [(32, -45), (16, -60), (22, -30)]
|
||||
hides = [(), ("case", "cap"), ("case",)]
|
||||
titles = ["COMPLETE MODULE", "HOUSING REMOVED", "AXIAL EXPLODED VIEW"]
|
||||
else:
|
||||
views = [(72, 15), (-72, 165), (72, 15)]
|
||||
hides = [(), (), ("cover", "pad", "case", "cap", "_frame_", "palm_chassis")]
|
||||
titles = [
|
||||
"PALMAR / CONTACT SURFACES",
|
||||
"DORSAL / SERVICE COVERS",
|
||||
"INTERNAL / TRANSMISSION EXPOSED",
|
||||
]
|
||||
image = Image.new("RGB", (2700, 1430), "#f1f2f5")
|
||||
draw = ImageDraw.Draw(image)
|
||||
heading = (
|
||||
"LH20 / TWO-STAGE PLANETARY ACTUATOR"
|
||||
if is_joint
|
||||
else "LH20 / TENDON-FREE DEXTEROUS HAND"
|
||||
)
|
||||
draw.text((90, 45), heading, font=font(52), fill="#1c2835")
|
||||
draw.text(
|
||||
(90, 112),
|
||||
"R0.1 | Actual parametric CAD geometry | Engineering prototype",
|
||||
font=font(25),
|
||||
fill="#526171",
|
||||
)
|
||||
for i, (v, h, t) in enumerate(zip(views, hides, titles)):
|
||||
print("Rendering", t, flush=True)
|
||||
image.paste(panel(leaves, v, h, explode=is_joint and i == 2), (900 * i, 215))
|
||||
draw.text((900 * i + 32, 185), t, font=font(21), fill="#364454")
|
||||
draw.text(
|
||||
(90, 1370),
|
||||
"Copper: unselected motor / Green: PCB space / Cyan: sensor space / Generic bearing raceways",
|
||||
font=font(25),
|
||||
fill="#4e5d6a",
|
||||
)
|
||||
if pose:
|
||||
draw.text(
|
||||
(90, 150),
|
||||
f"Pose: {pose} / independent kinematic inspection projection",
|
||||
font=font(20),
|
||||
fill="#526171",
|
||||
)
|
||||
path = OUT / f"{name}_{pose + '_' if pose else ''}overview.png"
|
||||
image.save(path)
|
||||
path.with_suffix(".json").write_text(
|
||||
json.dumps(
|
||||
{
|
||||
"canonical_package_sha256": hashlib.sha256(
|
||||
(OUT / f"{name}.scadpkg").read_bytes()
|
||||
).hexdigest(),
|
||||
"image_sha256": hashlib.sha256(path.read_bytes()).hexdigest(),
|
||||
"pose": pose,
|
||||
"source": "actual CAD BREP rendered by the SDK; colors distinguish reserved component spaces",
|
||||
},
|
||||
indent=2,
|
||||
)
|
||||
)
|
||||
print(path, flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--name", default="lh20_right_hand")
|
||||
ap.add_argument("--pose")
|
||||
a = ap.parse_args()
|
||||
main(a.name, a.pose)
|
||||
@@ -0,0 +1,113 @@
|
||||
"""Turn measured collision patches into explicit, reviewable clearance features.
|
||||
|
||||
Only palm structure/skin and adjacent-link frame/cover patches can be relieved.
|
||||
No actuator, gear or cross-finger collision is hidden by this design tool.
|
||||
Run the independent geometry verifier again after rebuilding.
|
||||
"""
|
||||
|
||||
from pathlib import Path
|
||||
import sys, json, argparse, hashlib, re
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
import numpy as np
|
||||
from OCP.BRepAlgoAPI import BRepAlgoAPI_Common
|
||||
from OCP.TopExp import TopExp_Explorer
|
||||
from OCP.TopAbs import TopAbs_SOLID
|
||||
from OCP.TopoDS import TopoDS
|
||||
from simplecadapi.verify import to_ocp
|
||||
from model_io import read_model, flatten_pose
|
||||
from parameters import HERE, OUT
|
||||
from verify.geometry import placed, bounds, volume
|
||||
|
||||
|
||||
def rank(path):
|
||||
if path == "/chassis":
|
||||
return (0, 0)
|
||||
if path.startswith("/palm_"):
|
||||
return (1, 0)
|
||||
tail = path.rsplit("/", 1)[-1]
|
||||
if tail.startswith("cover"):
|
||||
return (2, int(tail[-1]))
|
||||
if tail.startswith("frame"):
|
||||
return (3, int(tail[-1]))
|
||||
return (99, 0)
|
||||
|
||||
|
||||
def main(pose):
|
||||
suffix='' if pose=='neutral' else pose+'_'
|
||||
report = json.loads(
|
||||
(OUT / f"lh20_right_hand_{suffix}assembly_clearance.json").read_text()
|
||||
)
|
||||
leaves = {path: (p, m) for path, p, m in flatten_pose(read_model(), None if pose=='neutral' else pose)}
|
||||
path = HERE / "clearance_reliefs.json"
|
||||
data = (
|
||||
json.loads(path.read_text())
|
||||
if path.exists()
|
||||
else {
|
||||
"margin_mm": 0.4,
|
||||
"scope": "Measured representative-pose reliefs; not a swept-volume or strength qualification",
|
||||
"parts": {},
|
||||
}
|
||||
)
|
||||
count = 0
|
||||
for collision in report["interferences"]:
|
||||
a, b = collision["a"], collision["b"]
|
||||
if rank(b) < rank(a):
|
||||
a, b = b, a
|
||||
if rank(a)[0] >= 99:
|
||||
raise RuntimeError(f"No allowed structural relief target: {a} {b}")
|
||||
if rank(a)[0] >= 2 and a.split("/")[1] != b.split("/")[1]:
|
||||
raise RuntimeError(
|
||||
f"Cross-finger collision needs a kinematic redesign: {a} {b}"
|
||||
)
|
||||
p, m = leaves[a]
|
||||
p2, m2 = leaves[b]
|
||||
common = BRepAlgoAPI_Common(
|
||||
to_ocp(p.body), placed(to_ocp(p2.body), np.linalg.inv(m) @ m2)
|
||||
)
|
||||
if not common.IsDone():
|
||||
raise RuntimeError("Collision intersection failed")
|
||||
explorer = TopExp_Explorer(common.Shape(), TopAbs_SOLID)
|
||||
while explorer.More():
|
||||
solid = TopoDS.Solid_s(explorer.Current())
|
||||
v = volume(solid)
|
||||
if v > 1e-4:
|
||||
bb = bounds(solid)
|
||||
bb[:3] -= data["margin_mm"]
|
||||
bb[3:] += data["margin_mm"]
|
||||
record = dict(
|
||||
min_mm=np.round(bb[:3], 6).tolist(),
|
||||
max_mm=np.round(bb[3:], 6).tolist(),
|
||||
source_pose=pose,
|
||||
other=b,
|
||||
measured_patch_mm3=v,
|
||||
)
|
||||
records = data["parts"].setdefault(p.part_id, [])
|
||||
duplicate = any(
|
||||
all(
|
||||
old[k] == record[k]
|
||||
for k in ("min_mm", "max_mm", "source_pose", "other")
|
||||
)
|
||||
for old in records
|
||||
)
|
||||
if not duplicate:
|
||||
records.append(record)
|
||||
count += 1
|
||||
explorer.Next()
|
||||
path.write_text(json.dumps(data, indent=2) + "\n")
|
||||
digest = hashlib.sha256(path.read_bytes()).hexdigest()
|
||||
params = HERE / "parameters.py"
|
||||
params.write_text(
|
||||
re.sub(
|
||||
r'CLEARANCE_REVISION = "[^"]*"',
|
||||
f'CLEARANCE_REVISION = "{digest}"',
|
||||
params.read_text(),
|
||||
)
|
||||
)
|
||||
print("Recorded", count, "local relief boxes in", path, flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--pose", default="opposition_index")
|
||||
main(ap.parse_args().pose)
|
||||
@@ -0,0 +1,190 @@
|
||||
"""Reproducible kinematic, static-load and simulation interface checks."""
|
||||
|
||||
from pathlib import Path
|
||||
import sys, json, hashlib
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
import numpy as np
|
||||
import mujoco
|
||||
from parameters import FINGERS, SIZES, OUT
|
||||
from kinematics import finger_frames, pad_contact, jacobian
|
||||
from model_io import read_model, flatten
|
||||
|
||||
|
||||
def main():
|
||||
rng = np.random.default_rng(20)
|
||||
jac_errors = []
|
||||
for f in FINGERS:
|
||||
for _ in range(8):
|
||||
q = np.array([rng.uniform(*np.deg2rad(lim)) for lim in f.limits])
|
||||
eps = 1e-6
|
||||
numeric = np.column_stack(
|
||||
[
|
||||
(
|
||||
pad_contact(f, q + np.eye(4)[k] * eps)[0]
|
||||
- pad_contact(f, q - np.eye(4)[k] * eps)[0]
|
||||
)
|
||||
/ (2 * eps * 1000)
|
||||
for k in range(4)
|
||||
]
|
||||
)
|
||||
jac_errors.append(float(np.max(np.abs(numeric - jacobian(f, q)))))
|
||||
leaves = {path: m for path, p, m in flatten(read_model())}
|
||||
cad_errors = []
|
||||
for f in FINGERS:
|
||||
frames, _ = finger_frames(f)
|
||||
for k in range(4):
|
||||
cad_errors.append(
|
||||
float(
|
||||
np.max(np.abs(leaves[f"/{f.name}/frame{k}"] - frames[k]["moving"]))
|
||||
)
|
||||
)
|
||||
model = mujoco.MjModel.from_xml_path(str(OUT / "lh20.xml"))
|
||||
data = mujoco.MjData(model)
|
||||
sim_errors = []
|
||||
for _ in range(8):
|
||||
for i, f in enumerate(FINGERS):
|
||||
data.qpos[i * 4 : i * 4 + 4] = [
|
||||
rng.uniform(*np.deg2rad(lim)) for lim in f.limits
|
||||
]
|
||||
mujoco.mj_forward(model, data)
|
||||
for i, f in enumerate(FINGERS):
|
||||
frames, _ = finger_frames(f, data.qpos[i * 4 : i * 4 + 4])
|
||||
for k in range(4):
|
||||
bid = mujoco.mj_name2id(
|
||||
model, mujoco.mjtObj.mjOBJ_BODY, f"{f.name}_{k}"
|
||||
)
|
||||
fr = frames[k]["moving"]
|
||||
sim_errors.append(
|
||||
float(np.max(np.abs(data.xpos[bid] * 1000 - fr[:3, 3])))
|
||||
)
|
||||
sim_errors.append(
|
||||
float(np.max(np.abs(data.xmat[bid].reshape(3, 3) - fr[:3, :3])))
|
||||
)
|
||||
# A bounded numerical smoke test, deliberately not a grasp-success claim.
|
||||
mujoco.mj_resetData(model, data)
|
||||
for _ in range(200):
|
||||
data.ctrl[:] = np.clip(
|
||||
-0.25 * data.qpos - 0.035 * data.qvel,
|
||||
model.actuator_ctrlrange[:, 0],
|
||||
model.actuator_ctrlrange[:, 1],
|
||||
)
|
||||
mujoco.mj_step(model, data)
|
||||
finite = bool(np.isfinite(data.qpos).all() and np.isfinite(data.qvel).all())
|
||||
opposition = json.loads((OUT / "opposition.json").read_text())
|
||||
loads = []
|
||||
for i, f in enumerate(FINGERS):
|
||||
poses = {"open": np.zeros(4), "half_flex": np.deg2rad([35, 0, 45, 25])}
|
||||
for row in opposition:
|
||||
if f.name == row["finger"]:
|
||||
poses["opposition"] = np.deg2rad(row["finger_deg"])
|
||||
if f.name == "thumb":
|
||||
poses["opposition_" + row["finger"]] = np.deg2rad(row["thumb_deg"])
|
||||
for pose, q in poses.items():
|
||||
J = jacobian(f, q)
|
||||
_, normal = pad_contact(f, q)
|
||||
for force in (3.0, 8.0):
|
||||
tau = np.abs(J.T @ (force * normal))
|
||||
worst = force * np.linalg.norm(J, axis=0)
|
||||
targets = np.array(
|
||||
[
|
||||
SIZES[s].continuous_target
|
||||
if force == 3
|
||||
else SIZES[s].peak_target
|
||||
for s in f.sizes
|
||||
]
|
||||
)
|
||||
motor = np.array(
|
||||
[
|
||||
tau[k] / (SIZES[s].ratio * SIZES[s].efficiency_assumption)
|
||||
for k, s in enumerate(f.sizes)
|
||||
]
|
||||
)
|
||||
loads.append(
|
||||
dict(
|
||||
finger=f.name,
|
||||
pose=pose,
|
||||
contact_force_N=force,
|
||||
normal_load_joint_torque_Nm=tau.tolist(),
|
||||
any_direction_upper_bound_Nm=worst.tolist(),
|
||||
joint_torque_targets_Nm=targets.tolist(),
|
||||
motor_torque_required_Nm=motor.tolist(),
|
||||
within_output_target=bool(np.all(tau <= targets)),
|
||||
scope="Static contact load only; excludes gravity, inertia, gear friction uncertainty and thermal limits",
|
||||
)
|
||||
)
|
||||
motor_requirements = [
|
||||
dict(
|
||||
size=s.code,
|
||||
ratio=s.ratio,
|
||||
efficiency_assumed=s.efficiency_assumption,
|
||||
continuous_target_Nm=s.continuous_target,
|
||||
peak_target_Nm=s.peak_target,
|
||||
required_motor_continuous_mNm=1000
|
||||
* s.continuous_target
|
||||
/ (s.ratio * s.efficiency_assumption),
|
||||
required_motor_peak_mNm=1000
|
||||
* s.peak_target
|
||||
/ (s.ratio * s.efficiency_assumption),
|
||||
motor_envelope_mm=[s.motor_diameter, s.motor_length],
|
||||
)
|
||||
for s in SIZES.values()
|
||||
]
|
||||
# Side force produces a bending moment at the output pair; this is a screen,
|
||||
# not an NSK equivalent static load / contact-stress / life calculation.
|
||||
lever = max(np.linalg.norm(jacobian(f, np.zeros(4)), axis=0).max() for f in FINGERS)
|
||||
side_reaction = 3.0 * lever / 0.0032
|
||||
result = dict(
|
||||
canonical_package_sha256=hashlib.sha256(
|
||||
(OUT / "lh20_right_hand.scadpkg").read_bytes()
|
||||
).hexdigest(),
|
||||
simulation_xml_sha256=hashlib.sha256(
|
||||
(OUT / "lh20.xml").read_bytes()
|
||||
).hexdigest(),
|
||||
checks={
|
||||
"jacobian_max_error_m_per_rad": max(jac_errors),
|
||||
"cad_neutral_transform_max_error": max(cad_errors),
|
||||
"mujoco_fk_max_error_mm_or_rotation_entry": max(sim_errors),
|
||||
"active_dofs": [int(model.nq), int(model.nv), int(model.nu)],
|
||||
"mujoco_200_steps_finite": finite,
|
||||
},
|
||||
motor_requirements=motor_requirements,
|
||||
loads=loads,
|
||||
bearing_screen={
|
||||
"candidate": "SMB MR104 open envelope, two per output",
|
||||
"catalog_dimensions_mm": [4, 10, 3],
|
||||
"catalog_C0r_N": 27 * 9.80665,
|
||||
"center_spacing_mm": 3.2,
|
||||
"worst_zero_pose_lever_m": float(lever),
|
||||
"reaction_screen_at_3N_side_force_N": float(side_reaction),
|
||||
"catalog_static_rating_to_screen_ratio": float(
|
||||
27 * 9.80665 / side_reaction
|
||||
),
|
||||
"status": "Improved from the MR84 / 2.2 mm baseline. This moment-only reaction screen does not establish allowable load, combined axial load, stiffness or bearing life.",
|
||||
},
|
||||
unverified=[
|
||||
"motor supplier and thermal performance",
|
||||
"gear strength, life and backlash",
|
||||
"bearing mounting fits, retention, preload and life",
|
||||
"keyed shaft fits and fastener/axial retention details",
|
||||
"wiring bend life and full travel swept clearance",
|
||||
"calibrated tactile force and real impedance control",
|
||||
],
|
||||
)
|
||||
(OUT / "engineering_checks.json").write_text(json.dumps(result, indent=2))
|
||||
print(json.dumps(result["checks"], indent=2))
|
||||
print(
|
||||
"Output torque target violations",
|
||||
sum(not x["within_output_target"] for x in loads),
|
||||
"of",
|
||||
len(loads),
|
||||
)
|
||||
print("Side-load reaction screen", side_reaction, "N")
|
||||
assert max(jac_errors) < 1e-8
|
||||
assert max(cad_errors) < 1e-5
|
||||
assert max(sim_errors) < 1e-4
|
||||
assert (model.nq, model.nv, model.nu) == (20, 20, 20) and finite
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,248 @@
|
||||
"""Independent OCP measurements; a failed check is retained and exits nonzero."""
|
||||
|
||||
from pathlib import Path
|
||||
import sys, json, argparse, itertools, re, hashlib, shutil
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
import numpy as np
|
||||
from OCP.BRepCheck import BRepCheck_Analyzer
|
||||
from OCP.BRepGProp import BRepGProp
|
||||
from OCP.GProp import GProp_GProps
|
||||
from OCP.BRepBndLib import BRepBndLib
|
||||
from OCP.Bnd import Bnd_Box
|
||||
from OCP.BRepAlgoAPI import BRepAlgoAPI_Common, BRepAlgoAPI_Cut
|
||||
from OCP.BRepPrimAPI import BRepPrimAPI_MakeCylinder
|
||||
from OCP.BRepBuilderAPI import BRepBuilderAPI_Transform
|
||||
from OCP.gp import gp_Trsf, gp_Ax2, gp_Pnt, gp_Dir
|
||||
from simplecadapi.verify import to_ocp
|
||||
from model_io import read_model, flatten_pose, assembly_reports, pose_angles
|
||||
from parameters import OUT, SIZES
|
||||
|
||||
|
||||
def volume(shape):
|
||||
p = GProp_GProps()
|
||||
BRepGProp.VolumeProperties_s(shape, p)
|
||||
return p.Mass()
|
||||
|
||||
|
||||
def bounds(shape):
|
||||
b = Bnd_Box()
|
||||
BRepBndLib.AddOptimal_s(shape, b, False, False)
|
||||
return np.array(b.Get())
|
||||
|
||||
|
||||
def placed(shape, m):
|
||||
t = gp_Trsf()
|
||||
t.SetValues(*[float(v) for v in m[:3].reshape(-1)])
|
||||
return BRepBuilderAPI_Transform(shape, t, False).Shape()
|
||||
|
||||
|
||||
def overlap(a, b):
|
||||
common = BRepAlgoAPI_Common(a, b)
|
||||
if not common.IsDone():
|
||||
raise RuntimeError("Common boolean failed")
|
||||
return max(0.0, volume(common.Shape()))
|
||||
|
||||
|
||||
def main(name, collisions=False, external_only=False, pose=None):
|
||||
package_hash = hashlib.sha256((OUT / f"{name}.scadpkg").read_bytes()).hexdigest()
|
||||
root = read_model(name)
|
||||
leaves = flatten_pose(root, pose)
|
||||
unique = {p.part_id: p for _, p, _ in leaves}
|
||||
unique_shapes = {k: to_ocp(p.body) for k, p in unique.items()}
|
||||
facts = []
|
||||
for k, p in unique.items():
|
||||
s = unique_shapes[k]
|
||||
facts.append(
|
||||
{
|
||||
"part": k,
|
||||
"valid": bool(BRepCheck_Analyzer(s).IsValid()),
|
||||
"volume_mm3": volume(s),
|
||||
"bounds_mm": bounds(s).tolist(),
|
||||
}
|
||||
)
|
||||
print("unique geometry checked", len(facts), flush=True)
|
||||
occurrences = []
|
||||
mass = 0.0
|
||||
unassigned = []
|
||||
for path, p, m in leaves:
|
||||
s = placed(unique_shapes[p.part_id], m)
|
||||
bb = bounds(s)
|
||||
occurrences.append((path, p, s, bb))
|
||||
density = p.material.density if p.material else None
|
||||
if density is None:
|
||||
unassigned.append(path)
|
||||
else:
|
||||
mass += volume(unique_shapes[p.part_id]) * float(density) / 1e9
|
||||
bb_all = np.array([r[3] for r in occurrences])
|
||||
bbox = np.r_[bb_all[:, :3].min(axis=0), bb_all[:, 3:].max(axis=0)]
|
||||
reports = assembly_reports(root)
|
||||
interference = []
|
||||
pair_cache = {}
|
||||
checked_pairs = 0
|
||||
transforms = {path: m for path, p, m in leaves}
|
||||
|
||||
def module(path):
|
||||
match = re.match(r"(/[^/]+/joint\d+)/", path)
|
||||
return match[1] if match else None
|
||||
|
||||
envelopes = {}
|
||||
envelope_valid = {}
|
||||
envelope_cache = {}
|
||||
if external_only:
|
||||
for path, p, m in leaves:
|
||||
if path.endswith("/case") and module(path):
|
||||
code = p.part_id.rsplit("_", 1)[-1]
|
||||
s = SIZES[code]
|
||||
env = BRepPrimAPI_MakeCylinder(
|
||||
gp_Ax2(gp_Pnt(0, 0, -s.width / 2 - 0.05), gp_Dir(0, 0, 1)),
|
||||
s.diameter / 2 + 0.05,
|
||||
s.width + 0.1,
|
||||
).Shape()
|
||||
prefix = module(path)
|
||||
if code not in envelope_valid:
|
||||
ok = True
|
||||
for pp, part, mm in leaves:
|
||||
if module(pp) != prefix:
|
||||
continue
|
||||
local = placed(
|
||||
unique_shapes[part.part_id], np.linalg.inv(m) @ mm
|
||||
)
|
||||
cut = BRepAlgoAPI_Cut(local, env)
|
||||
ok = ok and cut.IsDone() and abs(volume(cut.Shape())) < 1e-5
|
||||
envelope_valid[code] = ok
|
||||
if envelope_valid[code]:
|
||||
envelopes[prefix] = (env, m, code)
|
||||
print("Module envelope containment", envelope_valid, flush=True)
|
||||
|
||||
def envelope_disjoint(path, p, m, path2, p2, m2):
|
||||
g1 = module(path)
|
||||
g2 = module(path2)
|
||||
if g1 in envelopes and g2 in envelopes:
|
||||
key = ("groups", g1, g2)
|
||||
if key not in envelope_cache:
|
||||
e1, t1, _ = envelopes[g1]
|
||||
e2, t2, _ = envelopes[g2]
|
||||
envelope_cache[key] = (
|
||||
overlap(e1, placed(e2, np.linalg.inv(t1) @ t2)) < 1e-7
|
||||
)
|
||||
return envelope_cache[key]
|
||||
if g1 in envelopes:
|
||||
path, p, m, path2, p2, m2, g1, g2 = path2, p2, m2, path, p, m, g2, g1
|
||||
if g2 in envelopes:
|
||||
env, tm, code = envelopes[g2]
|
||||
relative = np.linalg.inv(m) @ tm
|
||||
key = ("part_group", p.part_id, code, tuple(np.round(relative.ravel(), 6)))
|
||||
if key not in envelope_cache:
|
||||
envelope_cache[key] = (
|
||||
overlap(unique_shapes[p.part_id], placed(env, relative)) < 1e-7
|
||||
)
|
||||
return envelope_cache[key]
|
||||
return False
|
||||
|
||||
if collisions:
|
||||
for i, (path, p, s, bb) in enumerate(occurrences):
|
||||
for path2, p2, s2, bb2 in occurrences[i + 1 :]:
|
||||
if (
|
||||
external_only
|
||||
and module(path) is not None
|
||||
and module(path) == module(path2)
|
||||
):
|
||||
continue
|
||||
if np.any(
|
||||
np.minimum(bb[3:], bb2[3:]) - np.maximum(bb[:3], bb2[:3]) < 0.001
|
||||
):
|
||||
continue
|
||||
# Repeated fingers contain the same module geometry. Reuse only
|
||||
# when BOTH part ids and their relative rigid transform agree.
|
||||
m = leaves[i][2]
|
||||
m2 = transforms[path2]
|
||||
if external_only and envelope_disjoint(path, p, m, path2, p2, m2):
|
||||
continue
|
||||
relative = np.linalg.inv(m) @ m2
|
||||
key = (p.part_id, p2.part_id, tuple(np.round(relative.ravel(), 6)))
|
||||
if key not in pair_cache:
|
||||
pair_cache[key] = overlap(
|
||||
unique_shapes[p.part_id],
|
||||
placed(unique_shapes[p2.part_id], relative),
|
||||
)
|
||||
v = pair_cache[key]
|
||||
checked_pairs += 1
|
||||
if v > 0.01:
|
||||
interference.append({"a": path, "b": path2, "overlap_mm3": v})
|
||||
print("INTERFERENCE", path, path2, round(v, 5), flush=True)
|
||||
result = {
|
||||
"model": name,
|
||||
"scope": "nominal CAD pose only; material estimate excludes unassigned motor envelopes and unmodeled hardware",
|
||||
"occurrences": len(leaves),
|
||||
"unique_parts": len(unique),
|
||||
"geometry": facts,
|
||||
"assembly_reports": reports,
|
||||
"bounds_mm": bbox.tolist(),
|
||||
"size_mm": (bbox[3:] - bbox[:3]).tolist(),
|
||||
"assigned_material_mass_kg": mass,
|
||||
"unassigned_mass_occurrences": unassigned,
|
||||
"interference_checked": collisions,
|
||||
"interferences": interference,
|
||||
"geometry_pass": all(f["valid"] and f["volume_mm3"] > 0 for f in facts),
|
||||
"constraints_pass": all(r["solved"] for r in reports),
|
||||
}
|
||||
result.update(
|
||||
package_sha256=package_hash,
|
||||
external_only=external_only,
|
||||
candidate_pairs=checked_pairs,
|
||||
unique_boolean_pairs=len(pair_cache),
|
||||
interference_pass=bool(collisions and not interference),
|
||||
)
|
||||
result.update(
|
||||
pose=pose or "canonical notebook configuration",
|
||||
pose_angles_radians={k: v.tolist() for k, v in pose_angles(pose).items()}
|
||||
if name.startswith("lh20_right_hand")
|
||||
else None,
|
||||
constraint_report_scope="Canonical notebook solve; posed inspection uses independent FK"
|
||||
if pose
|
||||
else "Canonical notebook solve",
|
||||
)
|
||||
path = (
|
||||
OUT
|
||||
/ f"{name}_{pose + '_' if pose else ''}{'assembly_clearance' if external_only else 'geometry'}.json"
|
||||
)
|
||||
if path.exists():
|
||||
history = OUT / "history"
|
||||
history.mkdir(exist_ok=True)
|
||||
digest = hashlib.sha256(path.read_bytes()).hexdigest()[:12]
|
||||
shutil.copy2(path, history / f"{path.stem}_{digest}.json")
|
||||
path.write_text(json.dumps(result, indent=2))
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
k: v
|
||||
for k, v in result.items()
|
||||
if k
|
||||
not in [
|
||||
"geometry",
|
||||
"assembly_reports",
|
||||
"unassigned_mass_occurrences",
|
||||
"interferences",
|
||||
]
|
||||
},
|
||||
indent=2,
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
if (
|
||||
not result["geometry_pass"]
|
||||
or not result["constraints_pass"]
|
||||
or (collisions and interference)
|
||||
):
|
||||
raise SystemExit(1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--name", default="lh20_right_hand")
|
||||
ap.add_argument("--collisions", action="store_true")
|
||||
ap.add_argument("--external-only", action="store_true")
|
||||
ap.add_argument("--pose")
|
||||
a = ap.parse_args()
|
||||
main(a.name, a.collisions, a.external_only, a.pose)
|
||||
@@ -0,0 +1,139 @@
|
||||
"""Opposition search with conservative capsule self-collision penalties.
|
||||
|
||||
Final acceptance still needs exact CAD interference and hardware testing.
|
||||
"""
|
||||
|
||||
from pathlib import Path
|
||||
import sys, json
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
import numpy as np
|
||||
import mujoco
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial.transform import Rotation
|
||||
import xml.etree.ElementTree as ET
|
||||
from parameters import FINGERS, OUT, HERE
|
||||
from kinematics import pad_contact
|
||||
|
||||
|
||||
def collision_model():
|
||||
root = ET.Element("mujoco")
|
||||
ET.SubElement(root, "compiler", angle="radian")
|
||||
world = ET.SubElement(root, "worldbody")
|
||||
palm = ET.SubElement(world, "body", name="palm")
|
||||
ET.SubElement(palm, "geom", type="box", pos="0 .035 0", size=".045 .0425 .015")
|
||||
contacts = ET.SubElement(root, "contact")
|
||||
|
||||
def seq(a):
|
||||
return " ".join(str(float(x)) for x in a)
|
||||
|
||||
for f in FINGERS:
|
||||
parent = palm
|
||||
parent_name = "palm"
|
||||
for k in range(4):
|
||||
name = f"{f.name}_{k}"
|
||||
rot = Rotation.from_euler("xyz", f.pre_rotations[k], degrees=True)
|
||||
pos = (
|
||||
np.array([0, f.lengths[k - 1] / 1000, 0])
|
||||
if k
|
||||
else np.array(f.base) / 1000
|
||||
)
|
||||
if k == 0:
|
||||
rot = Rotation.from_euler("xyz", f.base_rpy, degrees=True) * rot
|
||||
x, y, z, w = rot.as_quat()
|
||||
body = ET.SubElement(
|
||||
parent, "body", name=name, pos=seq(pos), quat=seq([w, x, y, z])
|
||||
)
|
||||
axis = {"x": "1 0 0", "y": "0 1 0", "z": "0 0 1"}[f.axes[k]]
|
||||
ET.SubElement(body, "joint", axis=axis, type="hinge")
|
||||
ET.SubElement(
|
||||
body,
|
||||
"geom",
|
||||
type="capsule",
|
||||
fromto=f"0 .010 0 0 {max(0.011, f.lengths[k] / 1000 - 0.005)} 0",
|
||||
size=".010",
|
||||
density="1000",
|
||||
)
|
||||
ET.SubElement(contacts, "exclude", body1=parent_name, body2=name)
|
||||
parent = body
|
||||
parent_name = name
|
||||
m = mujoco.MjModel.from_xml_string(ET.tostring(root, encoding="unicode"))
|
||||
return m, mujoco.MjData(m)
|
||||
|
||||
|
||||
def solve(name, seed_row):
|
||||
model, data = collision_model()
|
||||
other = next(f for f in FINGERS if f.name == name)
|
||||
thumb = FINGERS[-1]
|
||||
idx = FINGERS.index(other)
|
||||
lo = np.deg2rad([a + 0.5 for f in (thumb, other) for a, b in f.limits])
|
||||
hi = np.deg2rad([b - 0.5 for f in (thumb, other) for a, b in f.limits])
|
||||
pairs = {
|
||||
(a, b): i
|
||||
for i, (a, b) in enumerate(
|
||||
((a, b) for a in range(model.ngeom) for b in range(a + 1, model.ngeom))
|
||||
)
|
||||
}
|
||||
|
||||
def evaluate(q):
|
||||
data.qpos[:] = 0
|
||||
data.qpos[16:20] = q[:4]
|
||||
data.qpos[idx * 4 : idx * 4 + 4] = q[4:]
|
||||
mujoco.mj_forward(model, data)
|
||||
penalties = np.zeros(len(pairs))
|
||||
for c in data.contact:
|
||||
key = tuple(sorted((int(c.geom1), int(c.geom2))))
|
||||
penalties[pairs[key]] = max(
|
||||
penalties[pairs[key]], max(0.0, -float(c.dist)) * 1000
|
||||
)
|
||||
return penalties
|
||||
|
||||
target = np.array([(other.base[0] + 10) / 2, 95.0, 65.0])
|
||||
|
||||
def residual(q):
|
||||
a, na = pad_contact(thumb, q[:4])
|
||||
b, nb = pad_contact(other, q[4:])
|
||||
return np.r_[
|
||||
a - b,
|
||||
20 * (na + nb),
|
||||
0.008 * ((a + b) / 2 - target),
|
||||
0.002 * q,
|
||||
5 * evaluate(q),
|
||||
]
|
||||
|
||||
rng = np.random.default_rng(30 + idx)
|
||||
seeds = [np.deg2rad(seed_row["thumb_deg"] + seed_row["finger_deg"])]
|
||||
seeds.extend(rng.uniform(lo, hi, size=(15, 8)))
|
||||
best = None
|
||||
for s in seeds:
|
||||
r = least_squares(
|
||||
residual, np.clip(s, lo + 1e-6, hi - 1e-6), bounds=(lo, hi), max_nfev=400
|
||||
)
|
||||
if best is None or np.linalg.norm(r.fun) < np.linalg.norm(best.fun):
|
||||
best = r
|
||||
q = best.x
|
||||
a, na = pad_contact(thumb, q[:4])
|
||||
b, nb = pad_contact(other, q[4:])
|
||||
penalties = evaluate(q)
|
||||
return dict(
|
||||
finger=name,
|
||||
thumb_deg=np.rad2deg(q[:4]).tolist(),
|
||||
finger_deg=np.rad2deg(q[4:]).tolist(),
|
||||
gap_error_mm=float(np.linalg.norm(a - b)),
|
||||
normal_dot=float(na @ nb),
|
||||
thumb_contact_mm=a.tolist(),
|
||||
finger_contact_mm=b.tolist(),
|
||||
max_capsule_penetration_mm=float(penalties.max()),
|
||||
scope="Pad position/normal IK with capsule penalties; not exact CAD collision qualification",
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
OUT.mkdir(exist_ok=True)
|
||||
baseline = json.loads((HERE / "poses_seed.json").read_text())
|
||||
result = []
|
||||
for row in baseline:
|
||||
r = solve(row["finger"], row)
|
||||
result.append(r)
|
||||
print(json.dumps(r), flush=True)
|
||||
(OUT / "opposition.json").write_text(json.dumps(result, indent=2))
|
||||
@@ -0,0 +1,92 @@
|
||||
"""Shared thumb-root search against four distinct opposition targets.
|
||||
|
||||
Produces a proposal, never silently changes the CAD parameters. Collision
|
||||
checking is separate from this kinematic objective.
|
||||
"""
|
||||
|
||||
from pathlib import Path
|
||||
import sys, json
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
from dataclasses import replace
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial.transform import Rotation
|
||||
from parameters import FINGERS, OUT
|
||||
from kinematics import pad_contact
|
||||
|
||||
|
||||
def main():
|
||||
thumb = FINGERS[-1]
|
||||
others = FINGERS[:4]
|
||||
baseline = json.loads((OUT / "opposition_precheck.json").read_text())
|
||||
if isinstance(baseline, dict):
|
||||
baseline = list(baseline.values())
|
||||
qs = np.concatenate(
|
||||
[np.deg2rad(r["thumb_deg"] + r["finger_deg"]) for r in baseline]
|
||||
)
|
||||
r0 = Rotation.from_euler("xyz", thumb.base_rpy, degrees=True)
|
||||
low = np.r_[
|
||||
[34, 20, 0],
|
||||
[-0.8] * 3,
|
||||
np.concatenate(
|
||||
[np.deg2rad([a for a, b in thumb.limits + f.limits]) for f in others]
|
||||
),
|
||||
]
|
||||
high = np.r_[
|
||||
[43, 40, 12],
|
||||
[0.8] * 3,
|
||||
np.concatenate(
|
||||
[np.deg2rad([b for a, b in thumb.limits + f.limits]) for f in others]
|
||||
),
|
||||
]
|
||||
|
||||
def spec(x):
|
||||
r = (Rotation.from_rotvec(x[3:6]) * r0).as_euler("xyz", degrees=True)
|
||||
return replace(thumb, base=tuple(x[:3]), base_rpy=tuple(r))
|
||||
|
||||
def residual(x):
|
||||
t = spec(x)
|
||||
res = []
|
||||
for i, f in enumerate(others):
|
||||
q = x[6 + 8 * i : 14 + 8 * i]
|
||||
a, na = pad_contact(t, q[:4])
|
||||
b, nb = pad_contact(f, q[4:])
|
||||
mid = (a + b) / 2
|
||||
target = np.array([(f.base[0] + 15) / 2, 90, 65])
|
||||
res.extend(np.r_[a - b, 25 * (na + nb), 0.08 * (mid - target), 0.02 * q])
|
||||
return np.r_[res, 0.06 * (x[:3] - thumb.base), 0.8 * x[3:6]]
|
||||
|
||||
seed = np.clip(np.r_[thumb.base, [0, 0, 0], qs], low + 1e-6, high - 1e-6)
|
||||
best = least_squares(residual, seed, bounds=(low, high), max_nfev=500)
|
||||
x = best.x
|
||||
t = spec(x)
|
||||
results = []
|
||||
for i, f in enumerate(others):
|
||||
q = x[6 + 8 * i : 14 + 8 * i]
|
||||
a, na = pad_contact(t, q[:4])
|
||||
b, nb = pad_contact(f, q[4:])
|
||||
results.append(
|
||||
dict(
|
||||
finger=f.name,
|
||||
thumb_deg=np.rad2deg(q[:4]).tolist(),
|
||||
finger_deg=np.rad2deg(q[4:]).tolist(),
|
||||
gap_error_mm=float(np.linalg.norm(a - b)),
|
||||
normal_dot=float(na @ nb),
|
||||
contact_mm=((a + b) / 2).tolist(),
|
||||
)
|
||||
)
|
||||
result = dict(
|
||||
base=t.base,
|
||||
base_rpy=t.base_rpy,
|
||||
results=results,
|
||||
cost=float(best.cost),
|
||||
evaluations=best.nfev,
|
||||
scope="Kinematic root-layout proposal, not collision-qualified",
|
||||
)
|
||||
(OUT / "thumb_layout_proposal.json").write_text(json.dumps(result, indent=2))
|
||||
print(json.dumps(result, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,47 @@
|
||||
"""Cross-check independent pose projection against a real driven notebook."""
|
||||
|
||||
from pathlib import Path
|
||||
import sys, json, hashlib
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
import numpy as np
|
||||
import simplecadapi as scad
|
||||
from simplecadapi.runtime import run_notebook
|
||||
from parameters import FINGERS, HERE, OUT
|
||||
from model_io import read_model, flatten, flatten_pose, pose_angles
|
||||
from kinematics import finger_frames
|
||||
|
||||
|
||||
def main():
|
||||
q = pose_angles("opposition_index")["index"]
|
||||
run = run_notebook(
|
||||
HERE / "finger.py",
|
||||
overrides={"FINGER": "index", "ANGLES": tuple(float(v) for v in np.rad2deg(q))},
|
||||
cache=False,
|
||||
)
|
||||
actual = {
|
||||
path: m for path, p, m in flatten(scad.materialize_definition(run.definition))
|
||||
}
|
||||
base = finger_frames(FINGERS[0])[0][0]["stator"]
|
||||
inv = np.linalg.inv(base)
|
||||
errors = []
|
||||
for path, p, m in flatten_pose(read_model(), "opposition_index"):
|
||||
if path.startswith("/index/"):
|
||||
key = path[len("/index") :]
|
||||
errors.append((key, float(np.max(np.abs(actual[key] - inv @ m)))))
|
||||
result = {
|
||||
"canonical_package_sha256": hashlib.sha256(
|
||||
(OUT / "lh20_right_hand.scadpkg").read_bytes()
|
||||
).hexdigest(),
|
||||
"scope": "Independent full-hand pose projection versus actual driven index-finger notebook solve",
|
||||
"leaf_placements_compared": len(errors),
|
||||
"maximum_matrix_entry_error": max(e for _, e in errors),
|
||||
"worst": sorted(errors, key=lambda a: a[1], reverse=True)[:5],
|
||||
}
|
||||
(OUT / "pose_projection_check.json").write_text(json.dumps(result, indent=2))
|
||||
print(json.dumps(result, indent=2))
|
||||
assert result["maximum_matrix_entry_error"] < 1e-5
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user