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