Robot_Arm/src/armcore.py

# -*- coding: utf-8 -*-
"""Shared constants + CAD/placement helpers for the robot arm build.

All motor interface numbers are MEASURED from
G:/Robot_v2/Model/Solidworks/电机.STEP and 配件/make_v9.py (see docs/interfaces.md).
"""
import math
import json
import os

import cadquery as cq
from OCP.gp import gp_Trsf, gp_Vec, gp_XYZ, gp_Mat, gp_Pnt, gp_Dir, gp_Ax1, gp_Ax2, gp_Quaternion
from OCP.BRepBuilderAPI import BRepBuilderAPI_Transform
from OCP.Bnd import Bnd_Box
from OCP.BRepBndLib import BRepBndLib
from OCP.BRepMesh import BRepMesh_IncrementalMesh
from OCP.BRep import BRep_Tool
from OCP.TopExp import TopExp_Explorer
from OCP.TopAbs import TopAbs_FACE, TopAbs_REVERSED, TopAbs_SOLID
from OCP.TopoDS import TopoDS

V = cq.Vector

# 路径全部由本文件位置推出，仓库可以整体搬迁；三个环境变量是临时覆盖入口。
_HERE = os.path.dirname(os.path.abspath(__file__)).replace("\\", "/")
WORKSPACE = os.environ.get("ARM_WORKSPACE") or os.path.dirname(_HERE)
ROOT = os.environ.get("ARM_ROOT") or os.path.dirname(WORKSPACE)
MODEL = os.environ.get("ARM_MODEL_ROOT") or (ROOT + r"/Model/Solidworks")
SRC_STEP = MODEL + r"/电机.STEP"
HALF_STEP = MODEL + r"/配件/端面齿半联轴器_相同件x2.STEP"
CLAMP_STEP = MODEL + r"/配件/端面齿联轴器_抱箍_两片装配.STEP"
OUT = WORKSPACE + r"/out"
WEB = WORKSPACE + r"/web"
DOCS = WORKSPACE + r"/docs"

# ---------------- MEASURED: motor module (local frame: origin = PCBA rear face centre, +Z = output) ----
M = dict(
    pcba_w=63.6,          # square PCBA across flats
    pcba_t=9.0,           # z 0 -> 9.0
    rearplate_d=60.0,     # z 5.5 -> 10.5
    hous_d=60.0,          # housing OD, z 10.5 -> 52.5
    hous_len=42.0,
    cover_d=60.30,        # top cover OD (R30.15), z 52.5 -> 56.0
    cover_z=56.0,         # cover front face = module total length
    flange_z=54.942,      # 2nd-stage output flange face
    neck_d=36.0,          # female neck bore in the cover
    neck_depth=1.0,
    centre_d=6.5,         # centre bore in the flange
    centre_depth=4.0,
    m2_bcd=29.0,
    m2_clock=(8.05, 128.07, 248.06),
    m2_tap_d=1.85,        # M2 pilot for self-tapping in printed material
    m2_depth=6.0,
    m2_clear_d=2.75,
    m2_cb_d=5.2,
)
# ---------------- MEASURED: face-spline coupling (existing, printed by the user) ------------------------
C = dict(
    n_teeth=12, tooth_r0=19.0, tooth_r1=24.5, tooth_s=2.5,
    flange_t=4.50, part_z=5.560,          # mirrored stack
    hub_r=27.0,
    gap=9.00,                             # cover-face to cover-face when closed
    clamp_od_r=30.0, clamp_len=7.80,
    lug_r1=37.0, lug_t=4.50,
    bolt_r=32.50, bolt_d=3.80,
    z0=1.66, z1=9.46,                     # clamp envelope in the flange frame (motor cover face = 1.060)
)
# axial stations, motor local frame:
COVER_Z = M["cover_z"]                    # 56.0
HUB_FACE_Z = COVER_Z + C["gap"]           # 65.0 = mating face of the driven hub
CLAMP_R = C["clamp_od_r"] + 0.6           # radial keep-out around the joint axis
LUG_R = C["lug_r1"] + 1.0                 # radial keep-out where the bolt lugs are
# ---------------- clearances / print rules -------------------------------------------------------------
CLR = dict(bore_slip=0.20, pilot=0.10, hole_3=3.4, hole_27=2.8, wall_min=2.0)
FIT = dict(motor_bore_d=M["hous_d"] + 2 * 0.15, cover_cb_d=M["cover_d"] + 2 * 0.35)

# 电机【轴向长度】公差（用户 2026-10-07 要求：余量偏大稍微减去 1mm，由 2mm 改为 1mm）。
# 这条链是"后端板螺钉 + 前端压盖螺钉"两个刚性夹持面，螺钉头一旦坐死在沉孔台阶上
# 就再也不能进给，所以公差没法靠"拧紧"消化，只能靠两件事吃掉（见 arm_build.Joint.pocket
# 与 motor_front_clamp）：
#   长出来的 0~+1mm  -> 后端板 4xM3 螺钉头下垫 0~1mm 垫片圈（螺钉被垫高 -> 电机被拉回）
#   短下去的 0~-1mm  -> 电机尾端面与后端板承压面之间垫 0~1mm 垫片（承压面已按 MOTOR_TOL
#                       让空，见 Joint.pocket 的 REAR_SEAT）
# 两边都不动压盖/凸台的几何，关节轴向长度不变。
MOTOR_TOL = 1.0

# ---------------- geometry helpers --------------------------------------------------------------------

def _nrm(v):
    m = v.Magnitude()
    return gp_Vec(v.X() / m, v.Y() / m, v.Z() / m)


def _dot(a, b):
    return a.X() * b.X() + a.Y() * b.Y() + a.Z() * b.Z()


def _perp(e, z):
    d = _dot(e, z)
    return gp_Vec(e.X() - d * z.X(), e.Y() - d * z.Y(), e.Z() - d * z.Z())


def trs(pos=(0, 0, 0), xdir=(1, 0, 0), zdir=(0, 0, 1)):
    """Local->global rigid transform: local +Z -> zdir, local +X -> xdir, y = z x x.
    Uses SetValues (row-major 3x4) because gp_Quaternion / SetCols overloads in this
    OCP build do not map axes the way the docs suggest - verified against known points."""
    z = _nrm(gp_Vec(*zdir))
    x = _perp(gp_Vec(*xdir), z)
    if x.Magnitude() < 1e-9:
        x = _perp(gp_Vec(0, 0, 1), z)
    x = _nrm(x)
    y = z.Crossed(x)
    t = gp_Trsf()
    t.SetValues(x.X(), y.X(), z.X(), pos[0],
                x.Y(), y.Y(), z.Y(), pos[1],
                x.Z(), y.Z(), z.Z(), pos[2])
    return t


def map_pt(t, p):
    v = cq.Vertex.makeVertex(*p)
    w = cq.Shape(BRepBuilderAPI_Transform(v.wrapped, t, True).Shape())
    q = w.Vertices()[0].Center()
    return (q.x, q.y, q.z)


def place(shape, t):
    if shape is None:
        return None
    return cq.Shape(BRepBuilderAPI_Transform(shape.wrapped, t, True).Shape())


def bb(shape):
    b = Bnd_Box()
    BRepBndLib.Add_s(shape.wrapped, b, True)
    lo, hi = b.CornerMin(), b.CornerMax()
    return (lo.X(), lo.Y(), lo.Z()), (hi.X(), hi.Y(), hi.Z())


def bbox_str(shape):
    a, b = bb(shape)
    return "[%.1f %.1f %.1f .. %.1f %.1f %.1f] (%.1fx%.1fx%.1f)" % (
        a[0], a[1], a[2], b[0], b[1], b[2], b[0] - a[0], b[1] - a[1], b[2] - a[2])


def cyl(r, z0, z1, x=0.0, y=0.0):
    return cq.Solid.makeCylinder(r, z1 - z0, V(x, y, z0), V(0, 0, 1))


def ring(r0, r1, z0, z1):
    return cyl(r1, z0, z1).cut(cyl(r0, z0 - 0.001, z1 + 0.001))


def box(x0, x1, y0, y1, z0, z1):
    return cq.Solid.makeBox(x1 - x0, y1 - y0, z1 - z0, V(x0, y0, z0))


def rect_pocket(cx, cy, w, h, z0, z1, rot=0.0):
    s = box(cx - w / 2.0, cx + w / 2.0, cy - h / 2.0, cy + h / 2.0, z0 - 0.001, z1 + 0.001)
    if rot:
        s = s.rotated(V(0, 0, 0), V(0, 0, 1), rot)
    return s


def holes(shape, pts, d, z0, z1):
    """Through/ blind cylindrical cuts at the given (x, y) points."""
    cut = [cyl(d / 2.0, z0, z1, p[0], p[1]) for p in pts]
    return shape.cut(*cut) if cut else shape


def slot_holes(shape, pts, d, cb_d=None, cb_z0=None, cb_z1=None, z0=-1, z1=1):
    cuts = [cyl(d / 2.0, z0, z1, p[0], p[1]) for p in pts]
    if cb_d and cb_z0 is not None:
        cuts += [cyl(cb_d / 2.0, cb_z0, cb_z1, p[0], p[1]) for p in pts]
    return shape.cut(*cuts)


def polygon_pts(r, n, phase=0.0):
    return [(r * math.cos(math.radians(phase + 360.0 * k / n)),
             r * math.sin(math.radians(phase + 360.0 * k / n))) for k in range(n)]


def convex_prism(pts2d, z0, z1):
    """Solid from an ordered 2D outline (assumed convex or simple)."""
    wp = cq.Workplane("XY").workplane(centerSection=(0, 0, z0))
    w2 = [(p[0], p[1]) for p in pts2d]
    r = (cq.Workplane("XY", origin=(0, 0, z0))
         .polyline(w2 + [w2[0]]).close().extrude(z1 - z0, combine=False))
    return r.val()


def fillet_all_edges(shape, r):
    try:
        return shape.edges().fillet(r).val() if isinstance(shape, cq.Workplane) else \
            cq.Workplane(obj=shape).edges().fillet(r).val()
    except Exception:
        return shape


def mesh_of(shape, defl=0.08):
    BRepMesh_IncrementalMesh(shape.wrapped, defl, False, 0.55, True)
    verts, tris, base = [], [], 0
    e = TopExp_Explorer(shape.wrapped, TopAbs_FACE)
    while e.More():
        cur = e.Current()
        f = TopoDS.Face_s(cur)
        tri = BRep_Tool.Triangulation_s(f, cur.Location())
        if tri is not None:
            trsf = cur.Location().Transformation()
            for i in range(1, tri.NbNodes() + 1):
                p = tri.Node(i).Transformed(trsf)
                verts.append((round(p.X(), 2), round(p.Y(), 2), round(p.Z(), 2)))
            rev = f.Orientation() == TopAbs_REVERSED
            for i in range(1, tri.NbTriangles() + 1):
                t = tri.Triangle(i)
                a, b, c = t.Value(1) - 1, t.Value(2) - 1, t.Value(3) - 1
                tris.append((base + a, base + c, base + b) if rev else (base + a, base + b, base + c))
            base += tri.NbNodes()
        e.Next()
    return verts, tris


def solids_of(shape):
    out = []
    e = TopExp_Explorer(shape.wrapped, TopAbs_SOLID)
    while e.More():
        out.append(TopoDS.Solid_s(e.Current()))
        e.Next()
    return [cq.Shape(s) for s in out]


def read_step(p):
    return cq.importers.importStep(p)


def norm_angle(a):
    return (a + 180.0) % 360.0 - 180.0