206 lines
11 KiB
Python
206 lines
11 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""Humanoid arm station solver - single source of truth for human_arm.py.
|
||
|
||
Every joint module occupies the same axial envelope in its own frame:
|
||
s = 0 rear datum (back face of the 63.6 square PCBA)
|
||
s = 0..56 the motor itself (square tail, then the Ø60 housing)
|
||
s = 56.6..64.4 the split clamp band, Ø62 with lugs out to R38
|
||
s = 65.0 the driven hub's mating plane (half B's back face + 4.06 spigot)
|
||
s = 65..74 the driven plate itself (PLATE_T thick, growing away from the motor)
|
||
|
||
so a module plus its joint costs 65 + PLATE_T of axial room and R_MOT = 38 across.
|
||
That is what forces the proportions: the shoulder axis ends up ~460 mm over the
|
||
foot for a 15 mm ground clearance under the fingertips. Everything below is derived
|
||
from those two facts, so changing Z_TIP or L1 re-flows the whole chain.
|
||
"""
|
||
import math
|
||
|
||
try:
|
||
import sys
|
||
sys.stdout.reconfigure(encoding="utf-8")
|
||
except Exception:
|
||
pass
|
||
|
||
HUB = 65.0 # rear datum -> driven hub mating plane (validated: 65.000)
|
||
PLATE_T = 9.0 # driven plate thickness, grows away from the motor
|
||
R_MOT = 38.0 # clamp lug radius = the across-axis envelope
|
||
R_BARREL = 30.0
|
||
GAP = 4.0 # design clearance between two module envelopes
|
||
GAP_MIN = 2.0
|
||
|
||
# ------------------------------------------------------------------ vertical chain
|
||
# Humanoid standard: J1 Roll(+X) -> J2 Pitch(+Y) -> J3 Upper Arm Roll(-Z) -> J4 Elbow Pitch(+Y) -> J5 Wrist Pitch(+Y) -> J6 Tool Roll(-Z)
|
||
FOOT_H = 20.0 # base foot height; the fingertips must stay above it
|
||
Z_RL = 610.0 # J1 shoulder roll axis
|
||
Z_SH = 490.0 # J2 shoulder pitch axis
|
||
Z_R3 = 445.0 # M3 upper arm roll rear datum (-Z axis)
|
||
Z_HUB3 = Z_R3 - HUB # 380.0 J3 hub plane
|
||
Z_EL = 332.0 # J4 elbow pitch axis (+Y axis) - compact direct knuckle
|
||
Z_EI = Z_EL # backward compatibility alias
|
||
FOREARM = 110.0 # shortened forearm length (was 135.0, shortened 25mm per user)
|
||
Z_WR = Z_EL - FOREARM # 222.0 J5 wrist pitch axis (+Y axis)
|
||
Z_R6 = Z_WR - 46.0 # 176.0 M6 tool roll rear datum (-Z axis)
|
||
Z_HUB6 = Z_R6 - HUB # 111.0 J6 hub plane
|
||
Z_TIP = Z_HUB6 - 18.0 # 93.0 P09 tool flange bottom face (73mm clearance above foot)
|
||
L1 = Z_SH - Z_EL # 158.0 humerus length (was 180.0, empty cylinder eliminated)
|
||
|
||
SWEEP_R = 92.0 # farthest point of the humerus from the shoulder pitch axis
|
||
POD_R = 47.0 # saddle drum outer radius about the roll axis
|
||
|
||
X_J1_HUB = 14.0 # J1 hub mating plane, forward of the humerus plate's ±44
|
||
X_BAY_EXT = 3.0 # M1 侧插槽加长量（2026-10-01 用户裁定：插槽开口 5.06+3=8.06，
|
||
# 比电机前端面多 3mm 斜插余量；P03 托架前端 8.5+3=11.5）
|
||
X_FORK_GAP = 1.0 # P04 肩叉前移量：叉板背面(10.5+1.0=11.5)与 P03 托架端面(11.5)
|
||
# 0.0 硬贴平（2026-10-02 用户两次要求"搞成平的"，0.5mm 缝仍被判台阶）。
|
||
# 注意：J1 相对转动时 M2 预紧会经此面产生摩擦阻力，若转动发涩可微松 M2。
|
||
X_ARM = X_J1_HUB + PLATE_T / 2.0 + X_FORK_GAP # 19.5 the limb's own plane in x
|
||
Z_R1 = X_J1_HUB - HUB # -51 J1 rear datum, measured in x: M1 lies behind the shoulder
|
||
Z_CHEST1 = Z_RL - 140.0 # 470 torso mast top
|
||
Z_CHEST0 = FOOT_H
|
||
Z_SH_TOP = Z_RL + R_MOT + GAP # 652 shoulder pod top
|
||
Z_R4 = Z_EL # compatibility
|
||
Z_CAP = Z_HUB3 # compatibility
|
||
Z_M6 = Z_R6 # compatibility
|
||
|
||
# ------------------------------------------------------------------ lateral chain (Y, left arm outboard +Y)
|
||
# Aligned Architecture: all motors centered laterally on Y_AXIS = 128.0 mm
|
||
# J2, J4, J5 face the SAME direction (+Y) with driven hubs on Y_HUB = 165.0 mm!
|
||
Y_AXIS = 128.0 # Shared lateral centerline of the entire limb
|
||
Y_HUB = 165.0 # Shared driven hub mating plane for J2, J4, J5
|
||
Y_REAR = Y_HUB - HUB # 100.0 (Rear datum for M2, M4, M5)
|
||
Y_COVER = Y_REAR + 56.0 # 156.0 (Front cover face for M2, M4, M5)
|
||
|
||
Y_H = Y_AXIS # backward compatibility: limb centre plane
|
||
Y_R_IN = Y_REAR
|
||
Y_R_OUT = Y_COVER
|
||
PL_IN = (Y_COVER, Y_HUB)
|
||
PL_OUT = (Y_COVER, Y_HUB)
|
||
Y_M6 = Y_AXIS
|
||
Y_CHEST = 62.0 # mast half width
|
||
X_CHEST = 6.0 # chest plate half thickness
|
||
|
||
# ------------------------------------------------------------------ 台架（非常规构件）
|
||
# P01 立柱 / P02 底盘 是【固定测试台架】，不是机器人本体的一部分（用户 2026-10-02
|
||
# 确认）；它在扫描中代表机械臂与**躯干/机身连接界面**的那块实体。因此：
|
||
#
|
||
# - 臂撞立柱 = 臂撞躯干，是真约束，不能忽略。sweep_v2 报出的组合卡死（J3 ±162°、
|
||
# J4 ±18°、J5 ±36° 等，全部是工具法兰 P09 撞立柱 P01_A）就是这类真约束。
|
||
# - 但台架的**位置与截面是一个假设**。躯干侧工作空间只和这个假设一样准：真机躯干
|
||
# 挪后一点或外形不同，这些限位就变。所以台架本身不要为了放宽行程去改（那是改测试
|
||
# 工装，不是改设计）；要改的是"真机躯干占位"这个输入。
|
||
# - `--no-stand` 仅用于诊断：给出不含躯干约束的自由空间行程，用来分辨某个限位到底是
|
||
# 臂自身折叠产生的、还是躯干挡的。默认不排除。
|
||
FIXTURE_PREFIX = ("P01_", "P02_")
|
||
|
||
|
||
def is_fixture(name):
|
||
"""该零件是否属于固定测试台架（代表躯干/安装界面，非机器人本体）。"""
|
||
return name.startswith(FIXTURE_PREFIX)
|
||
|
||
|
||
# ------------------------------------------------------------------ 关节表
|
||
# tag, cn, rear, d, xdir, holder of the motor, driven part
|
||
# J1 roll +X fore-aft, in the shoulder base -> drives P04 肩叉
|
||
# J2 pitch +Y outboard in the fork -> drives P05 大臂旋转座
|
||
# J3 roll -Z in the shoulder roll base -> drives P06 大臂骨架
|
||
# J4 pitch +Y in the humerus bone -> drives P07 小臂骨架
|
||
# J5 pitch +Y in the forearm bone -> drives P08 腕偏转座
|
||
# J6 roll -Z in the wrist bracket -> drives P09 工具法兰
|
||
JOINTS = [
|
||
("J1", "肩抬举", (Z_R1, Y_AXIS, Z_RL), (1, 0, 0), (0, 1, 0), "P03_肩吊臂_螺旋体35%_墙5底5_竖打", "P04_肩叉_螺旋体30%_墙5底5_竖打"),
|
||
("J2", "肩俯仰", (X_ARM, Y_REAR, Z_SH), (0, 1, 0), (1, 0, 0), "P04_肩叉_螺旋体30%_墙5底5_竖打", "P05_大臂旋转座_螺旋体30%_墙4底5_竖打"),
|
||
("J3", "大臂自转", (X_ARM, Y_AXIS, Z_R3), (0, 0, -1), (1, 0, 0), "P05_大臂旋转座_螺旋体30%_墙4底5_竖打", "P06_大臂骨架_螺旋体25%_墙4底5_竖打"),
|
||
("J4", "肘部俯仰", (X_ARM, Y_REAR, Z_EL), (0, 1, 0), (1, 0, 0), "P06_大臂骨架_螺旋体25%_墙4底5_竖打", "P07_小臂骨架_螺旋体30%_墙5底5_竖打"),
|
||
("J5", "腕部俯仰", (X_ARM, Y_REAR, Z_WR), (0, 1, 0), (1, 0, 0), "P07_小臂骨架_螺旋体30%_墙5底5_竖打", "P08_腕偏转座_螺旋体25%_墙4底5_竖打"),
|
||
("J6", "腕自转", (X_ARM, Y_AXIS, Z_R6), (0, 0, -1), (1, 0, 0), "P08_腕偏转座_螺旋体25%_墙4底5_竖打", "P09_工具法兰_螺旋体30%_墙4底5_竖打"),
|
||
]
|
||
AXIS = {t[0]: dict(rear=t[2], d=t[3], xdir=t[4]) for t in JOINTS}
|
||
|
||
|
||
def hub_plane(tag):
|
||
j = AXIS[tag]
|
||
return tuple(j["rear"][i] + j["d"][i] * HUB for i in range(3))
|
||
|
||
|
||
def seg_dist(p1, q1, p2, q2):
|
||
"""Minimum distance between two 3-D segments (parameters clamped to [0,1])."""
|
||
d1 = [q1[i] - p1[i] for i in range(3)]
|
||
d2 = [q2[i] - p2[i] for i in range(3)]
|
||
r = [p1[i] - p2[i] for i in range(3)]
|
||
a = sum(x * y for x, y in zip(d1, d1))
|
||
e = sum(x * y for x, y in zip(d2, d2))
|
||
f = sum(x * y for x, y in zip(d2, r))
|
||
b = sum(x * y for x, y in zip(d1, d2))
|
||
s = sum(x * y for x, y in zip(d1, r))
|
||
if a < 1e-12:
|
||
t0 = 0.0
|
||
t1 = max(0.0, min(1.0, f / e)) if e > 1e-12 else 0.0
|
||
elif e < 1e-12:
|
||
t1 = 0.0
|
||
t0 = max(0.0, min(1.0, -s / a))
|
||
else:
|
||
den = a * e - b * b
|
||
if abs(den) < 1e-12:
|
||
t0 = 0.0
|
||
t1 = max(0.0, min(1.0, f / e))
|
||
else:
|
||
t0 = max(0.0, min(1.0, (b * f - e * s) / den))
|
||
t1 = max(0.0, min(1.0, (e * f - b * s) / den))
|
||
c = [p1[i] + d1[i] * t0 - (p2[i] + d2[i] * t1) for i in range(3)]
|
||
return math.hypot(*c)
|
||
|
||
|
||
def columns():
|
||
"""Each module as a segment (rear datum -> hub plane) plus its across-axis radius."""
|
||
out = {}
|
||
for tag, cn, rear, d, xd, holder, driven in JOINTS:
|
||
q = tuple(rear[i] + d[i] * HUB for i in range(3))
|
||
out[tag] = (rear, q, R_MOT)
|
||
return out
|
||
|
||
|
||
def relabel():
|
||
"""The designed 1-D relations that set the stations. Each pair is separated along
|
||
one global coordinate because the two columns are perpendicular or collinear there."""
|
||
C = {}
|
||
C["pod drum clear of the pitch sweep"] = (Z_RL - POD_R) - (Z_SH + SWEEP_R)
|
||
C["bridge vs elbow sleeve"] = (Y_R_IN + 8.0) - 96.0
|
||
C["cheek vs arm x envelope"] = 64.0 - (X_ARM + 42.0)
|
||
C["fingertip above the foot"] = Z_TIP - FOOT_H
|
||
C["roll motor rear below the elbow motor"] = (Z_EI - R_MOT) - Z_R4
|
||
C["wrist-cap plate above the wrist motor"] = (Z_CAP - PLATE_T) - (Z_WR + R_MOT)
|
||
C["fork plate above the humerus plate"] = (Z_RL - 42.0) - (Z_SH + 44.0)
|
||
C["gripper motor below the wrist motor"] = (Z_WR - R_MOT) - Z_M6
|
||
C["gripper clamp band vs palm plate"] = PL_OUT[0] - (Y_M6 + 31.0)
|
||
C["elbow motor rear vs chest plate edge"] = Y_R_IN - Y_CHEST
|
||
C["gripper tail vs chest plate edge"] = (Y_M6 - 36.0) - Y_CHEST
|
||
C["fingertip to ground"] = Z_TIP
|
||
return C
|
||
|
||
|
||
def report():
|
||
print("vertical chain: tip=%.0f J6hub=%.0f J5axis=%.0f J4hub=%.0f J4rear=%.0f "
|
||
"J3axis=%.0f J2axis=%.0f J1roll=%.0f deck=%.0f pod top=%.0f"
|
||
% (Z_TIP, Z_M6 - HUB, Z_WR, Z_CAP, Z_R4, Z_EI, Z_SH, Z_RL, Z_CHEST1, Z_SH_TOP))
|
||
print("lateral: Y_R_IN=%.0f Y_H=%.0f Y_R_OUT=%.0f Y_M6=%.0f chest half-width=%.0f\n"
|
||
% (Y_R_IN, Y_H, Y_R_OUT, Y_M6, Y_CHEST))
|
||
for tag, cn, rear, d, xd, holder, driven in JOINTS:
|
||
h = hub_plane(tag)
|
||
print(" %-3s %-5s rear=(%6.1f,%6.1f,%6.1f) d=(%2d,%2d,%2d) hub=(%6.1f,%6.1f,%6.1f) %s -> %s"
|
||
% (tag, cn, rear[0], rear[1], rear[2], d[0], d[1], d[2], h[0], h[1], h[2], holder, driven))
|
||
print("\n-- designed relations (each must be >= %.0f mm) --" % GAP_MIN)
|
||
bad = 0
|
||
for k, v in relabel().items():
|
||
if v < GAP_MIN:
|
||
bad += 1
|
||
print(" %+-7.1f %-46s %s" % (v, k, "OK" if v >= GAP_MIN else "*** CLASH ***"))
|
||
print("\n humerus L1=%.0f forearm(elbow->wrist)=%.0f hand(wrist->tip)=%.0f"
|
||
% (L1, Z_EI - Z_WR, Z_WR - Z_TIP))
|
||
print(" shoulder->tip = %.0f mm height to pod top = %.0f mm torso %.0f..%.0f"
|
||
% (Z_SH - Z_TIP, Z_SH_TOP, Z_CHEST0, Z_CHEST1))
|
||
return bad
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(1 if report() else 0)
|