#!/usr/bin/env python3 """Generic six-joint arm: offline Cartesian/joint controls or read-only IOC view.""" import argparse import ctypes as ct import json import math from pathlib import Path import time ROOT = Path(__file__).resolve().parents[1] HOME = [0., -35., 70., 0., 35., 0.] LOWER = [-170., -120., -140., -180., -120., -180.] UPPER = [170., 120., 140., 180., 120., 180.] class Model: def __init__(self, geometry): if len(geometry) != 4 or not all(math.isfinite(x) and x > 0 for x in geometry): raise ValueError('Geometry requires four positive finite lengths in mm') path = next((ROOT/'build'/('libarm.'+ext) for ext in ('so','dylib') if (ROOT/'build'/('libarm.'+ext)).exists()), None) if path is None: raise RuntimeError('Build the viewer library first: make viewer-lib') self.lib = ct.CDLL(str(path)) ptr = ct.POINTER(ct.c_double) self.lib.arm_forward.argtypes = [ptr]*4 self.lib.arm_forward.restype = ct.c_int self.lib.arm_inverse.argtypes = [ptr]*5 self.lib.arm_inverse.restype = ct.c_int self.geometry = (ct.c_double*4)(*geometry) @staticmethod def six(values): if len(values) != 6 or not all(math.isfinite(x) for x in values): raise ValueError('Expected six finite numbers') return (ct.c_double*6)(*values) def forward(self, joints): pose, points = (ct.c_double*6)(), (ct.c_double*24)() if self.lib.arm_forward(self.geometry, self.six(joints), pose, points): raise ValueError('Invalid geometry or joint limits exceeded') return list(pose), [list(points[i:i+3]) for i in range(0,24,3)] def inverse(self, pose, seed): joints, residual = (ct.c_double*6)(), ct.c_double() status = self.lib.arm_inverse(self.geometry,self.six(pose),self.six(seed),joints,ct.byref(residual)) if status < 0: raise ValueError('Invalid geometry, pose or seed') return list(joints), residual.value, status == 0 def self_test(model): joints = [a+b for a,b in zip(HOME,[5,-4,3,2,-3,4])] pose, points = model.forward(joints) recovered, residual, ok = model.inverse(pose,HOME) pose2, _ = model.forward(recovered) if not ok or len(points) != 8 or max(abs(a-b) for a,b in zip(pose,pose2)) > 1e-4: raise RuntimeError('Python/C++ round trip failed') if model.inverse([10000,0,0,0,0,0],HOME)[2]: raise RuntimeError('Unreachable pose accepted') print(f'PASS: Python/C++ arm IK/FK; residual={residual:.3g}') def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('--prefix',help='Read-only IOC view, e.g. MTEST04-MTN-ARM:') parser.add_argument('--joints',action='store_true',help='Offline joint-space controls, home-relative degrees') parser.add_argument('--geometry',type=Path,default=ROOT/'cfg'/'geometry.json') parser.add_argument('--self-test',action='store_true') parser.add_argument('--snapshot',type=Path,help='Save one offline view as PNG') args = parser.parse_args() if args.prefix and (args.joints or args.snapshot): parser.error('--prefix cannot be combined with --joints or --snapshot') model = Model(json.loads(args.geometry.read_text())) if args.self_test: self_test(model) return import matplotlib if args.snapshot: matplotlib.use('Agg') import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from matplotlib.widgets import Slider, Button source = None if args.prefix: from epics_source import IocSource source = IocSource(args.prefix) fig = plt.figure(figsize=(12,9)) ax = fig.add_axes([.04,.32,.68,.61],projection='3d') status = fig.text(.73,.85,'',family='monospace',fontsize=9,va='top') fig.suptitle('ecmc 6-DOF arm — generic simulation (no collision checking)') sliders = [] labels = [f'J{i+1} offset [deg]' for i in range(6)] if args.joints else ['X [mm]','Y [mm]','Z [mm]','Rx [deg]','Ry [deg]','Rz [deg]'] for i,label in enumerate(labels): limit = 45 if args.joints else (50 if i<3 else 20) slider = Slider(fig.add_axes([.16,.25-i*.035,.46,.02]),label,-limit,limit,valinit=0) if source: slider.set_active(False) sliders.append(slider) state = dict(current=[0.]*6,start=[0.]*6,target=[0.]*6,started=time.monotonic(), command=list(HOME),actual=list(HOME),pose=[0.]*6) def move(_): state.update(start=list(state['current']),target=[s.val for s in sliders],started=time.monotonic()) button = Button(fig.add_axes([.75,.12,.2,.05]),'Move (3 seconds)') button.on_clicked(move) if source: button.set_active(False) button.label.set_text('IOC monitor') if args.snapshot: state.update(current=[10,-5,5,3,-3,4],start=[10,-5,5,3,-3,4],target=[10,-5,5,3,-3,4]) for slider,value in zip(sliders,state['current']): slider.set_val(value) def update(_): try: fault, residual, ok = 0, 0., True if source: live = source.snapshot() state['command'] = [h+v for h,v in zip(HOME,live['joint_set'])] state['actual'] = [h+v for h,v in zip(HOME,live['joint_actual'])] state['pose'] = list(live['pose_actual']) desired_pose = live['pose_set'] residual, fault = live['residual'], live['fault'] else: t = min(1.,(time.monotonic()-state['started'])/3.) blend = t*t*t*(10+t*(-15+6*t)) state['current'] = [a+blend*(b-a) for a,b in zip(state['start'],state['target'])] if args.joints: state['command'] = [h+v for h,v in zip(HOME,state['current'])] desired_pose, _ = model.forward(state['command']) else: desired_pose = state['current'] command,residual,ok = model.inverse(desired_pose,state['command']) if ok: state['command'] = command # Offline ideal plant. There is no dynamics or drive model. state['actual'] = list(state['command']) state['pose'], _ = model.forward(state['actual']) _, requested = model.forward(state['command']) _, actual = model.forward(state['actual']) ax.clear() ax.plot(*zip(*requested),'o--',color='tab:orange',label='IK command',alpha=.7) ax.plot(*zip(*actual),'o-',color='tab:blue',linewidth=3,label='Joint feedback') for index,label in ((0,'J1'),(1,'J2'),(2,'J3'),(3,'J4/5/6')): ax.text(*actual[index],label) reach = sum(model.geometry) ax.set(xlim=(-reach,reach),ylim=(-reach,reach),zlim=(0,reach),xlabel='X [mm]',ylabel='Y [mm]',zlabel='Z [mm]') ax.set_box_aspect((2,2,1)) ax.legend(loc='upper left') text = ('IOC read-only' if source else ('Offline joint mode' if args.joints else 'Offline Cartesian mode')) text += f'\nIK: {"OK" if ok else "NO SOLUTION"}\nWeighted residual: {residual:.3g}\nIOC fault: {fault}\n' text += '\nJoint offsets [deg]\n IK set Actual\n' text += '\n'.join(f'J{i+1} {a-h:8.3f} {b-h:8.3f}' for i,(h,a,b) in enumerate(zip(HOME,state['command'],state['actual']))) text += '\n\nTool offsets [mm / deg]\n Set Actual\n' text += '\n'.join(f'{n:2} {a:8.3f} {b:8.3f}' for n,a,b in zip(('X','Y','Z','Rx','Ry','Rz'),desired_pose,state['pose'])) if not ok: text += '\n\nHolding last valid joints.' text += '\n\nNo collision checking.' status.set_text(text) status.set_color('black' if ok and fault==0 else 'red') except (RuntimeError,ValueError,TypeError) as exc: status.set_text(str(exc)) status.set_color('red') return () if args.snapshot: update(0) args.snapshot.parent.mkdir(parents=True,exist_ok=True) fig.savefig(args.snapshot,dpi=130) else: animation = FuncAnimation(fig,update,interval=50,cache_frame_data=False) try: plt.show() finally: if source: source.close() if __name__ == '__main__': main()