motor_group.py 11.7 KB
Newer Older
1
2
3
4
# -*- coding: utf-8 -*-
#
# This file is part of the bliss project
#
Benoit Formet's avatar
Benoit Formet committed
5
# Copyright (c) 2015-2020 Beamline Control Unit, ESRF
6
7
# Distributed under the GNU LGPLv3. See LICENSE for more info.

8
import gevent
9
import numpy
10
import uuid
11
12
13

# absolute import to avoid circular import
import bliss.controllers.motor as motor
14
15
16
17
18
from bliss.common.axis import (
    _prepare_one_controller_motions,
    _start_one_controller_motions,
    _stop_one_controller_motions,
)
19
from bliss.common.axis import Axis, AxisState, GroupMove
20
from bliss.common.utils import grouped
21

22

23
24
25
26
def is_motor_group(obj):
    return isinstance(obj, _Group)


27
28
29
30
31
32
def Group(*axes_list):
    axes = dict()
    for axis in axes_list:
        if not isinstance(axis, Axis):
            raise ValueError("invalid axis %r" % axis)
        axes[axis.name] = axis
33
34
    # ensure a pseudo axis is not present with one of its corresponding real axes
    def check_axes(*axes_to_check):
35
        grp_axes = axes.values()
36
        for axis in axes_to_check:
37
            if isinstance(axis.controller, motor.CalcController):
38
39
40
41
42
43
44
45
46
47
48
49
50
                names = [
                    grp_axis.name
                    for grp_axis in grp_axes
                    if grp_axis in axis.controller.reals
                ]
                if names:
                    raise RuntimeError(
                        "Virtual axis '%s` cannot be present in group with any of its corresponding real axes: %r"
                        % (axis.name, names)
                    )
                # also check reals, that can be calc axes themselves too
                check_axes(*axis.controller.reals)

51
    check_axes(*axes.values())
52

53
54
55
56
    # group names will be generated, since _Group objects
    # are anonymous
    group_name = uuid.uuid4().hex
    g = _Group(group_name, axes)
57
    return g
58

59

60
class _Group(object):
61
62
    def __init__(self, name, axes_dict):
        self.__name = name
Vincent Michel's avatar
Vincent Michel committed
63
        self._group_move = GroupMove(self)
64
        self._axes = dict(axes_dict)
65
66
67
        self._axes_with_reals = {
            axis.name: axis for axis in motor.get_real_axes(*self.axes.values())
        }
68

Benoit Formet's avatar
Benoit Formet committed
69
70
71
72
73
74
75
    def __info__(self):
        info = "MOTOR GROUP:"
        info += "\n    axes: " + ", ".join(self.axes)
        info += "\n    is moving: " + str(self.is_moving)
        info += "\n    state: " + str(self.state)
        return info

76
77
78
79
80
81
82
83
    @property
    def name(self):
        return self.__name

    @property
    def axes(self):
        return self._axes

84
85
86
87
    @property
    def axes_with_reals(self):
        return self._axes_with_reals

88
89
    @property
    def is_moving(self):
90
91
92
        return self._group_move.is_moving or any(
            m.is_moving for m in self._axes.values()
        )
93

Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
94
    @property
95
    def state(self):
96
97
        if self.is_moving:
            return AxisState("MOVING")
98
        grp_state = AxisState("READY")
99
        for i, (name, state) in enumerate(
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
100
            [(axis.name, axis.state) for axis in self._axes.values()]
101
        ):
102
            if state.MOVING:
103
                new_state = "MOVING" + " " * i
104
                grp_state.create_state(
105
106
                    new_state, "%s: %s" % (name, grp_state._state_desc["MOVING"])
                )
107
                grp_state.set("MOVING")
108
                grp_state.set(new_state)
109
110
111
            for axis_state in state._current_states:
                if axis_state == "READY":
                    continue
112
                new_state = axis_state + " " * i
113
                grp_state.create_state(
114
115
                    new_state, "%s: %s" % (name, state._state_desc[axis_state])
                )
116
                grp_state.set(new_state)
117
        return grp_state
118

Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
119
    @property
120
    def position(self):
121
        return self._dial_or_position("position")
122

Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
123
    @property
124
    def dial(self):
125
126
127
128
        return self._dial_or_position("dial")

    @property
    def position_with_reals(self):
129
130
131
        """
        ???
        """
132
133
134
135
136
137
138
        return self._dial_or_position("position", with_reals=True)

    @property
    def dial_with_reals(self):
        return self._dial_or_position("dial", with_reals=True)

    def _dial_or_position(self, attr, with_reals=False):
139
140
141
        """
        ???
        """
142
        positions_dict = dict()
143
144
145
146
147
148
149
150

        if with_reals:
            axes = self._axes_with_reals
        else:
            axes = self.axes

        for axis in axes.values():
            positions_dict[axis] = getattr(axis, attr)
151
152
        return positions_dict

153
    def _check_ready(self):
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
154
        initial_state = self.state
155
        if not initial_state.READY:
156
            raise RuntimeError("all motors are not ready")
157

158
159
160
    def rmove(self, *args, **kwargs):
        kwargs["relative"] = True
        return self.move(*args, **kwargs)
Vincent Michel's avatar
Vincent Michel committed
161

162
163
    def move(self, *args, **kwargs):
        self._check_ready()
164

165
166
        wait = kwargs.pop("wait", True)
        relative = kwargs.pop("relative", False)
167
        polling_time = kwargs.pop("polling_time", None)
168
169
        axis_pos_dict = {}
        motions_dict = {}
Vincent Michel's avatar
Vincent Michel committed
170

171
        if len(args) == 1:
172
173
174
175
176
177
178
            axis, target_pos = next(iter(args[0].items()))
            if numpy.isfinite(target_pos):
                axis_pos_dict.update(args[0])
            else:
                raise RuntimeError(
                    f"axis {axis.name} cannot be moved to position: {target_pos}"
                )
179
180
        else:
            for axis, target_pos in grouped(args, 2):
181
182
183
184
185
186
                if numpy.isfinite(target_pos):
                    axis_pos_dict[axis] = target_pos
                else:
                    raise RuntimeError(
                        f"axis {axis.name} cannot be moved to position: {target_pos}"
                    )
Vincent Michel's avatar
Vincent Michel committed
187

188
189
        self._group_move = GroupMove(self)

Vincent Michel's avatar
Vincent Michel committed
190
        for axis, target_pos in axis_pos_dict.items():
191
192
193
            motion = axis.get_motion(
                target_pos, relative=relative, polling_time=polling_time
            )
194
            # motion can be None if axis is not supposed to move
195
            if motion is not None:
196
                motions_dict.setdefault(axis.controller, []).append(motion)
197
                motion.axis._group_move = self._group_move
198

Vincent Michel's avatar
Vincent Michel committed
199
200
        self._group_move.move(
            motions_dict,
201
202
203
            _prepare_one_controller_motions,
            _start_one_controller_motions,
            _stop_one_controller_motions,
204
205
206
            wait=wait,
            polling_time=polling_time,
        )
Matias Guijarro's avatar
Matias Guijarro committed
207
208

    def wait_move(self):
Vincent Michel's avatar
Vincent Michel committed
209
        self._group_move.wait()
210

211
    def stop(self, wait=True):
Vincent Michel's avatar
Vincent Michel committed
212
        self._group_move.stop(wait)
213

214

215
class TrajectoryGroup:
216
217
218
    """
    Group for motor trajectory
    """
219

220
    def __init__(self, *trajectories, **kwargs):
221
        calc_axis = kwargs.pop("calc_axis", None)
Vincent Michel's avatar
Vincent Michel committed
222
        self.__trajectories = trajectories
223
        self.__trajectories_dialunit = None
Vincent Michel's avatar
Vincent Michel committed
224
        self.__group = Group(*self.axes)
225
226
        self.__calc_axis = calc_axis
        self.__disabled_axes = set()
Vincent Michel's avatar
Vincent Michel committed
227

228
229
230
231
232
233
    @property
    def trajectories(self):
        """
        Get/Set trajectories for this movement
        """
        return self.__trajectories
234

235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
    @trajectories.setter
    def trajectories(self, trajectories):
        self.__trajectories = trajectories
        self.__trajectories_dialunit = None
        self.__group = Group(*self.axes)

    @property
    def axes(self):
        """
        Axes for this motion
        """
        return [t.axis for t in self.__trajectories]

    @property
    def disabled_axes(self):
        """
        Axes which are disabled for the next motion
        """
        return self.__disabled_axes

    def disable_axis(self, axis):
        """
        Disable an axis for the next motion
        """
        self.__disabled_axes.add(axis)

    def enable_axis(self, axis):
        """
        Enable an axis for the next motion
        """
        try:
            self.__disabled_axes.remove(axis)
267
268
        except KeyError:  # was already enable
            pass  # should we raise?
269
270
271
272
273
274
275
276

    @property
    def calc_axis(self):
        """
        calculation axis if any
        """
        return self.__calc_axis

277
278
279
    @property
    def trajectories_by_controller(self):
        controller_trajectories = dict()
280
281
282
283
        if self.__trajectories_dialunit is not None:
            for traj in self.__trajectories_dialunit:
                if traj.axis in self.__disabled_axes:
                    continue
284
                tlist = controller_trajectories.setdefault(traj.axis.controller, [])
285
                tlist.append(traj)
286
287
288
289
        return controller_trajectories

    @property
    def is_moving(self):
Vincent Michel's avatar
Vincent Michel committed
290
        return self.__group.is_moving
Vincent Michel's avatar
Vincent Michel committed
291

Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
292
    @property
293
    def state(self):
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
294
        return self.__group.state
Vincent Michel's avatar
Vincent Michel committed
295

296
297
298
299
300
301
    def prepare(self):
        """
        prepare/load trajectories in controllers
        """
        if self.__trajectories_dialunit is None:
            trajectories = list()
302
            for trajectory in self.trajectories:
303
                trajectories.append(trajectory.convert_to_dial())
304
            self.__trajectories_dialunit = trajectories
Vincent Michel's avatar
Vincent Michel committed
305

306
307
        prepare = [
            gevent.spawn(controller._prepare_trajectory, *trajectories)
Vincent Michel's avatar
Vincent Michel committed
308
            for controller, trajectories in self.trajectories_by_controller.items()
309
        ]
310
311
312
313
314
315
        try:
            gevent.joinall(prepare, raise_error=True)
        except:
            gevent.killall(prepare)
            raise

316
317
318
319
320
321
322
    def _prepare_move_to_trajectory(self, controller, motions):
        try:
            controller.prepare_all(*motions)
        except NotImplementedError:
            for motion in motions:
                controller.prepare_move(motion)

323
324
325
326
327
328
329
    def _move_to_trajectory(self, controller, motions):
        trajectories = self.trajectories_by_controller[controller]
        controller.move_to_trajectory(*trajectories)

    def _stop_trajectory(self, controller, motions):
        trajectories = self.trajectories_by_controller[controller]
        controller.stop_trajectory(*trajectories)
330

331
332
333
    def _start_trajectory(self, controller, motions):
        trajectories = self.trajectories_by_controller[controller]
        controller.start_trajectory(*trajectories)
Vincent Michel's avatar
Vincent Michel committed
334

335
    def move_to_start(self, wait=True, polling_time=None):
336
337
338
339
        """
        Move all enabled motors to the first point of the trajectory
        """
        self.__group._check_ready()
340
341
342

        motions_dict = {}
        for trajectory in self.trajectories:
343
            pvt = trajectory.pvt
344
            final_pos = pvt["position"][0]
345
            motion = trajectory.axis.get_motion(final_pos, polling_time)
346
347
348
            if not motion:
                # already at final pos
                continue
349
350
            # no backlash to go to the first position
            # otherwise it may break next trajectory motion (move_to_end)
351
            motion.backlash = 0
352
            motions_dict.setdefault(motion.axis.controller, []).append(motion)
353

Vincent Michel's avatar
Vincent Michel committed
354
355
        self.__group._group_move.move(
            motions_dict,
356
            self._prepare_move_to_trajectory,
Vincent Michel's avatar
Vincent Michel committed
357
358
359
            self._move_to_trajectory,
            self._stop_trajectory,
            wait=wait,
360
        )
361

362
    def move_to_end(self, wait=True, polling_time=None):
363
364
365
366
        """
        Move all enabled motors to the last point of the trajectory
        """
        self.__group._check_ready()
367
368
369

        motions_dict = {}
        for trajectory in self.trajectories:
370
            pvt = trajectory.pvt
371
            final_pos = pvt["position"][-1]
372
            motion = trajectory.axis.get_motion(final_pos, polling_time)
373
374
            if not motion:
                continue
375
            motions_dict.setdefault(motion.axis.controller, []).append(motion)
376

Vincent Michel's avatar
Vincent Michel committed
377
378
        self.__group._group_move.move(
            motions_dict,
379
            None,  # no prepare needed
Vincent Michel's avatar
Vincent Michel committed
380
381
382
            self._start_trajectory,
            self._stop_trajectory,
            wait=wait,
383
384
            polling_time=polling_time,
        )
385
386
387

    def stop(self, wait=True):
        """
388
        Stop the motion on all motors
389
        """
Vincent Michel's avatar
Vincent Michel committed
390
        self.__group.stop(wait)
Vincent Michel's avatar
Vincent Michel committed
391

392
    def wait_move(self):
Vincent Michel's avatar
Vincent Michel committed
393
        self.__group.wait_move()