sunxianghui
93773f3887
Includes FastAPI backend, vendored step2urdf frontend, and A7 handtuned arm JSON for URDF generation.
121 lines
4.4 KiB
Python
121 lines
4.4 KiB
Python
"""A7 / ARM7 gold kinematics from lkls73_i1_arm_description/urdf/ARM7_urdf.urdf.
|
||
|
||
STEP Viewer / step2urdf UI stores linear joint origins in **millimetres**;
|
||
export applies ×0.001 → metres. Values below are therefore in mm so they match
|
||
the interactive UI and still export to the gold URDF metre numbers.
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
from app.models import JointDef, LinkDef, RobotDraft
|
||
|
||
# UI / CAD units (mm). Gold URDF metres × 1000.
|
||
A7_LINK_ORDER = [
|
||
"base_link",
|
||
"A1_Link",
|
||
"A2_Link",
|
||
"A3_Link",
|
||
"A4_Link",
|
||
"A5_Link",
|
||
"A6_Link",
|
||
"A7_Link",
|
||
"A8_Link",
|
||
]
|
||
|
||
# (name, parent, child, type, xyz_mm, axis, lower, upper, effort, velocity)
|
||
A7_GOLD_JOINTS_MM: list[tuple] = [
|
||
("A1_joint", "base_link", "A1_Link", "revolute", [0.0, 0.0, 9.0], [0.0, 0.0, -1.0], -2.9, 1.0, 80.0, 10.47),
|
||
("A2_joint", "A1_Link", "A2_Link", "revolute", [0.0, 0.0, 59.5], [1.0, 0.0, 0.0], -0.15, 3.14, 80.0, 10.47),
|
||
("A3_joint", "A2_Link", "A3_Link", "revolute", [0.0, 0.0, 123.0], [0.0, 0.0, -1.0], -2.35, 2.35, 36.0, 9.1),
|
||
("A4_joint", "A3_Link", "A4_Link", "revolute", [0.0, -0.10871, 187.1], [1.0, 0.0, 0.0], 0.0, 2.2, 36.0, 9.1),
|
||
("A5_joint", "A4_Link", "A5_Link", "revolute", [0.0, -0.13906, 124.0], [0.0, 0.0, -1.0], -2.35, 2.35, 36.0, 9.1),
|
||
("A6_joint", "A5_Link", "A6_Link", "revolute", [0.0, -0.20755, 88.099], [1.0, 0.0, 0.0], -1.57, 1.57, 36.0, 9.1),
|
||
("A7_joint", "A6_Link", "A7_Link", "revolute", [0.60033, 0.45533, 65.501], [0.0, 1.0, 0.0], -1.57, 1.57, 36.0, 9.1),
|
||
("A8_joint", "A7_Link", "A8_Link", "fixed", [-0.041388, -1.1787, 44.999], [0.0, 0.0, 0.0], 0.0, 0.0, 0.0, 0.0),
|
||
]
|
||
|
||
|
||
def gold_joint_defs() -> list[JointDef]:
|
||
out: list[JointDef] = []
|
||
for name, parent, child, jtype, xyz, axis, lo, hi, eff, vel in A7_GOLD_JOINTS_MM:
|
||
out.append(
|
||
JointDef(
|
||
name=name,
|
||
joint_type=jtype, # type: ignore[arg-type]
|
||
parent=parent,
|
||
child=child,
|
||
origin_xyz=list(xyz),
|
||
origin_rpy=[0.0, 0.0, 0.0],
|
||
axis=list(axis),
|
||
lower=lo,
|
||
upper=hi,
|
||
effort=eff,
|
||
velocity=vel,
|
||
rationale="ARM7_urdf.urdf gold kinematics (origins in mm for step2urdf UI).",
|
||
)
|
||
)
|
||
return out
|
||
|
||
|
||
def empty_gold_links() -> list[LinkDef]:
|
||
return [LinkDef(name=n, part_names=[]) for n in A7_LINK_ORDER]
|
||
|
||
|
||
def enforce_a7_gold_kinematics(draft: RobotDraft) -> RobotDraft:
|
||
"""Keep part_names from draft when link names match; always replace joints with gold.
|
||
|
||
Also remaps common aliases (link1 → A1_Link, etc.) so LLM/profile seeds still work.
|
||
"""
|
||
alias = {
|
||
"link1": "A1_Link",
|
||
"link2": "A2_Link",
|
||
"link3": "A3_Link",
|
||
"link4": "A4_Link",
|
||
"link5": "A5_Link",
|
||
"link6": "A6_Link",
|
||
"link7": "A7_Link",
|
||
"link_1": "A1_Link",
|
||
"link_2": "A2_Link",
|
||
"link_3": "A3_Link",
|
||
"link_4": "A4_Link",
|
||
"link_5": "A5_Link",
|
||
"link_6": "A6_Link",
|
||
"link_7": "A7_Link",
|
||
"a1_link": "A1_Link",
|
||
"a2_link": "A2_Link",
|
||
"a3_link": "A3_Link",
|
||
"a4_link": "A4_Link",
|
||
"a5_link": "A5_Link",
|
||
"a6_link": "A6_Link",
|
||
"a7_link": "A7_Link",
|
||
"a8_link": "A8_Link",
|
||
}
|
||
|
||
buckets: dict[str, list[str]] = {n: [] for n in A7_LINK_ORDER}
|
||
for link in draft.links:
|
||
key = alias.get(link.name, alias.get(link.name.lower(), link.name))
|
||
if key not in buckets:
|
||
# Unknown link → fold into base
|
||
key = "base_link"
|
||
for pn in link.part_names:
|
||
if pn not in buckets[key]:
|
||
buckets[key].append(pn)
|
||
|
||
links = [LinkDef(name=n, part_names=buckets[n]) for n in A7_LINK_ORDER]
|
||
joints = gold_joint_defs()
|
||
notes = list(draft.notes) + [
|
||
"A7 关节轴/原点已强制替换为 ARM7 金标（原点单位 mm，导出时 ×0.001 → m）。",
|
||
"智谱仅用于零件归组；请勿依赖模型猜测的 axis/rpy。",
|
||
"轴规律: A1/A3/A5 → [0,0,-1]; A2/A4/A6 → [1,0,0]; A7 → [0,1,0]; A8 fixed。",
|
||
]
|
||
return RobotDraft(
|
||
name=draft.name or "ARM7_urdf",
|
||
profile="a7",
|
||
links=links,
|
||
joints=joints,
|
||
notes=notes,
|
||
raw_parts=draft.raw_parts,
|
||
axis_candidates=draft.axis_candidates,
|
||
llm_raw=draft.llm_raw,
|
||
)
|