Bernina/eco
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

implemented robot visualiyation methods for live viewing of robot motions and simulation of motions

Browse Source
This commit is contained in:
sander_m
2023-11-13 16:23:18 +01:00
parent 3be8c9cb0a
commit 7edea7bba4
11 changed files with 148 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
eco/bernina/bernina.py
+44 -1
View File
@@ -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(
eco/bernina/config.py
+2 -33
View File
@@ -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"],
eco/devices_general/motors.py
+2
View File
@@ -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)):
eco/endstations/bernina_diffractometers.py
+18
View File
@@ -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"""
eco/endstations/bernina_robots.py
+56 -5
View File
@@ -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"]
eco/timing/event_timing_new_new.py
+3 -1
View File
@@ -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):
eco/utilities/feedback.py
+7 -4
View File
@@ -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()
eco/utilities/runtable.py
+6 -4
View File
@@ -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):
eco/xoptics/dcm_new.py
+1 -1
View File
@@ -124,7 +124,7 @@ class DoubleCrystalMono(Assembly):
AdjustablePv,
energy_sp,
pvreadbackname=energy_rb,
accuracy=0.5,
accuracy=0.1,
name="energy",
)
self._append(
eco/xoptics/dcm_pathlength_compensation.py
+4 -3
View File
@@ -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)
eco/xoptics/offsetMirrors_new.py
+5
View File
@@ -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):