185 lines
8.5 KiB
Python
185 lines
8.5 KiB
Python
#!/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()
|