Commit 09f3e01a authored by Emmanuel Papillon's avatar Emmanuel Papillon
Browse files

add motion hook for detector x collision

parent 047900fb
import numpy
from bliss.common.hook import MotionHook
from bliss.common.logtools import log_debug
from bliss.physics.trajectory import LinearTrajectory
class DetectorXCollision(Exception):
pass
class DetectorXCollisionHook(MotionHook):
def __init__(self, name, config):
super(DetectorXCollisionHook, self).__init__()
self.__disable = False
self.__inited = False
self.__xlimit = config.get("xlimit")
self.sampling_time = 0.1
def init(self):
log_debug(self, "init")
tags_needed = ["nfdtx", "ffdtx1"]
self.axes_role = dict()
for axis in self.axes.values():
axis_tags = axis.config.get("tags", default="")
for tag in tags_needed:
if axis.name == tag or tag in axis_tags:
self.axes_role[axis] = tag
log_debug(self, f"{tag} axis = {axis.name}")
for tag in tags_needed:
if tag not in self.axes_role.values():
raise ValueError(f"DetectorXCollisionHook: Missing {tag} axis in config")
self.__inited = True
def disable(self):
""" Disable check for next move only """
self.__disable = True
def enable(self):
self.__disable = False
def pre_move(self, motion_list):
log_debug(self, "pre_move")
if self.__disable is True:
log_debug(self, "disabled. No checks.")
self.__disable = False
return
pos = dict([(tag, axis.position) for axis,tag in self.axes_role.items()])
traj = dict()
for motion in motion_list:
if motion.axis in self.axes_role:
axis = motion.axis
tag = self.axes_role[axis]
target_pos = axis.dial2user(float(motion.target_pos) / float(axis.steps_per_unit))
traj[tag] = LinearTrajectory(pos[tag],
target_pos,
axis.velocity,
axis.acceleration,
0.
)
maxtime = max([ t.duration for t in traj.values() ])
for sample in numpy.arange(0., maxtime+self.sampling_time, self.sampling_time):
tpos = dict(pos)
for tag, t in traj.items():
tpos[tag] = t.position(sample)
self.__check_collision(tpos)
def __check_collision(self, tpos):
log_debug(self, "check positions : %s", tpos)
if tpos["ffdtx1"] - tpos["nfdtx"] < self.__xlimit:
errmsg = "Possible Detector X Collision at:\n"
errmsg += self.__pos_text(tpos)
raise DetectorXCollision(errmsg)
def post_move(self, motion_list):
log_debug(self, "post_move")
def __pos_text(self, tpos):
msg = list()
for axis,tag in self.axes_role.items():
msg.append(f"{axis.name} {tpos[tag]:.3f}")
return " ; ".join(msg)
def __current_pos(self):
tpos = dict([(tag, axis.position) for axis,tag in self.axes_role.items()])
return tpos
def __info__(self):
if not self.__inited:
self.init()
tpos = self.__current_pos()
msg = "Detector X Collision Check Hook.\n"
msg += self.__pos_text(tpos)
val = self.__check_collision(tpos)
return msg
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment