From 8f6dcd52e2df601062cc2935e0458a8dd3ec089b Mon Sep 17 00:00:00 2001 From: Roman Mankowsky Date: Fri, 5 Jun 2020 10:34:56 +0200 Subject: [PATCH] Hexapod --- eco/bernina/config.py | 7 +- eco/devices_general/adjustable.py | 77 +++++++++++---- eco/devices_general/motors.py | 5 +- eco/endstations/bernina_diffractometers.py | 57 +++-------- eco/endstations/hexapod.py | 108 ++++++++++++++++++--- eco/xoptics/kb_bernina.py | 6 +- eco/xoptics/slit_USD.py | 2 +- 7 files changed, 175 insertions(+), 87 deletions(-) diff --git a/eco/bernina/config.py b/eco/bernina/config.py index 519d05b..e38b05d 100755 --- a/eco/bernina/config.py +++ b/eco/bernina/config.py @@ -312,7 +312,8 @@ components = [ "z_und": 142, "desc": "General purpose station", "type": "eco.endstations.bernina_diffractometers:GPS", - "kwargs": {"Id": "SARES22-GPS", "configuration": config["gps_config"]}, + "kwargs": {"Id": "SARES22-GPS", "configuration": config["gps_config"],"fina_hex_angle_offset":"~/eco/reference_values/hex_pi_angle_offset.json"}, + "lazy":False, }, { "args": [], @@ -580,7 +581,7 @@ components = [ "z_und": 141, "desc": "Upstream diagnostics slits", "type": "eco.xoptics.slit_USD:Upstream_diagnostic_slits", - "kwargs": {"right": "ESB1", "left": "ESB2", "up": "ESB17", "down": "ESB16"}, + "kwargs": {"right": "LIC4", "left": "LIC3", "up": "LIC2", "down": "LIC1"}, "lazy": True, }, { @@ -589,7 +590,7 @@ components = [ "z_und": 141, "desc": "Upstream diagnostics slits", "type": "eco.xoptics.slit_USD:Upstream_diagnostic_slits", - "kwargs": {"right": "ESB8", "left": "ESB9", "up": "ESB18", "down": "ESB3"}, + "kwargs": {"right": "LIC7", "left": "LIC8", "up": "LIC6", "down": "LIC5"}, "lazy": True, }, { diff --git a/eco/devices_general/adjustable.py b/eco/devices_general/adjustable.py index 5f6dd8a..aaaf03f 100644 --- a/eco/devices_general/adjustable.py +++ b/eco/devices_general/adjustable.py @@ -9,6 +9,8 @@ import time import logging import datetime import numpy as np +from pathlib import Path +from json import load, dump logger = logging.getLogger(__name__) @@ -21,16 +23,18 @@ class AdjustableError(Exception): # wrappers for adjustables >>>>>>>>>>> def default_representation(Obj): def get_name(Obj): - if Obj.alias: + if hasattr(Obj,'alias') and Obj.alias: return Obj.alias.get_full_name() elif Obj.name: return Obj.name - else: + elif hasattr(Obj,'Id') and Obj.Id: return Obj.Id + else: + return '' def get_repr(Obj): s = datetime.datetime.now().strftime("%Y/%m/%d %H:%M:%S") + ": " - s += f"{colorama.Style.BRIGHT}{Obj._get_name()}{colorama.Style.RESET_ALL} at {colorama.Style.BRIGHT}{Obj.get_current_value():g}{colorama.Style.RESET_ALL}" + s += f"{colorama.Style.BRIGHT}{Obj._get_name()}{colorama.Style.RESET_ALL} at {colorama.Style.BRIGHT}{str(Obj.get_current_value())}{colorama.Style.RESET_ALL}" return s Obj._get_name = get_name @@ -255,7 +259,30 @@ def _keywordChecker(kw_key_list_tups): for tkw, tkey, tlist in kw_key_list_tups: assert tkey in tlist, "Keyword %s should be one of %s" % (tkw, tlist) +@default_representation +@spec_convenience +class AdjustableFS: + def __init__(self,file_path,name=None): + self.file_path = Path(file_path) + self.name = name + def get_current_value(self): + with open(self.file_path,'r') as f: + res = load(f) + return res['value'] + + def _write_value(self,value): + with open(self.file_path,'w') as f: + dump({'value':value},f) + + def set_target_value(self,value,hold=False): + return Changer( + target=value, parent=self, changer=self._write_value, hold=hold, stopper=None + ) + + + +@spec_convenience class PvRecord: def __init__( self, pvsetname, pvreadbackname=None, accuracy=None, name=None, elog=None @@ -317,22 +344,22 @@ class PvRecord: ) # spec-inspired convenience methods - def mv(self, value): - self._currentChange = self.set_target_value(value) + # def mv(self, value): + # self._currentChange = self.set_target_value(value) - def wm(self, *args, **kwargs): - return self.get_current_value(*args, **kwargs) + # def wm(self, *args, **kwargs): + # return self.get_current_value(*args, **kwargs) - def mvr(self, value, *args, **kwargs): + # def mvr(self, value, *args, **kwargs): - if self.get_moveDone == 1: - startvalue = self.get_current_value(readback=True, *args, **kwargs) - else: - startvalue = self.get_current_value(readback=False, *args, **kwargs) - self._currentChange = self.set_target_value(value + startvalue, *args, **kwargs) + # if self.get_moveDone == 1: + # startvalue = self.get_current_value(readback=True, *args, **kwargs) + # else: + # startvalue = self.get_current_value(readback=False, *args, **kwargs) + # self._currentChange = self.set_target_value(value + startvalue, *args, **kwargs) - def wait(self): - self._currentChange.wait() + # def wait(self): + # self._currentChange.wait() def __repr__(self): return "%s is at: %s" % (self.Id, self.get_current_value()) @@ -399,6 +426,7 @@ class AdjustableVirtual: adjustables, foo_get_current_value, foo_set_target_value_current_value, + change_simultaneously=True, reset_current_value_to=False, append_aliases=False, name=None, @@ -416,6 +444,7 @@ class AdjustableVirtual: self._foo_set_target_value_current_value = foo_set_target_value_current_value self._foo_get_current_value = foo_get_current_value self._reset_current_value_to = reset_current_value_to + self._change_simultaneously = change_simultaneously if reset_current_value_to: for adj in self._adjustables: if not hasattr(adj, "reset_current_value_to"): @@ -427,12 +456,18 @@ class AdjustableVirtual: vals = (vals,) def changer(value): - self._active_changers = [ - adj.set_target_value(val, hold=False) - for val, adj in zip(vals, self._adjustables) - ] - for tc in self._active_changers: - tc.wait() + if self._change_simultaneously: + self._active_changers = [ + adj.set_target_value(val, hold=False) + for val, adj in zip(vals, self._adjustables) + ] + for tc in self._active_changers: + tc.wait() + else: + + for val, adj in zip(vals, self._adjustables): + self._active_changers = [adj.set_target_value(val, hold=False)] + self._active_changers[0].wait() def stopper(): for tc in self._active_changers: diff --git a/eco/devices_general/motors.py b/eco/devices_general/motors.py index c47f26f..fa73707 100755 --- a/eco/devices_general/motors.py +++ b/eco/devices_general/motors.py @@ -232,7 +232,8 @@ class MotorRecord: print(" ") p.stepsize = step_value p.print(value=self.get_current_value()) - self.add_value_callback(p.print) + ind_callback = self.add_value_callback(p.print) + pv.put(step_value) while k.isq() is False: if oldstep != step_value: p.stepsize = step_value @@ -273,7 +274,9 @@ class MotorRecord: break else: print(help) + self.clear_value_callback(index=ind_callback) print(f"final position: {self.get_current_value()}") + print(f"final tweak step: {pv.get()}") def tweak(self, *args, **kwargs): return self._tweak_ioc(*args, **kwargs) diff --git a/eco/endstations/bernina_diffractometers.py b/eco/endstations/bernina_diffractometers.py index af53dc8..a73d301 100644 --- a/eco/endstations/bernina_diffractometers.py +++ b/eco/endstations/bernina_diffractometers.py @@ -6,6 +6,8 @@ from ..devices_general.adjustable import PvRecord from epics import PV from ..aliases import Alias, append_object_to_object +from ..endstations.hexapod import HexapodPI +from pathlib import Path def addMotorRecordToSelf(self, name=None, Id=None): @@ -18,7 +20,7 @@ def addMotorRecordToSelf(self, name=None, Id=None): class GPS: def __init__( - self, name=None, Id=None, configuration=["base"], alias_namespace=None + self, name=None, Id=None, configuration=["base"], alias_namespace=None, fina_hex_angle_offset=None ): self.Id = Id self.name = name @@ -41,55 +43,18 @@ class GPS: addMotorRecordToSelf(self, Id=Id + ":MOT_HEX_TX", name="tphi") if "phi_hex" in self.configuration: + ### motors PI hexapod ### + if fina_hex_angle_offset: + fina_hex_angle_offset = Path(fina_hex_angle_offset).expanduser() + append_object_to_object( self, - PvRecord, - "SARES20-HEX_PI:SET-POSI-X", - pvreadbackname="SARES20-HEX_PI:POSI-X", - name="xhex", + HexapodPI, + "SARES20-HEX_PI", + name="hex", + fina_angle_offset = fina_hex_angle_offset ) - append_object_to_object( - self, - PvRecord, - "SARES20-HEX_PI:SET-POSI-Y", - pvreadbackname="SARES20-HEX_PI:POSI-Y", - name="yhex", - ) - append_object_to_object( - self, - PvRecord, - "SARES20-HEX_PI:SET-POSI-Z", - pvreadbackname="SARES20-HEX_PI:POSI-Z", - name="zhex", - ) - append_object_to_object( - self, - PvRecord, - "SARES20-HEX_PI:SET-POSI-U", - pvreadbackname="SARES20-HEX_PI:POSI-U", - name="uhex", - ) - append_object_to_object( - self, - PvRecord, - "SARES20-HEX_PI:SET-POSI-V", - pvreadbackname="SARES20-HEX_PI:POSI-V", - name="vhex", - ) - append_object_to_object( - self, - PvRecord, - "SARES20-HEX_PI:SET-POSI-W", - pvreadbackname="SARES20-HEX_PI:POSI-W", - name="whex", - ) - # self.hex_x = PV("SARES20-HEX_PI:POSI-X") - # self.hex_y = PV("SARES20-HEX_PI:POSI-Y") - # self.hex_z = PV("SARES20-HEX_PI:POSI-Z") - # self.hex_u = PV("SARES20-HEX_PI:POSI-U") - # self.hex_v = PV("SARES20-HEX_PI:POSI-V") - # self.hex_w = PV("SARES20-HEX_PI:POSI-W") if "hlxz" in self.configuration: ### motors heavy load goniometer ### diff --git a/eco/endstations/hexapod.py b/eco/endstations/hexapod.py index 8828d1e..a0b27af 100644 --- a/eco/endstations/hexapod.py +++ b/eco/endstations/hexapod.py @@ -1,6 +1,9 @@ from epics import PV -from ..devices_general.adjustable import PvEnum +from ..devices_general.adjustable import PvEnum,PvRecord,AdjustableFS,AdjustableVirtual from time import sleep +from ..aliases import append_object_to_object,Alias +from scipy.spatial.transform import Rotation +import datetime class Hexapod_PI: def __init__(self, Id): @@ -18,6 +21,77 @@ class Hexapod_PI: for i in "RST" ] +class HexapodPI: + def __init__(self,pvname,name=None,fina_angle_offset=None): + self.name = name + self.alias = Alias(name) + self.pvname = pvname + append_object_to_object(self,PvRecord,self.pvname+':SET-POSI-X',pvreadbackname=self.pvname+':POSI-X',accuracy=.001,name='x_raw') + append_object_to_object(self,PvRecord,self.pvname+':SET-POSI-Y',pvreadbackname=self.pvname+':POSI-Y',accuracy=.001,name='y_raw') + append_object_to_object(self,PvRecord,self.pvname+':SET-POSI-Z',pvreadbackname=self.pvname+':POSI-Z',accuracy=.001,name='z_raw') + append_object_to_object(self,PvRecord,self.pvname+':SET-POSI-U',pvreadbackname=self.pvname+':POSI-U',accuracy=.001,name='rx_raw') + append_object_to_object(self,PvRecord,self.pvname+':SET-POSI-V',pvreadbackname=self.pvname+':POSI-V',accuracy=.001,name='ry_raw') + append_object_to_object(self,PvRecord,self.pvname+':SET-POSI-W',pvreadbackname=self.pvname+':POSI-W',accuracy=.001,name='rz_raw') + append_object_to_object(self,PvRecord,self.pvname+':SET-PIVOT-R',pvreadbackname=self.pvname+':PIVOT-R',accuracy=.001,name='pivot_x') + append_object_to_object(self,PvRecord,self.pvname+':SET-PIVOT-S',pvreadbackname=self.pvname+':PIVOT-S',accuracy=.001,name='pivot_y') + append_object_to_object(self,PvRecord,self.pvname+':SET-PIVOT-T',pvreadbackname=self.pvname+':PIVOT-T',accuracy=.001,name='pivot_z') + if fina_angle_offset: + self.ref_frame_angle = AdjustableFS(fina_angle_offset) + self.x = AdjustableVirtual( + [self.x_raw,self.y_raw,self.z_raw], + lambda xraw,yraw,zraw: self._calc_xyz(xraw,yraw,zraw)[0], + lambda x:self._calc_xyzraw(x,self.y.get_current_value(),self.z.get_current_value()), + reset_current_value_to=False, + append_aliases=False, + name='x', + ) + self.y = AdjustableVirtual( + [self.x_raw,self.y_raw,self.z_raw], + lambda xraw,yraw,zraw: self._calc_xyz(xraw,yraw,zraw)[1], + lambda y:self._calc_xyzraw(self.x.get_current_value(),y,self.z.get_current_value()), + reset_current_value_to=False, + append_aliases=False, + name='y', + ) + self.z = AdjustableVirtual( + [self.x_raw,self.y_raw,self.z_raw], + lambda xraw,yraw,zraw: self._calc_xyz(xraw,yraw,zraw)[2], + lambda z: self._calc_xyzraw(self.x.get_current_value(),self.y.get_current_value(),z), + reset_current_value_to=False, + append_aliases=False, + name='z', + ) + @property + def rotation(self): + angs = self.ref_frame_angle.get_current_value() + angs = [angs['rx'],angs['ry'],angs['rz']] + return Rotation.from_euler('xyz',angs,degrees=True) + + def _calc_xyz(self,xraw,yraw,zraw): + return self.rotation.apply([xraw,yraw,zraw]) + + def _calc_xyzraw(self,x,y,z): + print(self.rotation.inv().apply([x,y,z])) + return self.rotation.inv().apply([x,y,z]) + + def get_status(self): + s = f'Hexapod {self.alias.get_full_name()} status ({datetime.datetime.now().strftime("%Y/%m/%d %H:%M:%S")})\n' + if hasattr(self,'ref_frame_angle'): + for var in ['x','y','z']: + s+=(' '*4+var.ljust(16)+f'{self.__dict__[var].get_current_value():g}\n') + s+= ' '*4+'ref_frame_angle'.ljust(16)+str(self.ref_frame_angle.get_current_value())+'\n' + for var in ['x_raw','y_raw','z_raw','rx_raw','ry_raw','rz_raw','pivot_x','pivot_y','pivot_z']: + s+=(' '*4+var.ljust(16)+f'{self.__dict__[var].get_current_value():g}\n') + return s + + def __str__(self): + return self.get_status() + + def __repr__(self): + return self.__str__() + + + class HexapodSymmetrie: def __init__(self,pv_master='SARES20-HEXSYM',name='hex_usd',offset=[0,0,0,0,0,0]): @@ -43,7 +117,14 @@ class HexapodSymmetrie: } self._ctrl_pv = PV(f'{self.pvname}:STATE#PANEL:SET.VAL') - def set_coordinates(self,x,y,z,rx,ry,rz): + def set_coordinates(self,x,y,z,rx,ry,rz,relative_to_eco_offset=True): + if relative_to_eco_offset: + x = x+self.offset[0] + y = y+self.offset[1] + z = z+self.offset[2] + rx = rx+self.offset[3] + ry = ry+self.offset[4] + rz = rz+self.offset[5] self.pvs_setpos['x'].put(x) self.pvs_setpos['y'].put(y) self.pvs_setpos['z'].put(z) @@ -51,13 +132,20 @@ class HexapodSymmetrie: self.pvs_setpos['ry'].put(ry) self.pvs_setpos['rz'].put(rz) - def get_coordinates(self): + def get_coordinates(self,relative_to_eco_offset=True): x = self.pvs_getpos['x'].get() y = self.pvs_getpos['y'].get() z = self.pvs_getpos['z'].get() rx = self.pvs_getpos['rx'].get() ry = self.pvs_getpos['ry'].get() rz = self.pvs_getpos['rz'].get() + if relative_to_eco_offset: + x = x-self.offset[0] + y = y-self.offset[1] + z = z-self.offset[2] + rx = rx-self.offset[3] + ry = ry-self.offset[4] + rz = rz-self.offset[5] return x,y,z,rx,ry,rz def set_control_on(self): @@ -79,18 +167,11 @@ class HexapodSymmetrie: def move_to_coordinates(self,x,y,z,rx,ry,rz,precision=[.001,.001,.001,.001,.001,.001],coordinate_type='absolute',relative_to_eco_offset=True): self.coordinate_switch.set_target_value(coordinate_type).wait() - if relative_to_eco_offset: - x = x+self.offset[0] - y = y+self.offset[1] - z = z+self.offset[2] - rx = rx+self.offset[3] - ry = ry+self.offset[4] - rz = rz+self.offset[5] - self.set_coordinates(x,y,z,rx,ry,rz) + self.set_coordinates(x,y,z,rx,ry,rz,relative_to_eco_offset=relative_to_eco_offset) sleep(.1) self.start_move(target=(x,y,z,rx,ry,rz),precision=precision,coordinate_type=coordinate_type) - def start_move(self,target=None,precision=[.001,.001,.001,.001,.001,.001],coordinate_type='absolute'): + def start_move(self,target=None,precision=[.001,.001,.001,.001,.001,.001],coordinate_type='absolute',relative_to_eco_offset=True): print('Starting to move... stop with Ctrl-C') self.set_control_on() sleep(0.2) @@ -98,7 +179,7 @@ class HexapodSymmetrie: while 1: try: if target: - coo = self.get_coordinates() + coo = self.get_coordinates(relative_to_eco_offset=relative_to_eco_offset) if all([abs(ctarg-cnow)