"""Door robot arm: kinematics of the design (pure Python, no bpy), shared by the design checks. Reading of the references (ref1 side view, pixels of the 2000 px original, measured on a 50 px grid): J1 turret yaw vertical axis through the turntable, x_img 1000; carriage top (turntable face) y_img 1550 J2 shoulder the perforated drum / orange-ring side caps, (1155, 1315): 155 px BEHIND the J1 axis, 235 px up J3 elbow the black slotted elbow cap, (1550, 720) J4 forearm roll the ringed roll section in front of the upper arm's big cap; axis horizontal at the rest pose J5 wrist pitch the wrist's round cap with concentric rings, (565, 613) = wrist centre WC (J4, J5, J6 meet here) J6 flange roll the gripper hub / the plate neck turns about the tool axis rod J3 actuator: barrel trunnion on the lower arm's slotted (knee) cap (1393, 1165), rod eye on the truss knob (1762, 888) of the upper arm; it is telescopic (its line points at the knee cap's centre within 4 degrees in ref1, and it has a gland collar where it narrows) Upright arm frame A at rest (all q = 0 is the ref1 pose): origin = turntable face centre, +X forward (toward the gripper, = -x_img), +Z up (= -y_img), +Y lateral (the arm's left; ref1 shows the arm's left side). """ import math PX = { # ref1 landmarks in arm-frame pixels (forward, up), origin turntable face centre "J2": (-155.0, 235.0), "J3": (-550.0, 830.0), "WC": (435.0, 937.0), "knee": (-393.0, 385.0), # slotted cap centre = rod barrel trunnion "knob": (-762.0, 662.0), # truss knob = rod eye pin "flange": 60.0, # WC -> J6 flange face, along the tool axis } PHI_L0 = math.atan2(PX["J3"][1] - PX["J2"][1], PX["J3"][0] - PX["J2"][0]) # lower arm direction at rest (r-h plane) PHI_F0 = 0.0 # forearm axis direction at rest L2_PX = math.hypot(PX["J3"][0] - PX["J2"][0], PX["J3"][1] - PX["J2"][1]) D4_PX = PX["WC"][0] - PX["J3"][0] # along the forearm axis A3_PX = PX["WC"][1] - PX["J3"][1] # the forearm axis passes this far above J3 at rest L3_PX = math.hypot(D4_PX, A3_PX) BETA = math.atan2(A3_PX, D4_PX) def rot2(v, a): c, s = math.cos(a), math.sin(a) return (c * v[0] - s * v[1], s * v[0] + c * v[1]) def sub(a, b): return tuple(x - y for x, y in zip(a, b)) def add(a, b): return tuple(x + y for x, y in zip(a, b)) def mul(a, k): return tuple(x * k for x in a) def dot(a, b): return sum(x * y for x, y in zip(a, b)) def cross(a, b): return (a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]) def norm(a): return math.sqrt(dot(a, a)) def unit(a): n = norm(a) return tuple(x / n for x in a) class Arm: """the arm at scale s (metres per ref1 pixel)""" def __init__(self, s): self.s = s self.j2 = (PX["J2"][0] * s, PX["J2"][1] * s) self.L2 = L2_PX * s self.d4 = D4_PX * s self.a3 = A3_PX * s self.L3 = L3_PX * s self.flange = PX["flange"] * s # rod: trunnion in the lower-arm frame (along, normal) and the knob in the upper-arm frame, relative J2 / J3 el = (math.cos(PHI_L0), math.sin(PHI_L0)) nl = (el[1], -el[0]) k = sub(PX["knee"], PX["J2"]) self.trun_l = (dot(k, el) * s, dot(k, nl) * s) # in lower-arm frame (e_l, n_l) kn = sub(PX["knob"], PX["J3"]) self.knob_f = (kn[0] * s, kn[1] * s) # in forearm frame (e_f, e_f_perp) at rest PHI_F0 = 0 # ---- planar chain in the turret plane (r forward, h up) ---- def planar_fk(self, phi_l, phi_f): j3 = add(self.j2, (self.L2 * math.cos(phi_l), self.L2 * math.sin(phi_l))) wc = add(j3, (self.d4 * math.cos(phi_f) - self.a3 * math.sin(phi_f), self.d4 * math.sin(phi_f) + self.a3 * math.cos(phi_f))) return j3, wc def planar_ik(self, rho, h): """WC at radial rho (from the J1 axis, along the arm's facing) and height h (arm frame): elbow-up as at rest""" dr, dh = rho - self.j2[0], h - self.j2[1] D = math.hypot(dr, dh) c2 = (self.L2 ** 2 + D * D - self.L3 ** 2) / (2 * self.L2 * D) c3 = (self.L2 ** 2 + self.L3 ** 2 - D * D) / (2 * self.L2 * self.L3) if abs(c2) > 1 or abs(c3) > 1: return None psi = math.atan2(dh, dr) phi_l = psi + math.acos(c2) j3 = add(self.j2, (self.L2 * math.cos(phi_l), self.L2 * math.sin(phi_l))) phi_3w = math.atan2(h - j3[1], rho - j3[0]) phi_f = phi_3w - BETA return dict(phi_l=phi_l, phi_f=phi_f, q2=phi_l - PHI_L0, q3=(phi_f - phi_l) - (PHI_F0 - PHI_L0), D=D, elbow_inner=math.acos(c3), j3=j3) def rod(self, phi_l, phi_f): """barrel trunnion and rod-eye pin in the turret plane; length""" el = (math.cos(phi_l), math.sin(phi_l)) nl = (el[1], -el[0]) t = add(self.j2, add(mul(el, self.trun_l[0]), mul(nl, self.trun_l[1]))) j3, wc = self.planar_fk(phi_l, phi_f) ef = (math.cos(phi_f), math.sin(phi_f)) efp = (-ef[1], ef[0]) k = add(j3, add(mul(ef, self.knob_f[0]), mul(efp, self.knob_f[1]))) return t, k, math.hypot(k[0] - t[0], k[1] - t[1]) # ---- frames: the hanging carriage frame C = world rotated 180 deg about x: X_C = +x, Y_C = -y, Z_C = -z ---- def w2c(v): return (v[0], -v[1], -v[2]) def c2w(v): return (v[0], -v[1], -v[2]) def solve(arm, wc_c, tool_c, branch5=+1): """full IK. wc_c: wrist centre in C (relative to the turntable face centre). tool_c: (x_t, y_t, z_t) tool axes in C (x_t = approach axis, from the flange into the door). Returns joint angles (radians) and the frames.""" q1 = math.atan2(wc_c[1], wc_c[0]) rho = math.hypot(wc_c[0], wc_c[1]) h = wc_c[2] p = arm.planar_ik(rho, h) if p is None: return None c1, s1 = math.cos(q1), math.sin(q1) cf, sf = math.cos(p["phi_f"]), math.sin(p["phi_f"]) X_T, Y_T, Z_T = (c1, s1, 0.0), (-s1, c1, 0.0), (0.0, 0.0, 1.0) x3 = add(mul(X_T, cf), mul(Z_T, sf)) y3 = Y_T z3 = add(mul(Z_T, cf), mul(X_T, -sf)) xt, yt, zt = tool_c M11, M21, M31 = dot(x3, xt), dot(y3, xt), dot(z3, xt) M12, M13 = dot(x3, yt), dot(x3, zt) q5 = branch5 * math.acos(max(-1.0, min(1.0, M11))) q4 = math.atan2(branch5 * M21, -branch5 * M31) q6 = math.atan2(branch5 * M12, branch5 * M13) return dict(q1=q1, q2=p["q2"], q3=p["q3"], q4=q4, q5=q5, q6=q6, phi_l=p["phi_l"], phi_f=p["phi_f"], rho=rho, h=h, elbow_inner=p["elbow_inner"], D=p["D"], frames=(x3, y3, z3)) def rx(a): c, s = math.cos(a), math.sin(a) return ((1, 0, 0), (0, c, -s), (0, s, c)) def ry(a): c, s = math.cos(a), math.sin(a) return ((c, 0, s), (0, 1, 0), (-s, 0, c)) def rz(a): c, s = math.cos(a), math.sin(a) return ((c, -s, 0), (s, c, 0), (0, 0, 1)) def mm(A, B): return tuple(tuple(sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)) for i in range(3)) def mv(A, v): return tuple(sum(A[i][k] * v[k] for k in range(3)) for i in range(3)) def col(A, j): return (A[0][j], A[1][j], A[2][j]) def fk(arm, q): """forward kinematics in C from joint angles: positions of J2, J3, WC, flange, tool axes (for the pose check)""" q1, q2, q3, q4, q5, q6 = q["q1"], q["q2"], q["q3"], q["q4"], q["q5"], q["q6"] phi_l = PHI_L0 + q2 phi_f = phi_l + q3 + (PHI_F0 - PHI_L0) j3p, wcp = arm.planar_fk(phi_l, phi_f) c1, s1 = math.cos(q1), math.sin(q1) def P(pl): return (pl[0] * c1, pl[0] * s1, pl[1]) R1 = rz(q1) # forearm frame: rotate about turret -Y? build from columns: x3 = cos*X_T + sin*Z_T etc. cf, sf = math.cos(phi_f), math.sin(phi_f) X_T, Y_T, Z_T = col(R1, 0), col(R1, 1), col(R1, 2) x3 = add(mul(X_T, cf), mul(Z_T, sf)) z3 = add(mul(Z_T, cf), mul(X_T, -sf)) R3 = tuple(tuple(v[i] for v in (x3, Y_T, z3)) for i in range(3)) R6 = mm(mm(mm(R3, rx(q4)), ry(q5)), rx(q6)) wc = P(wcp) xt = col(R6, 0) return dict(J2=P(arm.j2), J3=P(j3p), WC=wc, flange=add(wc, mul(xt, arm.flange)), R6=R6, R3=R3)