cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
1dfd48278b56de5e60b818229623ac7b2c522757
Robot_Arm/src/arm_build.py
T

708 lines
33 KiB
Python
Raw Normal View History

初始提交：机械臂 CadQuery 建模源码、设计文档与 3D 查看器
2026-09-26 18:05:21 +08:00
# -*- coding: utf-8 -*-
"""60 mm gearmotor -> 5-DOF printable robot arm (J1 yaw, J2 shoulder, J3 elbow,
J4 wrist pitch, J5 gripper).
Joint recipe (measured, see docs/interfaces.md):
proximal link holds the motor in a Ø60.3 slip bore + 64.4 square PCBA pocket +
4 radial M3 pinch screws; motor rear datum + 65.0 = the driven link's hub mating
plane, whose face replicates the motor output (Ø36x1.0 neck, Ø6.5x4.0 centre,
3x M2 on Ø29) so the existing face-spline halves + split clamp bolt straight on.
Run: python arm_build.py -> out/, web/mesh_arm.json, docs/
"""
import math
import os
import sys
import json
try:
sys.stdout.reconfigure(encoding="utf-8")
except Exception:
pass
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import cadquery as cq
from armcore import (M, C, V, trs, place, bb, bbox_str, cyl, ring, box, holes,
solids_of, mesh_of, OUT, WEB, DOCS, SRC_STEP, HALF_STEP, CLAMP_STEP)
from OCP.gp import gp_Trsf, gp_Vec, gp_Pnt, gp_Dir, gp_Ax1
from OCP.BRepAlgoAPI import BRepAlgoAPI_Common
from OCP.GProp import GProp_GProps
from OCP.BRepGProp import BRepGProp
from OCP.STEPControl import STEPControl_Writer, STEPControl_Controller, STEPControl_StepModelType
from OCP.Interface import Interface_Static
STEPControl_Controller.Init_s()
D2R = math.pi / 180.0
VERSION = os.environ.get("ARMVER", "r1")
ITER = int(os.environ.get("ARMITER", "1"))
# ---------------------------------------------------------------- measured stations
FLANGE_Z = M["flange_z"] # 54.942
COVER_Z = M["cover_z"] # 56.000
HUB_FACE_Z = 65.0 # motor rear datum -> driven hub mating plane
CLAMP_SPAN = (FLANGE_Z + 1.66, FLANGE_Z + 9.46)
CLAMP_R_BAND = 31.0
CLAMP_R_LUG = 38.0
BORE_R = M["hous_d"] / 2.0 + 0.15 # 30.15 slip fit on the housing
CB_R = M["cover_d"] / 2.0 + 0.35 # 30.50 clearance for the cover
PCB_W = M["pcba_w"] + 0.8 # 64.40 square pocket
HUB_M2_CLOCK = [(360.0 - a) % 360.0 for a in M["m2_clock"]] # phase of the mirrored half
HUB_PILOT_D = M["neck_d"] - 0.4 # 35.6 -> the half's pilot lands in a Ø36 recess
PINCH_D = 2.6 # M3 clearance in printed material
# ---------------------------------------------------------------- layout (ground X fwd, Y left, Z up)
PLATE_T = 9.0
ARM_L1, ARM_L2 = 175.0, 165.0
EL_A, FA_A = 58.0, 8.0
SH = (0.0, 125.0)
EL = (SH[0] + ARM_L1 * math.cos(EL_A * D2R), SH[1] + ARM_L1 * math.sin(EL_A * D2R))
WR = (EL[0] + ARM_L2 * math.cos(FA_A * D2R), EL[1] + ARM_L2 * math.sin(FA_A * D2R))
DIR_U = (math.cos(EL_A * D2R), math.sin(EL_A * D2R))
DIR_F = (math.cos(FA_A * D2R), math.sin(FA_A * D2R))
PERP_F = (-DIR_F[1], DIR_F[0]) # "up" in the arm plane for the wrist
Y0 = -26.0
Y_UP_IN = Y0 + HUB_FACE_Z
Y_UP_OUT = Y_UP_IN + PLATE_T
Y3_REAR = Y_UP_IN
Y_FA_OUT = Y3_REAR - HUB_FACE_Z - PLATE_T
Y_FA_IN = Y_FA_OUT + PLATE_T
Y4_REAR = Y_FA_IN
Y_WM_IN = Y4_REAR + HUB_FACE_Z
Y_WM_OUT = Y_WM_IN + PLATE_T
Y_M5, S0_M5 = -2.0, 58.0
BRACKET_BOLT = [(-26.0, Y0 - 3.0), (0.0, Y0 - 3.0), (26.0, Y0 - 3.0)] # turret <-> shoulder wall
COLUMN_BOLT = [(35.0 * math.cos(a * D2R), 35.0 * math.sin(a * D2R)) for a in (30, 150, 270)]
PARTS = []
CENTS = {}
def add(name, cn, shape, color, printed=True, note="", qty=1):
assert shape is not None, name
lo, hi = bb(shape)
PARTS.append(dict(name=name, cn=cn, shape=shape, color=color, printed=printed,
note=note, qty=qty, bb=bbox_str(shape), vol=vol_of(shape)))
CENTS[name] = [(lo[i] + hi[i]) / 2.0 for i in range(3)]
def vol_of(sh):
p = GProp_GProps()
BRepGProp.VolumeProperties_s(sh.wrapped, p)
return p.Mass()
def common_vol(a, b):
try:
c = BRepAlgoAPI_Common(a.wrapped, b.wrapped)
c.Build()
if not c.IsDone() or c.Shape().IsNull():
return 0.0
return vol_of(cq.Shape(c.Shape()))
except Exception:
return -1.0
def T(dx=0.0, dy=0.0, dz=0.0):
t = gp_Trsf()
t.SetTranslation(gp_Vec(dx, dy, dz))
return t
def R(deg, ax, ay, az, px=0.0, py=0.0, pz=0.0):
t = gp_Trsf()
t.SetRotation(gp_Ax1(gp_Pnt(px, py, pz), gp_Dir(ax, ay, az)), deg * D2R)
return t
RX90 = R(90, 1, 0, 0)
def Ycyl(r, y0, y1, px=0.0, pz=0.0):
return place(cyl(r, -y1, -y0, px, pz), RX90)
def Ybox(x0, x1, y0, y1, z0, z1):
return place(box(x0, x1, z0, z1, -y1, -y0), RX90)
# ------------------------------------------------------- pitch-joint pivot cartridge ----
# ONE recipe for every pitch joint (J2 shoulder, J3 elbow, J5 wrist) - they are the same
# degree of freedom, so an implementation that fixes only one of them leaves the identical
# defect in the other two. See human_layout PV_* for why a cartridge is needed at all:
# measured P04 n P05 = 0.00 mm3, so J2's 38 N*m had no path into the housing except peeling
# the face-spline teeth off across the print layers.
def pivot_geom(y_face, side, h, w, gap):
"""(boss_y0, boss_y1, pocket_y0) for a cartridge against the link face at y_face.
The race pocket always opens AWAY from the link, leaving an (h-w) shoulder behind the
outer race; a printed cap ring closes the open end. The boss stops `gap` short of the
link face, or a printed face would carry the thrust and the rolling elements nothing."""
near = y_face - gap if side < 0 else y_face + gap
if side < 0:
return near - h, near, near - h
return near, near + h, near + h - w
def pivot_boss(shape, pz, y0, y1, pk0, w, px, boss_od, race_od, bore, orace_id, side):
"""Housing side: boss on the joint axis with the outer race sunk into it. The passage
beyond the pocket is reamed to the outer race bore, not to the shaft: a shaft-sized bore
would end inside the inner race and bind it, and a bound bearing carries nothing - it
just moves the failure back into the print layers."""
shape = shape.fuse(Ycyl(boss_od / 2.0, y0, y1, px, pz))
shape = shape.cut(Ycyl(race_od / 2.0, pk0 - .01, pk0 + w + .01, px, pz))
if side < 0:
shape = shape.cut(Ycyl(orace_id / 2.0 + 0.1, pk0 + w, y1 + .01, px, pz))
else:
shape = shape.cut(Ycyl(orace_id / 2.0 + 0.1, y0 - .01, pk0, px, pz))
shape = shape.cut(Ycyl(bore / 2.0 + 0.2, y0 - .01, y1 + .01, px, pz))
for a in (90.0, 210.0, 330.0): # 3x M3 cap screws, outside the outer race
u, v = 26.5 * math.cos(a * math.pi / 180.0), 26.5 * math.sin(a * math.pi / 180.0)
shape = shape.cut(Ycyl(1.25, y0 - .01, y0 + 12.0, px + u, pz + v))
return shape
def pivot_journal(shape, pz, y_link, y_tip, y_collar0, y_collar1, px, bore, collar):
"""Link side: the stub the inner race rides on, poking past the boss so the cap can bite
it, plus an integral collar the inner race bears against. The collar replaces a separate
spacer sleeve and lives in the boss's outer-race passage, so it must stay under that
diameter - the 0.1 mm it leaves is what lets the race, not the collar, take the thrust."""
a, b = sorted((y_link, y_tip))
shape = shape.fuse(Ycyl(bore / 2.0, a, b, px, pz))
c0, c1 = sorted((y_collar0, y_collar1))
return shape.fuse(Ycyl(collar / 2.0, c0, c1, px, pz))
def prism(pts2, z0, z1):
w2 = [(p[0], p[1]) for p in pts2]
return (cq.Workplane("XY", origin=(0, 0, z0))
.polyline(w2 + [w2[0]]).close().extrude(z1 - z0, combine=False)).val()
def Yplate(pts_xz, y0, y1):
return place(prism(pts_xz, -y1, -y0), RX90)
def dogbone(a, b, ra, rb, n=20):
"""Tangent hull of two discs. The outer-tangent offset angle is acos((ra-rb)/L);
using asin made the ra==rb case sweep a full circle around b and self-intersect."""
dx, dy = b[0] - a[0], b[1] - a[1]
L = math.hypot(dx, dy) or 1.0
th = math.atan2(dy, dx)
beta = math.acos(max(-1.0, min(1.0, (ra - rb) / L)))
sw_a = 2 * math.pi - 2 * beta
sw_b = 2 * beta
pts = [(a[0] + ra * math.cos(th + beta + sw_a * k / n), a[1] + ra * math.sin(th + beta + sw_a * k / n))
for k in range(n + 1)]
pts += [(b[0] + rb * math.cos(th - beta + sw_b * k / n), b[1] + rb * math.sin(th - beta + sw_b * k / n))
for k in range(1, n + 1)]
return pts
def sector(r_out, r_in, a0, a1, z0, z1, n=14):
"""Annular sector between two radii and two angles (deg) - a joint shroud segment.
Must NOT include the axis point, or this becomes a solid pie wedge that fills the joint."""
pts = [(r_out * math.cos((a0 + (a1 - a0) * k / n) * D2R),
r_out * math.sin((a0 + (a1 - a0) * k / n) * D2R)) for k in range(n + 1)]
pts += [(r_in * math.cos((a1 - (a1 - a0) * k / n) * D2R),
r_in * math.sin((a1 - (a1 - a0) * k / n) * D2R)) for k in range(n + 1)]
return prism(pts, z0, z1)
# ---------------------------------------------------------------- joint frames & interfaces
class Joint:
"""One revolute joint. `rear` = motor rear datum point, `d` = axis unit dir (motor -> hub)."""
def __init__(self, jid, cn, rear, d, xdir, lug_phase=0.0):
self.jid, self.cn = jid, cn
self.rear = rear
self.d = d
self.xdir = xdir
self.t = trs(rear, xdir, d) # motor-local -> global
self.lug_phase = lug_phase # rotation of the clamp about the axis
def L(self, shape_local):
return place(shape_local, self.t)
def at(self, s):
return (self.rear[0] + self.d[0] * s, self.rear[1] + self.d[1] * s, self.rear[2] + self.d[2] * s)
def pocket(self, shape, front_pad=0.6):
"""Cut the motor pocket out of a proximal link body."""
c = [cyl(BORE_R, 5.5, COVER_Z - 3.5), cyl(CB_R, COVER_Z - 3.5, COVER_Z + front_pad),
box(-PCB_W / 2, PCB_W / 2, -PCB_W / 2, PCB_W / 2, -0.6, M["pcba_t"] + 0.4)]
return shape.cut(*[self.L(s) for s in c])
def rear_mount_holes(self, shape, t_plate=8.0, hole_d=2.8, cb_d=5.5, cb_depth=4.0):
"""Cut the 4x rear mounting bolt holes into a proximal link body.
Matches the 4x tapped holes on the motor rear metal endplate:
(±17.55, ±20.46) on the rear face at motor-local z = 0.
"""
cuts = []
for sx in (-17.55, 17.55):
for sy in (-20.46, 20.46):
h = cyl(hole_d / 2.0, -t_plate - 1.0, 3.0, sx, sy)
cuts.append(self.L(h))
if cb_d > 0 and cb_depth > 0:
cb = cyl(cb_d / 2.0, -t_plate - 1.0, -t_plate + cb_depth, sx, sy)
cuts.append(self.L(cb))
return shape.cut(*cuts)
def front_collar_pocket(self, shape, r_bore=30.25, z0=50.0, z1=56.5):
"""Cut front locating collar pocket for the motor front rim."""
p = cyl(r_bore, z0, z1)
return shape.cut(self.L(p))
def pinch(self, shape, fracs=(0.26, 0.58, 0.86), r_out=48.0, n=2):
"""Radial M3 pinch screws through the boss wall onto the housing."""
cuts = []
for fr in fracs[:n]:
for a in (self.lug_phase, self.lug_phase + 180.0):
c = cyl(PINCH_D / 2.0, -1.0, r_out, 0, 0)
c = place(c, R(a, 0, 0, 1))
c = place(c, T(0, 0, fr * COVER_Z))
cuts.append(self.L(c))
return shape.cut(*cuts)
def hub_face(self):
return self.at(HUB_FACE_Z)
def cut_hub(self, shape, depth=24.0, r_clear=32.0):
"""Cut the motor-output replica into the driven hub. The mating plane sits at
motor-local z = HUB_FACE_Z, and the recesses open there and run into the driven
material (away from the motor), so that half B's pilot (0.60) and spigot (4.06)
land in them."""
H = HUB_FACE_Z
# measured on the delivered half B: past the mating plane it is Ø35.6 for 1.4 mm
# then a Ø6.3 spigot to 4.06 - the nominal neck_depth of 1.0 is 0.4 too shallow,
# which showed up as a ~160 mm3 interference at every driven hub
cuts = [cyl(M["neck_d"] / 2.0, H - 0.05, H + 1.70),
cyl(M["centre_d"] / 2.0, H - 0.05, H + 5.05),
# probed on the delivered half: its Ø54 back face sits ~0.06 mm PAST the
# nominal plane, so the driven face gets a 0.35 deep, Ø54.6 relief. That is
# also the printed-part tolerance land the joint wants.
cyl(27.30, H - 0.05, H + 0.35)]
for a in HUB_M2_CLOCK:
u = M["m2_bcd"] / 2.0 * math.cos(a * D2R)
v = M["m2_bcd"] / 2.0 * math.sin(a * D2R)
cuts.append(cyl(M["m2_tap_d"] / 2.0, H - 0.05, H + M["m2_depth"], u, v))
return shape.cut(*[self.L(s) for s in cuts])
def hub_island(self, shape, r=30.0, t=PLATE_T + 0.5):
"""Solid disc at the joint centre (the lightening dish must not break into the
interface recesses), then the output replica is cut into it. Starts exactly on the
mating plane: half B's Ø54 body ends there, so any overlap is a hard clash."""
shape = shape.fuse(self.L(cyl(r, HUB_FACE_Z, HUB_FACE_Z + t)))
shape = self.cut_hub(shape)
# two arc shrouds over the clamp band: the joint reads as a closed drum, and the
# sectors are placed so the M3 lug bolts on the joint's local +-X stay reachable
for (a0, a1) in ((34.0, 146.0), (214.0, 326.0)):
shape = shape.fuse(self.L(sector(38.0, 31.6, a0, a1, FLANGE_Z + 1.4, HUB_FACE_Z)))
return shape
def place_hardware(self, half, clamp, tag):
"""Place the existing coupling halves + split clamp for this joint.
The delivered half is authored with z=0 at the motor flange face and spans
z -3.00..8.06. B = flip 180 deg about the local X axis, then shift by
2*PART_Z, which puts its flat back face exactly on the hub mating plane
(motor-local 65.00) - verified against the measured feature bounds.
"""
tf = trs(self.at(FLANGE_Z), self.xdir, self.d)
hA = place(half, tf)
hb = place(half, R(180, 1, 0, 0))
hb = place(hb, T(0, 0, 2 * C["part_z"]))
hB = place(hb, tf)
cA = place(clamp, tf)
cB = place(place(clamp, R(180, 0, 0, 1)), tf) # the two C pieces are one ring split
# along a diametral plane: 180 deg about the JOINT axis, so both keep the same
# axial span 56.602..64.402 and sit on opposite sides of the band.
return [("%s_半联轴器A" % tag, hA), ("%s_半联轴器B" % tag, hB),
("%s_抱箍A" % tag, cA), ("%s_抱箍B" % tag, cB)]
# ---------------------------------------------------------------- printed part builders
def p_base_plate():
s = cyl(76, 0, 8).cut(cyl(25, -0.01, 8.01))
for a in (45, 135, 225, 315):
u, v = 58 * math.cos(a * D2R), 58 * math.sin(a * D2R)
s = s.cut(cyl(2.75, -0.01, 8.01, u, v)).cut(cyl(5.5, -0.01, 2.8, u, v))
for a in range(0, 360, 45):
u, v = 68 * math.cos(a * D2R), 68 * math.sin(a * D2R)
s = s.cut(cyl(4.0, -0.01, 3.0, u, v))
return s
def p_column(j1):
"""Ø88 column z 8..64 holding M1; open at the bottom so it slides over the motor."""
s = cyl(44, 8, 64)
for a in (0, 90, 180, 270):
blk = box(21, 45, -12, 12, 22, 50)
blk = place(blk, R(a, 0, 0, 1))
s = s.cut(blk)
s = j1.pocket(s, front_pad=0.0)
s = j1.pinch(s, n=2)
# J1 hub must be able to pull up: top face keeps a Ø61 recess ring for the cover
s = s.cut(ring(30.6, 36.0, 63.4, 64.01))
# cable duct from the base to the turret
duct = box(-9, 9, -46, -30, 20, 64)
s = s.cut(duct)
return s
def p_turret(j1):
"""J1 driven hub: mating face at z=73, plate 73..82, carries the shoulder bracket."""
face = j1.hub_face()
zf = 73.0
s = cyl(52, zf, zf + PLATE_T)
s = s.cut(ring(38.5, 52.0, zf - 0.01, zf + 2.2)) # dish the underside for the clamp
for a in range(30, 360, 60): # lightening (off the bolt lines)
u, v = 40 * math.cos(a * D2R), 40 * math.sin(a * D2R)
s = s.cut(cyl(7.0, zf + 2.4, zf + PLATE_T - 2.4, u, v))
s = j1.hub_island(s, 30.0, PLATE_T - 0.02)
for (u, v) in COLUMN_BOLT: # 3x M3 down into the column
s = s.cut(cyl(1.6, zf - 0.01, zf + PLATE_T + 0.01, u, v))
for (u, v) in BRACKET_BOLT: # 3x M3 up for the shoulder wall
s = s.cut(cyl(1.55, zf + PLATE_T - 6.0, zf + PLATE_T + 0.01, u, v))
return s
def p_shoulder_bracket(j2):
"""Wall on the turret top + motor sleeve; holds M2 (axis +Y at SH)."""
zt = 82.0
y0, y1 = Y0 - 6.0, Y0
s = Ybox(SH[0] - 40, SH[0] + 40, y0, y1, zt + 0.02, SH[1] + 39.0)
s = s.fuse(Ycyl(38.0, Y0, Y0 + COVER_Z - 3.0, SH[0], SH[1]))
s = s.fuse(Ybox(SH[0] - 38, SH[0] + 38, Y0, Y0 + 6.0, SH[1] - 20, SH[1] + 20))
for sx in (-1, 1): # feet down onto the turret
s = s.fuse(Ybox(sx * 26 - 6, sx * 26 + 6, y0, y1, zt + 0.02, zt + 30.0))
s = j2.pocket(s) # relieve AFTER every fuse
s = j2.pinch(s, n=2)
for (bx_, by_) in BRACKET_BOLT: # 3x M3 down into the turret
s = s.cut(cyl(1.7, zt + 0.01, zt + 8.0, bx_, by_))
return s
def p_upper_arm(j2, j3):
"""Plate normal Y, y 39..48. J2 hub at the shoulder end; holds M3 at the elbow (axis -Y,
so its boss reaches toward -Y from the plate)."""
pts = dogbone(EL, SH, 41.0, 41.0)
s = Yplate(pts, Y_UP_IN, Y_UP_OUT)
s = s.fuse(Ycyl(38.0, Y3_REAR - 22.0, Y_UP_OUT, EL[0], EL[1])) # elbow motor boss
inner = dogbone(EL, SH, 41.0 - 10.0, 41.0 - 10.0)
s = s.cut(Yplate(inner, Y_UP_IN + 2.6, Y_UP_OUT - 2.6)) # lightening dish
for k in range(1, 4): # web ribs in the dish
t = k / 4.0
cx = SH[0] + (EL[0] - SH[0]) * t
cz = SH[1] + (EL[1] - SH[1]) * t
s = s.fuse(Ybox(cx - 3.5, cx + 3.5, Y_UP_IN + 2.6, Y_UP_OUT - 2.6, cz - 20, cz + 20))
s = j2.hub_island(s, 30.0, PLATE_T + 0.5)
s = j3.pocket(s)
s = j3.pinch(s, n=2)
return s
def p_forearm(j3, j4):
"""Plate normal Y, y -35..-26. J3 hub at the elbow end; holds M4 at the wrist (axis +Y,
boss reaches toward +Y)."""
pts = dogbone(WR, EL, 40.0, 40.0)
s = Yplate(pts, Y_FA_OUT, Y_FA_IN)
s = s.fuse(Ycyl(37.0, Y_FA_OUT, Y4_REAR + 22.0, WR[0], WR[1])) # wrist motor boss
inner = dogbone(WR, EL, 40.0 - 10.0, 40.0 - 10.0)
s = s.cut(Yplate(inner, Y_FA_OUT + 2.6, Y_FA_IN - 2.6))
for k in range(1, 3):
t = k / 3.0
cx = EL[0] + (WR[0] - EL[0]) * t
cz = EL[1] + (WR[1] - EL[1]) * t
s = s.fuse(Ybox(cx - 3.5, cx + 3.5, Y_FA_OUT + 2.6, Y_FA_IN - 2.6, cz - 16, cz + 16))
s = j3.hub_island(s, 30.0, PLATE_T + 0.5)
s = j4.pocket(s)
s = j4.pinch(s, n=2)
return s
def p_wrist_plate(j4):
"""J4 driven hub + round palm disc with the joint shrouds; bolts to the gripper housing."""
zf = Y_WM_IN
s = Ycyl(41.0, zf, zf + PLATE_T, WR[0], WR[1])
s = j4.hub_island(s, 30.0, PLATE_T + 0.5)
for a in range(0, 360, 60): # lightening, clear of the hub island
u, v = 34 * math.cos(a * D2R), 34 * math.sin(a * D2R)
if math.hypot(u, v) > 31.0:
s = s.cut(Ycyl(6.5, zf + 2.6, zf + PLATE_T - 2.6, WR[0] + u, WR[1] + v))
for a in (0, 120, 240): # 3x M3 forward into the housing
u, v = 30 * math.cos(a * D2R), 30 * math.sin(a * D2R)
s = s.cut(Ycyl(1.7, zf + PLATE_T - 6.0, zf + PLATE_T + 0.01, WR[0] + u, WR[1] + v))
return s
def p_shell(j4, j5):
"""Tapered gripper housing: holds M5, carries the finger guides and the crank window."""
zf = Y_WM_IN + PLATE_T
rear = j5.L(bx(-41, 41, -38, 38, -14.0, 30.0))
fore = j5.L(bx(-41, 41, -38, 38, 30.0, 62.0))
body = rear.fuse(fore)
# cavity must stay inside both sections: it was ±35.5/±29.5 against a ±34/±26 shell,
# which ate the side walls and left the guide blocks as three disconnected solids
cav = j5.L(bx(-32.6, 32.6, -32.6, 32.6, -9.0, 54.0))
s = body.cut(cav)
# mounting flange overlapping the shell by 6 mm: a thin 0.1 mm land would fuse into two
# disconnected solids, which would print as two pieces
s = s.fuse(j5.L(bx(-41, 41, -35, 35, -16.0, -8.0)))
s = j5.pocket(s)
s = j5.pinch(s, n=2)
s = s.cut(j5.L(cyl(22.5, 53.9, 62.01))) # crank pocket in the front wall
for sd in (-1, 1):
for sy in (-1, 1): # y-guides, rooted in the side wall
s = s.fuse(j5.L(bx(33.0 * sd, 41.0 * sd, 11.6 * sy, 14.2 * sy, 55.0, 66.0)))
s = s.cut(j5.L(bx(10.0 * sd, 30.0 * sd, -12.5, 12.5, 53.9, 66.01))) # cam clearance
s = s.cut(j5.L(bx(29.5 * sd, 40.5 * sd, -12.6, 12.6, 44.0, 66.01))) # finger arm exit
for a in (0, 120, 240): # M3 pilots back into the palm
u, v = 30 * math.cos(a * D2R), 30 * math.sin(a * D2R)
s = s.cut(j5.L(cyl(1.55, -16.0, -8.0, u, v)))
return s
def bx(x0, x1, y0, y1, z0, z1):
return box(min(x0, x1), max(x0, x1), min(y0, y1), max(y0, y1), min(z0, z1), max(z0, z1))
def p_finger(j5, side=+1):
"""Prismatic finger: arm riding the y-guides, 45 deg cam ramp on the inner face, inward pad.
The crank's two opposed pins bear on the two ramps and push both jaws open together."""
u = side
arm = bx(30.0 * u, 40.0 * u, -10.5, 10.5, 50.0, 140.0)
arm = arm.fuse(bx(20.0 * u, 30.0 * u, -10.0, 10.0, 60.0, 78.0)) # cam heel block
ramp = bx(-9, 9, -12, 12, 58.0, 80.0)
ramp = place(ramp, R(45 if u > 0 else -45, 0, 0, 1))
ramp = place(ramp, T(17.0 * u, 0, 0))
arm = arm.cut(ramp)
foot = bx(8.0 * u, 30.0 * u, -12.0, 12.0, 96.0, 124.0)
for k in range(3): # 2 mm grooves cut INTO the contact face
foot = foot.cut(bx(6.0 * u, 8.2 * u, -10.5 + 7.0 * k, -7.5 + 7.0 * k, 98.0, 122.0))
arm = arm.fuse(foot).fuse(bx(30.0 * u, 37.0 * u, -8.0, 8.0, 140.0, 147.0))
return j5.L(arm)
def p_crank(j5):
"""Printed disc bolted onto the M5 output: spigot + pilot + disc + 2 opposed pins at R14."""
fz = FLANGE_Z
s = j5.L(cyl(3.15, fz - 2.7, fz + 0.5)) # into the Ø6.5 bore
s = s.fuse(j5.L(cyl(17.8, fz + 0.06, COVER_Z))) # into the Ø36 neck
s = s.fuse(j5.L(cyl(20.0, COVER_Z, 62.0))) # disc
for a in (0.0, 180.0):
u, v = 14.0 * math.cos(a * D2R), 14.0 * math.sin(a * D2R)
s = s.fuse(j5.L(cyl(2.9, 62.0, 78.0, u, v)))
s = s.fuse(j5.L(cyl(4.6, 62.0, 64.4, u, v)))
for a in M["m2_clock"]: # M2 into the motor
u, v = M["m2_bcd"] / 2.0 * math.cos(a * D2R), M["m2_bcd"] / 2.0 * math.sin(a * D2R)
s = s.cut(j5.L(cyl(M["m2_clear_d"] / 2.0, fz - 0.01, 59.2, u, v)))
s = s.cut(j5.L(cyl(M["m2_cb_d"] / 2.0, 59.2, 62.01, u, v)))
return s
GRP = { # part -> the joint group that carries it (kinematic tree)
"P01_底座板": "root", "P02_立柱": "root", "M1_J1": "root",
"J1_半联轴器A": "root", "J1_抱箍A": "root", "J1_抱箍B": "root",
"P03_转塔": "J1", "J1_半联轴器B": "J1",
"P04_肩支架": "J1", "M2_J2": "J1", "J2_半联轴器A": "J1",
"J2_抱箍A": "J1", "J2_抱箍B": "J1",
"P05_大臂": "J2", "J2_半联轴器B": "J2", "M3_J3": "J2",
"J3_半联轴器A": "J2", "J3_抱箍A": "J2", "J3_抱箍B": "J2",
"P06_小臂": "J3", "J3_半联轴器B": "J3", "M4_J4": "J3",
"J4_半联轴器A": "J3", "J4_抱箍A": "J3", "J4_抱箍B": "J3",
"P07_腕板": "J4", "P11_爪壳": "J4", "J4_半联轴器B": "J4", "M5_J5": "J4",
"P08_指A": "J4", "P09_指B": "J4", "P10_曲柄": "J5",
}
JAW = {"P08_指A": 1.0, "P09_指B": -1.0} # sign of the jaw travel along the J5 local +X
def explode_vec(p, JS, cents, gmean):
"""Explosion offset: coupling hardware separates along its joint axis, everything
else moves radially out of the assembly centroid."""
nm = p["name"]
c = cents[nm]
for jid in ("J1", "J2", "J3", "J4"):
if nm.startswith(jid + "_半联轴器B"):
return [JS[jid].d[i] * 24.0 for i in range(3)]
if nm.startswith(jid + "_半联轴器A") or nm.startswith(jid + "_抱箍"):
k = -26.0 if "抱箍" in nm else -16.0
return [JS[jid].d[i] * k for i in range(3)]
v = [c[i] - gmean[i] for i in range(3)]
m = math.sqrt(sum(x * x for x in v)) or 1.0
return [v[i] / m * 34.0 for i in range(3)]
# ---------------------------------------------------------------- assembly
def build():
global MOTOR, HALF, CLAMP
MOTOR = cq.importers.importStep(SRC_STEP).val()
MOTOR = place(MOTOR, T(0, 0, -2.57))
motor = MOTOR
half = HALF = solids_of(cq.importers.importStep(HALF_STEP).val())[0]
clamp = CLAMP = solids_of(cq.importers.importStep(CLAMP_STEP).val())[0]
j1 = Joint("J1", "底座回转", (0, 0, 8.0), (0, 0, 1), (1, 0, 0), lug_phase=45)
j2 = Joint("J2", "肩俯仰", (SH[0], Y0, SH[1]), (0, 1, 0), (1, 0, 0), lug_phase=90)
j3 = Joint("J3", "肘俯仰", (EL[0], Y3_REAR, EL[1]), (0, -1, 0), (1, 0, 0), lug_phase=90)
j4 = Joint("J4", "腕俯仰", (WR[0], Y4_REAR, WR[1]), (0, 1, 0), (1, 0, 0), lug_phase=90)
j5 = Joint("J5", "夹爪", (WR[0] + S0_M5 * DIR_F[0], Y_M5, WR[1] + S0_M5 * DIR_F[1]),
(DIR_F[0], 0, DIR_F[1]), (0, 1, 0), lug_phase=0)
JS = dict(J1=j1, J2=j2, J3=j3, J4=j4, J5=j5)
add("P01_底座板", "底座安装板 Ø152x8", p_base_plate(), 0x4b5563, note="4x M5 地脚，中心 Ø50 走线")
add("P02_立柱", "立柱，装 M1", p_column(j1), 0x4b5563, note="Ø60.3 滑配 + 64.4 方 Pocket + 2x M3 顶丝")
add("P03_转塔", "J1 从动盘 + 转塔", p_turret(j1), 0x2563eb, note="底面复刻电机输出：Ø36x1.0 / Ø6.5x4 / 3xM2@Ø29")
add("P04_肩支架", "肩支架，装 M2", p_shoulder_bracket(j2), 0x4b5563, note="立式抱紧 M2，底面 3x M3 接转塔")
add("P05_大臂", "大臂板 y39..48", p_upper_arm(j2, j3), 0x059669, note="根部和肘部各一次接口；肘部装 M3")
add("P06_小臂", "小臂板 y-35..-26", p_forearm(j3, j4), 0x059669, note="肘部从动盘 + 腕部装 M4")
add("P07_腕板", "J4 从动盘 + 掌盘", p_wrist_plate(j4), 0xd97706,
note="含关节包壳弧段；正面 3x M3 接爪壳")
add("P11_爪壳", "夹爪壳体，装 M5", p_shell(j4, j5), 0xb45309,
note="锥形壳 + 两根指导轨 + 曲柄过孔；后端 3x M3 锁在掌盘上")
add("P08_指A", "夹爪指 A", p_finger(j5, +1), 0xdc2626, note="45° 斜槽 + 导轨舌")
add("P09_指B", "夹爪指 B（与 A 镜像）", p_finger(j5, -1), 0xdc2626, note="打印 2 件时镜像即可")
add("P10_曲柄", "夹爪曲柄盘", p_crank(j5), 0x7c3aed, note="双销 R14，直接锁在 M5 输出上")
for k, (jid, cn) in enumerate([("J1", "底座回转"), ("J2", "肩俯仰"), ("J3", "肘俯仰"),
("J4", "腕俯仰"), ("J5", "夹爪电机")]):
j = JS[jid]
add("M%d_%s" % (k + 1, jid), "%s 电机模组" % jid, place(motor, j.t), 0x9ca3af,
printed=False, note="复用 装配体2.STEP：Ø60 机座 + 两级 0.5M 行星减速")
if jid != "J5":
for nm, sh in j.place_hardware(half, clamp, jid):
add(nm, "%s 端面齿联轴器/抱箍" % jid, sh, 0xeab308, printed=False,
note="沿用 配件/ 现有 v9 零件，打印件")
return JS
def write_step(items, path):
w = STEPControl_Writer()
for nm, sh in items:
Interface_Static.SetCVal_s('write.step.product.name', nm)
w.Transfer(sh.wrapped, STEPControl_StepModelType.STEPControl_AsIs)
w.Write(path.replace("/", "\\"))
return os.path.getsize(path)
def export(JS):
for d in (OUT, OUT + "/parts_step", OUT + "/print_stl", OUT + "/assembly", WEB, DOCS):
os.makedirs(d, exist_ok=True)
written = []
for p in PARTS:
fn = "%s/%s.step" % (OUT + "/parts_step", p["name"])
write_step([(p["name"], p["shape"])], fn)
written.append(fn)
if p["printed"]:
stl = "%s/%s.stl" % (OUT + "/print_stl", p["name"])
cq.exporters.export(cq.Workplane(obj=p["shape"]), stl, exportType="STL")
write_step([(p["name"], p["shape"]) for p in PARTS],
OUT + "/assembly/ROBOT_ARM_%s.stp" % VERSION)
# printed-only assembly (what you actually bolt together)
write_step([(p["name"], p["shape"]) for p in PARTS if p["printed"]],
OUT + "/assembly/ROBOT_ARM_printed_only.stp")
# mesh json for the web viewer
parts = []
for p in PARTS:
key = "motor" if p["name"].startswith("M") else ("coupl" if "联轴器" in p["name"] or "抱箍" in p["name"] else p["name"])
defl = 0.85 if p["name"].startswith("M") else (0.20 if key == "coupl" else 0.055)
v, t = mesh_of(p["shape"], defl)
parts.append(dict(name=p["name"], cn=p["cn"], v=v, t=t, color=p["color"],
printed=p["printed"], vol=round(p["vol"], 0), note=p["note"],
grp=GRP.get(p["name"], "root"), jaw=JAW.get(p["name"], 0.0),
ex=[round(x, 1) for x in explode_vec(p, JS, CENTS,
[sum(CENTS[k][i] for k in CENTS) / float(len(CENTS)) for i in range(3)])
],
idx=len(parts) + 1))
joints = [dict(id=k, cn=JS[k].cn, rear=JS[k].rear, d=JS[k].d, xdir=JS[k].xdir, hub=JS[k].hub_face())
for k in JS]
json.dump(dict(parts=parts, joints=joints, layout=dict(SH=SH, EL=EL, WR=WR,
L1=ARM_L1, L2=ARM_L2, ver=VERSION, it=ITER),
measured=dict(M=M, C=C, HUB_FACE_Z=HUB_FACE_Z, CLAMP_SPAN=list(CLAMP_SPAN))),
open(WEB + "/mesh_arm.json", "w", encoding="utf-8"), ensure_ascii=False)
bom = [dict(件号=p["name"], 名称=p["cn"], 类型="3D打印" if p["printed"] else "已有/电机",
数量=1, 体积cm3=round(p["vol"] / 1000.0, 1),
质量g_PLA=round(p["vol"] * 1.24e-3, 1), 包络=p["bb"], 说明=p["note"]) for p in PARTS]
json.dump(bom, open(DOCS + "/bom.json", "w", encoding="utf-8"), ensure_ascii=False, indent=1)
return bom
def collide():
rows = []
ps = [p for p in PARTS if p["vol"] > 1.0]
for i in range(len(ps)):
for j in range(i + 1, len(ps)):
a, b = ps[i], ps[j]
if a["name"].startswith("M") and b["name"].startswith("M"):
continue
av, bv = bb(a["shape"]), bb(b["shape"])
if any(av[1][k] < bv[0][k] - 0.05 or bv[1][k] < av[0][k] + 0.05 for k in range(3)):
continue
v = common_vol(a["shape"], b["shape"])
if v > 2.0:
try:
c = BRepAlgoAPI_Common(a["shape"].wrapped, b["shape"].wrapped)
c.Build()
cb = cq.Shape(c.Shape())
lo, hi = bb(cb)
loc = "@[%.0f %.0f %.0f..%.0f %.0f %.0f]" % (
lo[0], lo[1], lo[2], hi[0], hi[1], hi[2])
except Exception:
loc = "?"
rows.append((a["name"], b["name"], round(v, 1), loc))
rows.sort(key=lambda r: -r[2])
json.dump([r[:3] for r in rows], open(DOCS + "/collision.json", "w", encoding="utf-8"),
ensure_ascii=False)
return rows
def axial_extent(j, shape):
lo, hi = bb(shape)
vals = []
for i in (0, 1):
for k in range(3):
p = [lo[0], lo[1], lo[2]]
p[k] = hi[k] if i else lo[k]
vals.append(sum((p[q] - j.rear[q]) * j.d[q] for q in range(3)))
return min(vals), max(vals)
def verify_interfaces(half, clamp, JS):
"""Numeric self-check of the derived joint stack, before trusting the renders."""
rows = []
for k, j in JS.items():
if k == "J5":
continue
parts = dict(j.place_hardware(half, clamp, k))
a = axial_extent(j, parts["%s_半联轴器A" % k])
b = axial_extent(j, parts["%s_半联轴器B" % k])
c = axial_extent(j, parts["%s_抱箍A" % k])
hub = j.at(HUB_FACE_Z)
rows.append(dict(jid=k, halfA=[round(x, 3) for x in a], halfB=[round(x, 3) for x in b],
clamp=[round(x, 3) for x in c], hub_face=round(HUB_FACE_Z, 3),
spigot_proud=round(b[1] - HUB_FACE_Z, 3),
clamp_clear=round(c[0] - COVER_Z, 3)))
return rows
if __name__ == "__main__":
JS = build()
vi = verify_interfaces(HALF, CLAMP, JS)
bom = export(JS)
cl = collide()
with open(DOCS + "/collision.txt", "w", encoding="utf-8") as fh:
fh.write("干涉对（体积 mm3，>2 才算数）\n")
for r in cl:
fh.write(" %-16s x %-16s %10.1f %s\n" % r)
fh.write("total pairs: %d\n" % len(cl))
print("parts %d printed %d" % (len(PARTS), sum(1 for p in PARTS if p["printed"])))
for p in PARTS:
print(" %-22s %9.1f cm3 %s" % (p["name"], p["vol"] / 1000.0, p["bb"]))
print("JOINT STACK (hub_face=%.3f cover=%.3f)" % (HUB_FACE_Z, COVER_Z))
for r in vi:
print(" %-3s halfA=%s halfB=%s clamp=%s B销钉伸出hub面=%+.3f(应+4.06) 抱箍起点离盖板=%+.3f" %
(r["jid"], r["halfA"], r["halfB"], r["clamp"], r["spigot_proud"], r["clamp_clear"]))
print("COLLISION pairs=%d" % len(cl))
for r in cl[:18]:
print(" %-16s x %-16s %9.1f mm3 %s" % r)
print("WROTE", OUT + "/assembly/ROBOT_ARM_%s.stp" % VERSION)
Reference in New Issue Copy Permalink