add motion hook for detector x collision

id11/tdxrd/motionhook.py

+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