From 7edea7bba4b76aaea983c4f09cfc1ba794a0e825 Mon Sep 17 00:00:00 2001 From: Mathias Sander Date: Mon, 13 Nov 2023 16:23:18 +0100 Subject: [PATCH] implemented robot visualiyation methods for live viewing of robot motions and simulation of motions --- eco/bernina/bernina.py | 45 +++++++++++++++- eco/bernina/config.py | 35 +------------ eco/devices_general/motors.py | 2 + eco/endstations/bernina_diffractometers.py | 18 +++++++ eco/endstations/bernina_robots.py | 61 ++++++++++++++++++++-- eco/timing/event_timing_new_new.py | 4 +- eco/utilities/feedback.py | 11 ++-- eco/utilities/runtable.py | 10 ++-- eco/xoptics/dcm_new.py | 2 +- eco/xoptics/dcm_pathlength_compensation.py | 7 +-- eco/xoptics/offsetMirrors_new.py | 5 ++ 11 files changed, 148 insertions(+), 52 deletions(-) diff --git a/eco/bernina/bernina.py b/eco/bernina/bernina.py index d2f1619..bc603bf 100644 --- a/eco/bernina/bernina.py +++ b/eco/bernina/bernina.py @@ -170,6 +170,43 @@ namespace.append_obj( module_name="eco.xoptics.slits", lazy=True, ) + +# namespace.append_obj( +# "Pprm", +# "SARFE10-PPRM064", +# "SARFE10-PPRM064", +# name= "prof_fe", +# # "z_und": 64, +# # "desc": "Profile monitor after Front End", +# module_name="eco.xdiagnostics.profile_monitors", +# ) +# namespace.append_obj( +# "Pprm", +# "SAROP11-PPRM066", +# "SAROP11-PPRM066", +# name= "prof_mirr_alv1", +# # "z_und": 66, +# # "desc": "Profile monitor after Alvra Mirror 1", +# module_name="eco.xdiagnostics.profile_monitors", +# ) +# namespace.append_obj( +# "Pprm", +# "SAROP21-PPRM094", +# "SAROP21-PPRM094", +# name= "prof_mirr1", +# # "z_und": 94, +# # "desc": "Profile monitor after Mirror 1", +# module_name="eco.xdiagnostics.profile_monitors", +# ) + + + +namespace.append_obj( + "OffsetMirrorsBernina", + name="offset", + module_name="eco.xoptics.offsetMirrors_new", +) + namespace.append_obj( "SlitBlades", "SAROP21-OAPU102", @@ -791,6 +828,12 @@ namespace.append_obj( name="gps", pvname="SARES22-GPS", configuration=config_bernina.gps_config(), + pgroup_adj=config_bernina.pgroup, + configsjf_adj=config_JFs, + detectors=[ + {"name": "det_fluo", "jf_id": "JF04T01V01"}, + {"name": "det_vHamos", "jf_id": "JF05T01V01"}, + ], fina_hex_angle_offset="/sf/bernina/config/eco/reference_values/hex_pi_angle_offset.json", lazy=True, ) @@ -800,7 +843,7 @@ namespace.append_obj( module_name="eco.endstations.bernina_robots", name="rob", pshell_url="http://PC14742:8080/", - lazy=True, + lazy=False, ) namespace.append_obj( diff --git a/eco/bernina/config.py b/eco/bernina/config.py index 4a6eacf..e64f100 100755 --- a/eco/bernina/config.py +++ b/eco/bernina/config.py @@ -153,39 +153,8 @@ components = [ # "args": [], # "kwargs": {}, # }, - { - "name": "prof_fe", - "args": ["SARFE10-PPRM064"] * 2, - "kwargs": {}, - "z_und": 64, - "desc": "Profile monitor after Front End", - "type": "eco.xdiagnostics.profile_monitors:Pprm", - }, - { - "name": "prof_mirr_alv1", - "args": ["SAROP11-PPRM066"] * 2, - "kwargs": {}, - "z_und": 66, - "desc": "Profile monitor after Alvra Mirror 1", - "type": "eco.xdiagnostics.profile_monitors:Pprm", - }, - { - "name": "prof_mirr1", - "args": ["SAROP21-PPRM094"] * 2, - "kwargs": {}, - "z_und": 94, - "desc": "Profile monitor after Mirror 1", - "type": "eco.xdiagnostics.profile_monitors:Pprm", - }, - { - "name": "offset", - "args": [], - "kwargs": {}, - "z_und": 96, - "desc": "offset mirrors in pink mode", - "type": "eco.xoptics.offsetMirrors_new:OffsetMirrorsBernina", - "kwargs": {}, - }, + + # { # "name": "mono_old", # "args": ["SAROP21-ODCM098"], diff --git a/eco/devices_general/motors.py b/eco/devices_general/motors.py index bb39de5..15d3cce 100755 --- a/eco/devices_general/motors.py +++ b/eco/devices_general/motors.py @@ -448,6 +448,8 @@ class PshellMotor(Assembly): self._cb = None def move(self, value, check=True, wait=False, update_value_time=0.05, timeout=120): + if not self.robot.config.powered(): + self.robot.config.powered(True) if check: lim_low, lim_high = self.get_limits() if not ((lim_low < value) and (value < lim_high)): diff --git a/eco/endstations/bernina_diffractometers.py b/eco/endstations/bernina_diffractometers.py index 7c32a9d..d2f9c3e 100644 --- a/eco/endstations/bernina_diffractometers.py +++ b/eco/endstations/bernina_diffractometers.py @@ -32,8 +32,11 @@ class GPS(Assembly): pvname=None, configuration=["base"], alias_namespace=None, + pgroup_adj=None, + configsjf_adj=None, fina_hex_angle_offset=None, diffcalc=False, + detectors=None, ): super().__init__(name=name) self.pvname = pvname @@ -283,6 +286,21 @@ class GPS(Assembly): is_setting=False, is_display=False, ) + if detectors: + for tdet in detectors: + tname = tdet["name"] + tid = tdet["jf_id"] + self._append( + Jungfrau, + tid, + name=tname, + is_setting=False, + is_display=False, + pgroup_adj=pgroup_adj, + config_adj=configsjf_adj, + view_toplevel_only=True, + ) + def gui(self, guiType="xdm"): """Adjustable convention""" diff --git a/eco/endstations/bernina_robots.py b/eco/endstations/bernina_robots.py index bd75834..bdbe733 100644 --- a/eco/endstations/bernina_robots.py +++ b/eco/endstations/bernina_robots.py @@ -1,12 +1,15 @@ from eco.pshell.client import PShellClient from eco.elements.assembly import Assembly -from eco.elements.adjustable import AdjustableVirtual, AdjustableGetSet, value_property +from eco.elements.adjustable import AdjustableFS, AdjustableGetSet, value_property from eco.devices_general.motors import PshellMotor from eco.elements.detector import DetectorGet from eco.elements.adj_obj import AdjustableObject, DetectorObject from eco.devices_general.utilities import Changer +from threading import Thread import time import numpy as np +import os +os.sys.path.insert(0, "/sf/bernina/config/src/python/bernina_urdf/") class RobotError(Exception): pass @@ -60,6 +63,15 @@ class StaeubliTx200(Assembly): ] for [name, name_pshell, unit] in motors: self._append(PshellMotor, robot=self, name=name, name_pshell=name_pshell, unit=unit, is_setting=True, is_display=True) + self._urdf = None + try: + import bernina_urdf + self._urdf = bernina_urdf.models.Tx200_Ceiling() + self._append(AdjustableFS, f'/sf/bernina/config/eco/reference_values/robot_auto_update_simulation.json', default_value=True, name="auto_update_simulation", is_setting=False) + self._auto_update_simulation_thread = Thread(target=self._auto_updater_simulation) + self._auto_update_simulation_thread.start() + except: + print("Loading bernina URDF robot model failed") def _get_info(self): return {k: v for k, v in self._cache.items() if k in self._info_fields} @@ -87,7 +99,7 @@ class StaeubliTx200(Assembly): return self._run_cmd(" ".join(cmd)) ######## Motion simulation ########## - def simulate_sphercial_motion(self, t_det=None, gamma=None, delta=None, coordinates="joint"): + def simulate_sphercial_motion(self, t_det=None, gamma=None, delta=None, coordinates="joint", plot=True): """ Simulated motion in the spherical coordinate system using a linear moteion movel command to change the radius from point tcp_p_spherical[0] to tcp_p_spherical[1], followed @@ -107,9 +119,19 @@ class StaeubliTx200(Assembly): coordinates is returned. Setting coordinates only has an effect, when the motion is simulated. """ sim = self._get_eval_result(f"robot.move_spherical(r={t_det}, gamma={gamma}, delta={delta}, simulate=True, coordinates='{coordinates}')") - return np.array(sim) + if plot: + if self._urdf is not None: + self.auto_update_simulation(False) + self._urdf.sim.move_trajectory(sim) + res = "" + while not res in ["y", "n"]: + res = input("Resume real time visualization of bernina robot in the hutch (y/n)?: ") + if res == "y": + self.auto_update_simulation(True) + else: + return np.array(sim) - def simulate_cartesian_motion(self, x=None, y=None, z=None, rx=None, ry=None, rz=None, coordinates="joint"): + def simulate_cartesian_motion(self, x=None, y=None, z=None, rx=None, ry=None, rz=None, coordinates="joint", plot=True): """ Simulated motion in the cartesian coordinate system using a linear motion movel command to move from point tcp_p_spherical[0] to tcp_p_spherical[1]. @@ -127,9 +149,38 @@ class StaeubliTx200(Assembly): coordinates is returned. Setting coordinates only has an effect, when the motion is simulated. """ sim = self._get_eval_result(f"robot.move_cartesian(x={x}, y={y}, z={z}, rx={rx}, ry={ry}, rz={rz}, simulate=True, coordinates='{coordinates}')") - return np.array(sim) + if plot: + if self._urdf is not None: + self.auto_update_simulation(False) + self._urdf.sim.move_trajectory(sim) + res = "" + while not res in ["y", "n"]: + res = input("Resume real time visualization of bernina robot in the hutch (y/n)?: ") + if res == "y": + self.auto_update_simulation(True) + else: + return np.array(sim) + + def simulate_current_pos(self): + js = np.array([self._cache["pos"][k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]]) + self._urdf.sim.pos = js + self._urdf.sim._ensure_vis_running() + self._urdf.sim.vis.step(0) + + def show(self): + self._urdf.sim.show() ######## Helper functions ########## + def _auto_updater_simulation(self):# + while(True): + js = np.array([self._cache["pos"][k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]]) + if self.auto_update_simulation(): + if not np.all(js == self._urdf.sim.pos): + self._urdf.sim.pos = js + if self._urdf.sim._vis_running(): + self._urdf.sim.vis.step(0) + time.sleep(.05) + def _get_on_poll_info(self): cfg = self._get_eval_result("robot.on_poll_info()") self._info_fields = cfg["info"] diff --git a/eco/timing/event_timing_new_new.py b/eco/timing/event_timing_new_new.py index 46f9cd3..861849e 100644 --- a/eco/timing/event_timing_new_new.py +++ b/eco/timing/event_timing_new_new.py @@ -302,7 +302,7 @@ class EvrPulser(Assembly): ) self.description = EpicsString(pv_base + "-Name-I") - if True: #self._eventcode is not None: + if self._eventcode is not None: self._append( AdjustableVirtual, [self._eventcode.frequency], @@ -324,6 +324,8 @@ class EvrPulser(Assembly): lambda x: x - self.delay_eventcode.get_current_value(), name="delay", ) + else: + print(f"Error initializing pulser {self.name} of EVR {self.pv_base}: Event code {self.eventcode.get_current_value()} is missing in Timing Master") @property def _eventcode(self): diff --git a/eco/utilities/feedback.py b/eco/utilities/feedback.py index 1199d91..e9a055b 100644 --- a/eco/utilities/feedback.py +++ b/eco/utilities/feedback.py @@ -171,23 +171,26 @@ class Feedback(Assembly): if self.callback_stop_feedback: self.callback_stop_feedback() - def run_continuously(self): + def run_continuously(self, set_control=True): while self.running.get_current_value(): valcurr = self.foo_detector() set_val = self.pid_object(valcurr) if self.callback_start_control: self.callback_start_control() - self.control_adj.set_target_value(set_val).wait() + if set_control: + self.control_adj.set_target_value(set_val).wait() self.feedback_history.append([valcurr, set_val]) if self.callback_stop_control: self.callback_stop_control() - def start_feedback(self): + def start_feedback(self, set_control=True): if self.callback_start_feedback: self.callback_start_feedback() self.create_new_pid() self.running.set_target_value(True).wait() - self.feedback = Thread(target=self.run_continuously) + self.feedback = Thread( + target=self.run_continuously, kwargs={"set_control": set_control} + ) self.feedback.start() diff --git a/eco/utilities/runtable.py b/eco/utilities/runtable.py index 3e8f2af..c05241b 100644 --- a/eco/utilities/runtable.py +++ b/eco/utilities/runtable.py @@ -418,10 +418,10 @@ class Run_Table_DataFrame(DataFrame): self.bad_adjustables = {} ###parsing options - self._parse_exclude_keys = "status_indicators settings_collection status_indicators_collection presets memory _elog _currentChange _flags __ alias namespace daq scan MasterEventSystem _motor Alias".split( + self._parse_exclude_keys = "status_indicators rob settings_collection status_indicators_collection presets memory _elog _currentChange _flags __ alias namespace daq scan MasterEventSystem _motor Alias".split( " " ) - self._parse_exclude_class_types = "__ alias namespace daq scan MasterEventSystem _motor Alias AdjustablePv Collection".split( + self._parse_exclude_class_types = "__ alias namespace daq scan MasterEventSystem _motor Alias StaeubliTx200 AdjustablePv Collection".split( " " ) self._adj_exclude_class_types = ( @@ -597,6 +597,8 @@ class Run_Table_DataFrame(DataFrame): def _get_all_adjustables_fewerparents( self, device, adj_prefix=None, parent_name=None, verbose=False ): + if verbose: + print(f"parsing children of {parent_name}") if adj_prefix is not None: name = ".".join([adj_prefix, device.name]) else: @@ -821,9 +823,9 @@ class Run_Table_DataFrame(DataFrame): #### diagnostic and convenience functions #### - def check_timeouts(self, include_bad_adjustables=True, repeats=1, plot=True): + def check_timeouts(self, include_bad_adjustables=True, repeats=1, plot=True, verbose=True): if len(self.adjustables) == 0: - self._parse_parent_fewerparents() + self._parse_parent_fewerparents(verbose=verbose) ts = [] devs=[] def get_dev_adjs(dev): diff --git a/eco/xoptics/dcm_new.py b/eco/xoptics/dcm_new.py index d7378ed..7e2acfc 100644 --- a/eco/xoptics/dcm_new.py +++ b/eco/xoptics/dcm_new.py @@ -124,7 +124,7 @@ class DoubleCrystalMono(Assembly): AdjustablePv, energy_sp, pvreadbackname=energy_rb, - accuracy=0.5, + accuracy=0.1, name="energy", ) self._append( diff --git a/eco/xoptics/dcm_pathlength_compensation.py b/eco/xoptics/dcm_pathlength_compensation.py index d85af26..0d3b1d1 100644 --- a/eco/xoptics/dcm_pathlength_compensation.py +++ b/eco/xoptics/dcm_pathlength_compensation.py @@ -25,7 +25,8 @@ def energy2tthe(energy, hkl=(1, 1, 1), material=materials.Si): def calcDcmExtension(energy, offset=20e-3, hkl=(1, 1, 1), material=materials.Si): tthe = energy2tthe(energy, hkl, material=material) - return offset / np.sin(tthe) * (1 - np.cos(tthe)) + return offset *(1/ np.sin(tthe) - 1/ np.tan(tthe)) + # return offset / np.sin(tthe) * (1 - np.cos(tthe)) class MonoTimecompensation(Assembly): @@ -85,7 +86,7 @@ class MonoTimecompensation(Assembly): delta_delay = x_delay / constants.c target_delay_adj = ( target_delay - + (self.laser_delay_inverted.get_current_value() * -2 + 1) * delta_delay + + (self.laser_delay_inverted.get_current_value() * -2 + 1) * delta_delay # NB: boolean to ± 1 conversion ) print("debug here") print(x_delay, delta_delay) @@ -98,7 +99,7 @@ class MonoTimecompensation(Assembly): delta_delay = x_delay / constants.c real_delay = ( delay_adjusted - - (self.laser_delay_inverted.get_current_value() * -2 + 1) * delta_delay + - (self.laser_delay_inverted.get_current_value() * -2 + 1) * delta_delay # NB: boolean to ± 1 conversion ) print("debug here") print(x_delay, delta_delay) diff --git a/eco/xoptics/offsetMirrors_new.py b/eco/xoptics/offsetMirrors_new.py index 55e9c20..1ae486a 100644 --- a/eco/xoptics/offsetMirrors_new.py +++ b/eco/xoptics/offsetMirrors_new.py @@ -29,6 +29,11 @@ class OffsetMirror(Assembly): name="asymmetry", is_setting=True, ) + self._append( + AdjustablePvEnum, + self.pvname+":COATING", + pvname_set=self.pvname+":COATING_SP", + name='coating') class OffsetMirrorsBernina(Assembly):