#!/usr/bin/env python3 """Offline hexapod demo or read-only EPICS view; both use the C++ model.""" import argparse import ctypes as ct import json import math from pathlib import Path import sys import time ROOT = Path(__file__).resolve().parents[1] DEFAULT_GEOMETRY = [200, 120, 250, 10, 10, 180, 400] class Model: def __init__(self, geometry=DEFAULT_GEOMETRY): if len(geometry) != 7 or not all(math.isfinite(x) for x in geometry): raise ValueError('Geometry must contain seven finite numbers') candidates = [ROOT / 'build' / ('libhexapod.' + ext) for ext in ('so', 'dylib')] path = next((p for p in candidates if p.exists()), None) if path is None: raise RuntimeError('Build the viewer C++ library first: make viewer-lib') self.lib = ct.CDLL(str(path)) pointer = ct.POINTER(ct.c_double) self.lib.hex_inverse.argtypes = [pointer] * 4 self.lib.hex_inverse.restype = ct.c_int self.lib.hex_forward.argtypes = [pointer] * 5 self.lib.hex_forward.restype = ct.c_int self.geometry = (ct.c_double * 7)(*geometry) self.home = self.inverse([0] * 6)[0] @staticmethod def six(values): if len(values) != 6 or not all(math.isfinite(x) for x in values): raise ValueError('Expected six finite values') return (ct.c_double * 6)(*values) def inverse(self, pose): lengths, points = (ct.c_double * 6)(), (ct.c_double * 36)() status = self.lib.hex_inverse(self.geometry, self.six(pose), lengths, points) if status < 0: raise ValueError('Invalid geometry or pose') pts = [list(points[i:i+3]) for i in range(0, 36, 3)] return list(lengths), pts[:6], pts[6:], status == 0 def forward(self, lengths, seed): pose, residual = (ct.c_double * 6)(), ct.c_double() status = self.lib.hex_forward(self.geometry, self.six(lengths), self.six(seed), pose, ct.byref(residual)) return list(pose), residual.value, status == 0 def self_test(model): pose = [5, -8, 12, 3, -4, 6] lengths, _, _, valid = model.inverse(pose) actual, residual, converged = model.forward(lengths, [0]*6) if not valid or not converged or max(abs(a-b) for a, b in zip(actual, pose)) > 1e-5: raise RuntimeError('C API roundtrip failed') print(f'PASS: Python/C++ IK/FK roundtrip, residual={residual:.3g} mm') def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('--prefix', help='Read-only EPICS mode, e.g. HEX: (requires pyepics)') 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='Render one offline frame to PNG') args = parser.parse_args() if args.prefix and args.snapshot: parser.error('--snapshot is offline only; omit --prefix') 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 pvs = None if args.prefix: from epics_source import IocSource pvs = IocSource(args.prefix) fig = plt.figure(figsize=(11, 8)) ax = fig.add_axes([0.04, 0.31, .72, .66], projection='3d') status = fig.text(.75, .78, '', family='monospace', fontsize=9, va='top') fig.suptitle('ecmc hexapod — 6 coordinate axes + 6 leg axes') sliders = [] for i, label in enumerate(('X [mm]', 'Y [mm]', 'Z [mm]', 'Rx [deg]', 'Ry [deg]', 'Rz [deg]')): slider_ax = fig.add_axes([.15, .245-i*.035, .5, .02]) limit = 30 if i < 3 else 10 sliders.append(Slider(slider_ax, label, -limit, limit, valinit=0)) if pvs: sliders[-1].set_active(False) state = dict(pose=[0.]*6, seed=[0.]*6, start=[0.]*6, target=[0.]*6, started=time.monotonic(), legs=list(model.home)) def move(_): state.update(start=list(state['pose']), target=[s.val for s in sliders], started=time.monotonic()) button = Button(fig.add_axes([.73, .13, .2, .05]), 'Move (3 seconds)') button.on_clicked(move) if pvs: button.set_active(False) button.label.set_text('IOC monitor (read-only)') if args.snapshot: state['pose'] = [8, -5, 12, 4, -3, 5] state['start'] = list(state['pose']) state['target'] = list(state['pose']) for slider, value in zip(sliders, state['pose']): slider.set_val(value) def update(_): try: fault = 0 if pvs: live = pvs.snapshot() fault = live['fault'] state['pose'] = list(live['pose_set']) state['seed'] = list(live['pose_actual']) state['legs'] = [h+s for h,s in zip(model.home, live['leg_actual'])] leg_set = live['leg_set'] leg_actual = live['leg_actual'] residual = live['residual'] ok = fault == 0 for slider, value in zip(sliders, state['pose']): slider.set_val(value) else: t = min(1., (time.monotonic()-state['started'])/3.) s = t*t*t*(10+t*(-15+6*t)) state['pose'] = [a+s*(b-a) for a,b in zip(state['start'],state['target'])] requested, _, _, allowed = model.inverse(state['pose']) if allowed: # Visual demo's simple first-order leg response; not an ecmc plant model. state['legs'] = [a+.2*(b-a) for a,b in zip(state['legs'], requested)] lengths, base, requested, allowed = model.inverse(state['pose']) if not pvs: recovered, residual, ok = model.forward(state['legs'], state['seed']) if ok: state['seed'] = recovered leg_set = [l-h for l,h in zip(lengths, model.home)] leg_actual = [l-h for l,h in zip(state['legs'], model.home)] _, _, actual, _ = model.inverse(state['seed']) ax.clear() def outline(points, color, style): order = sorted(range(6), key=lambda i: math.atan2(points[i][1], points[i][0])) ring = [points[i] for i in order] + [points[order[0]]] ax.plot(*zip(*ring), color=color, linestyle=style) outline(base, '0.3', '-') outline(requested, 'tab:orange', '--') outline(actual, 'tab:blue', '-') for i in range(6): ax.plot(*zip(base[i], requested[i]), color='tab:orange', linestyle='--', alpha=.6) ax.plot(*zip(base[i], actual[i]), color='tab:blue', linewidth=2) ax.text(*base[i], str(i+1)) radius = model.geometry[0]*1.3 ax.set(xlim=(-radius,radius), ylim=(-radius,radius), zlim=(0,model.geometry[2]*1.7), xlabel='X [mm]', ylabel='Y [mm]', zlabel='Z base frame [mm]') ax.set_box_aspect((2*radius,2*radius,model.geometry[2]*1.7)) message = ('IOC / EPICS read-only' if pvs else 'Offline demo') message += '\nBlue: actual pose\nOrange: setpoint pose\n' message += f'\nStroke: {"OK" if allowed else "OUTSIDE"}\nFK residual: {residual:.2g} mm\nIOC fault: {fault}\n' message += '\nLeg stroke [mm]\n Set Actual\n' message += '\n'.join(f'L{i+1} {a:8.3f} {b:8.3f}' for i,(a,b) in enumerate(zip(leg_set,leg_actual))) message += '\n\nPose [mm / deg]\n Set Actual\n' message += '\n'.join(f'{n:2} {a:8.3f} {b:8.3f}' for n,a,b in zip(('X','Y','Z','Rx','Ry','Rz'),state['pose'],state['seed'])) if not ok and not pvs: message += '\nFK FAILED: last valid pose' status.set_text(message) status.set_color('black' if allowed and ok and not fault else 'red') except (RuntimeError, ValueError, TypeError) as exc: status.set_text(str(exc)); status.set_color('red') return () if args.snapshot: state['legs'] = model.inverse(state['pose'])[0] update(0) fig.savefig(args.snapshot, dpi=140) else: animation = FuncAnimation(fig, update, interval=50, cache_frame_data=False) try: plt.show() finally: if pvs: pvs.close() if __name__ == '__main__': main()