motor.py 35.7 KB
Newer Older
1
2
3
4
# -*- coding: utf-8 -*-
#
# This file is part of the bliss project
#
5
# Copyright (c) 2015-2022 Beamline Control Unit, ESRF
6
# Distributed under the GNU LGPLv3. See LICENSE for more info.
7

8
9
10
11
12
13
"""
bliss.controller.motor.EncoderCounterController
bliss.controller.motor.Controller
bliss.controller.motor.CalcController
"""

14
import functools
15
import numpy
16
import collections.abc
17
18
from gevent import lock

19
20
# absolute import to avoid circular import
import bliss.common.motor_group as motor_group
21
from bliss.common.motor_config import MotorConfig
22
from bliss.common.motor_settings import ControllerAxisSettings, floatOrNone
23
from bliss.common.axis import Trajectory
Lucas Felix's avatar
Lucas Felix committed
24
from bliss.common.closed_loop import ClosedLoopState
25
from bliss.common import event
26
from bliss.controllers.counter import SamplingCounterController
27
from bliss.physics import trajectory
28
from bliss.common.utils import set_custom_members, object_method, grouped
29
from bliss import global_map
30
from bliss.config.channels import Cache
Matias Guijarro's avatar
Matias Guijarro committed
31

32
33
from bliss.controllers.bliss_controller import BlissController

34

35
36
37
38
39
40
class EncoderCounterController(SamplingCounterController):
    def __init__(self, motor_controller):
        super().__init__("encoder")

        self.motor_controller = motor_controller

41
42
43
        # High frequency acquisition loop
        self.max_sampling_frequency = None

44
    def read_all(self, *encoders):
45
        steps_per_unit = numpy.array([enc.steps_per_unit for enc in encoders])
46
47
48
49
50
        try:
            positions_array = numpy.array(
                self.motor_controller.read_encoder_multiple(*encoders)
            )
        except NotImplementedError:
51
52
53
54
55
56
57
58
            try:
                positions_array = numpy.array(
                    list(map(self.motor_controller.read_encoder, encoders))
                )
            except Exception as exc:
                # raise any exception, but suppress context (first "NotImplementedError"),
                # see issue #3294
                raise exc from None
59
60
61
        return positions_array / steps_per_unit


62
def check_disabled(func):
63
64
65
66
67
    """
    Decorator used to raise exception if accessing an attribute of a disabled
    motor controller.
    """

68
69
70
    @functools.wraps(func)
    def func_wrapper(self, *args, **kwargs):
        if self._disabled:
71
            raise RuntimeError("Controller is disabled. Check hardware and restart.")
72
73
74
75
76
        return func(self, *args, **kwargs)

    return func_wrapper


77
class Controller(BlissController):
78
    """
79
    Motor controller base class
80
    """
81

82
83
84
85
86
87
88
89
    def __init__(self, *args, **kwargs):  # config

        if len(args) == 1:
            config = args[0]
        else:
            # handle old signature: args = [ name, config, axes, encoders, shutters, switches ]
            config = args[1]

90
91
92
        super().__init__(config)

        self.__motor_config = MotorConfig(config)
93
        self.__initialized_hw = Cache(self, "initialized", default_value=False)
94
        self.__initialized_hw_axis = dict()
95
96
        self.__initialized_encoder = dict()
        self.__initialized_axis = dict()
97
        self.__lock = lock.RLock()
98
        self._encoder_counter_controller = EncoderCounterController(self)
Matias Guijarro's avatar
Matias Guijarro committed
99
        self._axes = dict()
Matias Guijarro's avatar
Matias Guijarro committed
100
        self._encoders = dict()
101
102
        self._shutters = dict()
        self._switches = dict()
Matias Guijarro's avatar
Matias Guijarro committed
103
        self._tagged = dict()
104
        self._disabled = False
Matias Guijarro's avatar
Matias Guijarro committed
105

106
        self.axis_settings = ControllerAxisSettings()
107
        global_map.register(self, parents_list=["controllers"])
108

109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
    def _load_config(self):
        self._axes_config = {}
        self._encoders_config = {}
        self._shutters_config = {}
        self._switches_config = {}

        for k, v in self._subitems_config.items():
            cfg, pkey = v
            if pkey == "axes":
                self._axes_config[k] = cfg

            elif pkey == "encoders":
                self._encoders_config[k] = cfg

            elif pkey == "shutters":
                self._shutters_config[k] = cfg

            elif pkey == "switches":
                self._switches_config[k] = cfg

    def _get_subitem_default_module(self, class_name, cfg, parent_key):
        if parent_key == "axes":
            return "bliss.common.axis"

        elif parent_key == "encoders":
            return "bliss.common.encoder"

        elif parent_key == "shutters":
            return "bliss.common.shutter"

        elif parent_key == "switches":
            return "bliss.common.switch"

    def _get_subitem_default_class_name(self, cfg, parent_key):
        if parent_key == "axes":
            return "Axis"
        elif parent_key == "encoders":
            return "Encoder"
        elif parent_key == "shutters":
            return "Shutter"
        elif parent_key == "switches":
            return "Switch"

    @check_disabled
153
154
155
    def _create_subitem_from_config(
        self, name, cfg, parent_key, item_class, item_obj=None
    ):
156
157

        if parent_key == "axes":
158
159
            if item_class is None:  # it is a reference
                axis = item_obj
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
            else:
                axis = item_class(name, self, cfg)

            self._axes[name] = axis

            axis_tags = cfg.get("tags")
            if axis_tags:
                for tag in axis_tags.split():
                    self._tagged.setdefault(tag, []).append(axis)

            if axis.controller is self:
                set_custom_members(self, axis, self._initialize_axis)
            else:
                # reference axis
                return axis

            if axis.controller is self:
                axis_initialized = Cache(axis, "initialized", default_value=0)
                self.__initialized_hw_axis[axis] = axis_initialized
                self.__initialized_axis[axis] = False

            self._add_axis(axis)
            return axis

        elif parent_key == "encoders":
            encoder = self._encoder_counter_controller.create_counter(
                item_class, name, motor_controller=self, config=cfg
            )
            self._encoders[name] = encoder
            self.__initialized_encoder[encoder] = False
            return encoder
191

Perceval Guillou's avatar
Perceval Guillou committed
192
193
194
195
196
197
198
199
200
201
        elif parent_key == "switches":
            switch = item_class(name, cfg)
            self._switches[name] = switch
            return switch

        elif parent_key == "shutters":
            shutter = item_class(name, cfg)
            self._shutters[name] = shutter
            return shutter

202
    def _init(self):
203
204
        try:
            self.initialize()
Perceval Guillou's avatar
Perceval Guillou committed
205
            self._disabled = False
206
207
208
        except BaseException:
            self._disabled = True
            raise
209

210
211
212
213
    @property
    def config(self):
        return self.__motor_config

Matias Guijarro's avatar
Matias Guijarro committed
214
215
216
217
    @property
    def axes(self):
        return self._axes

218
219
220
221
    @property
    def encoders(self):
        return self._encoders

222
223
224
225
226
    @property
    def shutters(self):
        return self._shutters

    @property
Matias Guijarro's avatar
Matias Guijarro committed
227
    def switches(self):
228
229
        return self._switches

230
231
232
233
234
235
236
237
238
239
240
241
    @check_disabled
    def get_axis(self, name):
        return self._get_subitem(name)

    @check_disabled
    def get_encoder(self, name):
        return self._get_subitem(name)

    @check_disabled
    def get_shutter(self, name):
        return self._get_subitem(name)

242
    @check_disabled
243
    def get_switch(self, name):
244
        return self._get_subitem(name)
245

246
247
248
249
250
    def steps_position_precision(self, axis):
        """
        Return a float value representing the precision of the position in steps

        * 1e-6 is the default value: it means the motor can deal with floating point
Valentin Valls's avatar
Typo    
Valentin Valls committed
251
          steps up to 6 digits
252
        * 1 means the motor controller can only deal with an integer number of steps
Cyril Guilloud's avatar
Cyril Guilloud committed
253
        * soft axis controller return -inf to shunt the _is_already_on_position() check
254
255
256
        """
        return 1e-6

257
    def check_limits(self, *axis_dial_pos_groups):
258
        """
259
        Check limits for sequence of (axis, positions) tuples
260
        """
261
262
        for axis, dial_positions in axis_dial_pos_groups:
            self._do_check_limits(axis, dial_positions)
263

264
    def _do_check_limits(self, axis, dial_positions):
265
        try:
266
            min_pos = min(dial_positions)
267
        except TypeError:
268
            min_pos = dial_positions
269
        try:
270
            max_pos = max(dial_positions)
271
        except TypeError:
272
            max_pos = dial_positions
273

Matias Guijarro's avatar
Matias Guijarro committed
274
        ll, hl = axis.dial_limits
275
276
        if min_pos < ll:
            # get motion object, this will raise ValueError exception
Matias Guijarro's avatar
Matias Guijarro committed
277
            axis._get_motion(axis.dial2user(min_pos))
278
279
        elif max_pos > hl:
            # get motion object, this will raise ValueError exception
Matias Guijarro's avatar
Matias Guijarro committed
280
            axis._get_motion(axis.dial2user(max_pos))
281

Matias Guijarro's avatar
Matias Guijarro committed
282
283
    def initialize(self):
        pass
Matias Guijarro's avatar
Matias Guijarro committed
284

285
286
287
288
    def initialize_hardware(self):
        """
        This method should contain all commands needed to initialize the controller hardware.
        i.e: reset, power on....
289
        This initialization will be called once (by the first client).
290
291
292
        """
        pass

Matias Guijarro's avatar
Matias Guijarro committed
293
294
    def finalize(self):
        pass
295

296
297
298
299
300
    @check_disabled
    def encoder_initialized(self, encoder):
        return self.__initialized_encoder[encoder]

    @check_disabled
301
    def _initialize_encoder(self, encoder):
302
303
304
        with self.__lock:
            if self.__initialized_encoder[encoder]:
                return
305
            self.__initialized_encoder[encoder] = True
306
            self._initialize_hardware()
307

308
309
            try:
                self.initialize_encoder(encoder)
310
            except BaseException as enc_init_exc:
311
                self.__initialized_encoder[encoder] = False
312
313
314
                raise RuntimeError(
                    f"Cannot initialize {self.name} encoder"
                ) from enc_init_exc
315

316
317
318
319
    @check_disabled
    def axis_initialized(self, axis):
        return self.__initialized_axis[axis]

320
321
    def _initialize_hardware(self):
        # initialize controller hardware only once.
Cyril Guilloud's avatar
Cyril Guilloud committed
322

323
324
325
326
327
328
329
330
        if not self.__initialized_hw.value:
            try:
                self.initialize_hardware()
            except BaseException:
                self._disabled = True
                raise
            self.__initialized_hw.value = True

331
    @check_disabled
332
    def _initialize_axis(self, axis, *args, **kwargs):
333
        """
Cyril Guilloud's avatar
Cyril Guilloud committed
334
        Called by axis.lazy_init
335
        """
336
        with self.__lock:
337
338
339
            if self.__initialized_axis[axis]:
                return

340
            self._initialize_hardware()
341

Cyril Guilloud's avatar
Cyril Guilloud committed
342
343
            # Consider axis is initialized
            # => prevent re-entering  _initialize_axis()  in lazy_init
344
            self.__initialized_axis[axis] = True
345

346
            try:
347
348
349
350
351
352
353
354
                # Call specific axis initialization.
                self.initialize_axis(axis)

                # Call specific hardware axis initialization.
                # Done only once even in case of multi clients.
                axis_initialized = self.__initialized_hw_axis[axis]
                if not axis_initialized.value:
                    self.initialize_hardware_axis(axis)
355
356
                    axis.settings.check_config_settings()
                    axis.settings.init()  # get settings, from config or from cache, and apply to hardware
Lucas Felix's avatar
Lucas Felix committed
357
358
359
360
361
362
                    if axis.closed_loop:
                        if axis.closed_loop.state != self.get_closed_loop_state(axis):
                            self.activate_closed_loop(
                                axis, axis.closed_loop.state == ClosedLoopState.ON
                            )
                        axis.closed_loop._activate_setters()
363
364
                    axis_initialized.value = 1

365
            except BaseException:
366
                # Failed to initialize
367
368
369
                self.__initialized_axis[axis] = False
                raise

370
371
372
373
374
375
376
377
    def _add_axis(self, axis):
        """
        This method is called when a new axis is attached to
        this controller.
        This is called only once per axis.
        """
        pass

Matias Guijarro's avatar
Matias Guijarro committed
378
379
    def initialize_axis(self, axis):
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
380

381
382
    def initialize_hardware_axis(self, axis):
        """
383
384
        This method should contain all commands needed to initialize the
        hardware for this axis.
385
        i.e: power, closed loop configuration...
386
        This initialization will call only once (by the first client).
387
388
        """
        pass
389

390
391
392
    def finalize_axis(self, axis):
        raise NotImplementedError

Lucas Felix's avatar
Lucas Felix committed
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
    def get_closed_loop_requirements(self):
        """Return the list of keys this controller expects in a closed loop config"""
        raise NotImplementedError

    def get_closed_loop_state(self, axis):
        raise NotImplementedError

    def activate_closed_loop(self, axis, onoff=True):
        raise NotImplementedError

    def set_closed_loop_param(self, axis, param, value):
        raise NotImplementedError

    def get_closed_loop_param(self, axis, param):
        raise NotImplementedError

409
410
411
    def get_class_name(self):
        return self.__class__.__name__

Matias Guijarro's avatar
Matias Guijarro committed
412
413
414
    def initialize_encoder(self, encoder):
        raise NotImplementedError

415
416
417
418
419
420
421
    def has_trajectory(self):
        """
        should return True if trajectory is available
        on this controller.
        """
        return False

422
423
424
425
426
    def has_trajectory_event(self):
        return False

    def _prepare_trajectory(self, *trajectories):
        for traj in trajectories:
427
            if traj.has_events() and not self.has_trajectory_event():
428
429
430
                raise NotImplementedError(
                    "Controller does not support trajectories with events"
                )
431
432
433
434
435
        else:
            self.prepare_trajectory(*trajectories)
            if self.has_trajectory_event():
                self.set_trajectory_events(*trajectories)

436
437
    def prepare_trajectory(self, *trajectories):
        pass
438

439
440
441
    def prepare_all(self, *motion_list):
        raise NotImplementedError

cyril@lid269's avatar
cyril@lid269 committed
442
    def prepare_move(self, motion):
Matias Guijarro's avatar
Matias Guijarro committed
443
        return
Matias Guijarro's avatar
Matias Guijarro committed
444

Damien Naudet's avatar
Damien Naudet committed
445
    def start_jog(self, axis, velocity, direction):
446
447
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
448
449
    def start_one(self, motion):
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
450

Matias Guijarro's avatar
Matias Guijarro committed
451
452
    def start_all(self, *motion_list):
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
453

454
    def move_to_trajectory(self, *trajectories):
455
456
457
458
459
        """
        Should go move to the first point of the trajectory
        """
        raise NotImplementedError

460
    def start_trajectory(self, *trajectories):
461
462
463
464
        """
        Should move to the last point of the trajectory
        """
        raise NotImplementedError
465
466
467
468
469
470
471

    def set_trajectory_events(self, *trajectories):
        """
        Should set trigger event on trajectories.
        Each trajectory define .events_positions or events_pattern_positions.
        """
        raise NotImplementedError
472

Matias Guijarro's avatar
Matias Guijarro committed
473
474
    def stop(self, axis):
        raise NotImplementedError
475

476
477
    def stop_jog(self, axis):
        return self.stop(axis)
478

479
    def stop_all(self, *motions):
480
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
481

482
483
    def stop_trajectory(self, *trajectories):
        raise NotImplementedError
484

485
    def state(self, axis):
Matias Guijarro's avatar
Matias Guijarro committed
486
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
487

488
489
490
491
492
493
494
495
496
497
498
    def check_ready_to_move(self, axis, state):
        """
        method to check if the axis can move with the current state
        """
        if not state.READY and not state.MOVING:
            # read state from hardware
            state = axis.hw_state
            axis._update_settings(state=state)

        return state.READY

499
500
501
    def get_info(self, axis):
        raise NotImplementedError

502
503
504
    def get_id(self, axis):
        raise NotImplementedError

505
    def raw_write(self, com):
506
507
        raise NotImplementedError

508
    def raw_write_read(self, com):
509
510
        raise NotImplementedError

511
    def home_search(self, axis, switch):
Cyril Guilloud's avatar
Cyril Guilloud committed
512
513
        raise NotImplementedError

514
    def home_state(self, axis):
Cyril Guilloud's avatar
Cyril Guilloud committed
515
516
        raise NotImplementedError

517
    def limit_search(self, axis, limit):
518
519
        raise NotImplementedError

520
    def read_position(self, axis):
Matias Guijarro's avatar
Matias Guijarro committed
521
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
522

523
    def set_position(self, axis, new_position):
524
525
526
        """Set the position of <axis> in controller to <new_position>.
        This method is called by `position` property of <axis>.
        """
527
528
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
529
    def read_encoder(self, encoder):
Valentin Valls's avatar
Valentin Valls committed
530
        """Return the encoder value in *encoder steps*."""
Matias Guijarro's avatar
Matias Guijarro committed
531
532
        raise NotImplementedError

533
    def read_encoder_multiple(self, *encoder):
Valentin Valls's avatar
Valentin Valls committed
534
        """Return the encoder value in *encoder steps*."""
535
536
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
537
    def set_encoder(self, encoder, new_value):
Valentin Valls's avatar
Valentin Valls committed
538
        """Set encoder value. <new_value> is in encoder steps."""
Matias Guijarro's avatar
Matias Guijarro committed
539
        raise NotImplementedError
540

541
542
543
544
545
546
    def read_velocity(self, axis):
        raise NotImplementedError

    def set_velocity(self, axis, new_velocity):
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
547
    def set_on(self, axis):
blissadm@ID16NI's avatar
blissadm@ID16NI committed
548
549
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
550
    def set_off(self, axis):
blissadm@ID16NI's avatar
blissadm@ID16NI committed
551
552
        raise NotImplementedError

cyril@lid269's avatar
cyril@lid269 committed
553
554
555
556
557
558
    def read_acceleration(self, axis):
        raise NotImplementedError

    def set_acceleration(self, axis, new_acc):
        raise NotImplementedError

559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
    def set_event_positions(self, axis_or_encoder, positions):
        """
        This method is use to load into the controller
        a list of positions for event/trigger.
        The controller should generate an event
        (mainly electrical pulses) when the axis or
        the encoder pass through one of this position.
        """
        raise NotImplementedError

    def get_event_positions(self, axis_or_encoder):
        """
        @see set_event_position
        """
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
574

Perceval Guillou's avatar
Perceval Guillou committed
575
    def _is_already_on_position(self, axis, delta):
Valentin Valls's avatar
Valentin Valls committed
576
577
        """Return True if the difference between current position
        and new position (delta) is smaller than the positioning precision
Perceval Guillou's avatar
Perceval Guillou committed
578
579
580
581
582
        """
        if abs(delta) < (self.steps_position_precision(axis) / 2):
            return True  # Already in position
        return False

Matias Guijarro's avatar
Matias Guijarro committed
583

584
class CalcController(Controller):
Matias Guijarro's avatar
Matias Guijarro committed
585
    def __init__(self, *args, **kwargs):
Matias Guijarro's avatar
Matias Guijarro committed
586

587
        self._reals_group = None
Matias Guijarro's avatar
Matias Guijarro committed
588
        self.reals = []
589
        self.pseudos = []
590
        self._in_real_pos_update = False
591
        self._real_move_is_done = True
592

593
594
595
596
597
598
        super().__init__(*args, **kwargs)

        self.axis_settings.config_setting["velocity"] = False
        self.axis_settings.config_setting["acceleration"] = False
        self.axis_settings.config_setting["steps_per_unit"] = False

599
600
601
602
    def _get_subitem_default_class_name(self, cfg, parent_key):
        if parent_key == "axes":
            return "CalcAxis"

603
604
605
    def _init(self):
        # As any motors can be used into a calc
        # force for all axis creation
606

607
608
609
610
611
        for axis_name in self._axes_config.keys():
            self.get_axis(axis_name)

        super()._init()

612
    def initialize(self):
613
        for real_axis in self._tagged["real"]:
614
615
            # check if real axis is really from another controller
            if real_axis.controller == self:
616
                raise RuntimeError("Real axis '%s` doesn't exist" % real_axis.name)
Matias Guijarro's avatar
Matias Guijarro committed
617
            self.reals.append(real_axis)
618
619
            event.connect(real_axis, "internal_position", self._real_position_update)
            event.connect(real_axis, "internal__set_position", self._real_setpos_update)
620

621
        self._reals_group = motor_group.Group(*self.reals)
622
        event.connect(self._reals_group, "move_done", self._real_move_done)
623
        global_map.register(self, children_list=self.reals)
624

625
    def close(self):
626
        event.disconnect(self._reals_group, "move_done", self._real_move_done)
627
        for pseudo_axis in self.pseudos:
628
            event.disconnect(pseudo_axis, "sync_hard", self._pseudo_sync_hard)
629
630

        for real_axis in self.reals:
Matias Guijarro's avatar
Matias Guijarro committed
631
            event.disconnect(real_axis, "internal_position", self._real_position_update)
632
633
634
            event.disconnect(
                real_axis, "internal__set_position", self._real_setpos_update
            )
635
636
637
638
639

        self._reals_group = None
        self.reals = []
        self.pseudos = []

640
    def initialize_axis(self, axis):
641
642
643
644
645
        pass  # nothing to do

    def _add_axis(self, axis):
        self.pseudos.append(axis)
        event.connect(axis, "sync_hard", self._pseudo_sync_hard)
646
647
648

    def _pseudo_sync_hard(self):
        for real_axis in self.reals:
649
650
651
652
653
654
655
656
657
658
659
660
661
662
            event.disconnect(real_axis, "internal_position", self._real_position_update)
            event.disconnect(
                real_axis, "internal__set_position", self._real_setpos_update
            )
            try:
                real_axis.sync_hard()
            finally:
                event.connect(
                    real_axis, "internal_position", self._real_position_update
                )
                event.connect(
                    real_axis, "internal__set_position", self._real_setpos_update
                )
        self._calc_from_real()
Matias Guijarro's avatar
Matias Guijarro committed
663

664
    def _axis_tag(self, axis):
665
666
        return [
            tag
Vincent Michel's avatar
Vincent Michel committed
667
            for tag, axes in self._tagged.items()
668
669
            if tag != "real" and len(axes) == 1 and axis in axes
        ][0]
670

671
    def _get_set_positions(self):
672
673
        setpos_dict = dict()
        for axis in self.pseudos:
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
674
            setpos_dict[self._axis_tag(axis)] = axis.user2dial(axis._set_position)
675
        return setpos_dict
676

677
    def _real_position_update(self, pos, sender=None):
678
679
680
681
682
683
        # avoid recursion
        if self._in_real_pos_update:
            return

        try:
            self._in_real_pos_update = True
Matias Guijarro's avatar
Matias Guijarro committed
684

685
686
687
            for axis in self.pseudos:
                self._initialize_axis(axis)

688
689
690
            return self._calc_from_real()
        finally:
            self._in_real_pos_update = False
Matias Guijarro's avatar
Matias Guijarro committed
691

692
693
694
    def _real_setpos_update(self, _):
        real_setpos = dict()
        for axis in self.reals:
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
695
            real_setpos[self._axis_tag(axis)] = axis._set_position
696
697

        new_setpos = self.calc_from_real(real_setpos)
698

Vincent Michel's avatar
Vincent Michel committed
699
        for tagged_axis_name, setpos in new_setpos.items():
700
701
            axis = self._tagged[tagged_axis_name][0]
            axis.settings.set("_set_position", axis.dial2user(setpos))
702

703
    def check_limits(self, *axis_dial_pos_groups):
704
        axes = set()
705
        positions_len = []
706
707
708
709
710
        for axis, dial_pos in axis_dial_pos_groups:
            if not axis.controller is self:
                raise RuntimeError(
                    f"limits check: Axis {axis.name} is not managed by this controller"
                )
711
            axes.add(axis)
712
713
            if isinstance(dial_pos, collections.abc.Sequence):
                positions_len.append(len(dial_pos))
714
            else:
715
                positions_len.append(1)
716

717
        # number of positions must be equal
718
719
720
721
722
723
        try:
            assert min(positions_len) == max(positions_len)
        except AssertionError:
            raise RuntimeError(
                f"Axes {axes} doesn't have the same number of positions to check"
            )
724

725
726
        axis_to_positions = self._get_complementary_pseudos_pos_dict(axes)

727
728
729
730
731
732
733
734
        position_len = positions_len[0]
        if position_len > 1:
            # need to extend other
            axis_to_positions = {
                tag: numpy.ones(position_len) * pos
                for tag, pos in axis_to_positions.items()
            }
        axis_to_positions.update(
735
            {self._axis_tag(axis): dial_pos for axis, dial_pos in axis_dial_pos_groups}
736
737
738
739
740
741
742
743
744
745
746
747
748
        )
        real_positions = self.calc_to_real(axis_to_positions)
        real_min_max = dict()
        for rtag, pos in real_positions.items():
            try:
                iter(pos)
            except TypeError:
                real_min_max[self._tagged[rtag][0]] = [pos]
            else:
                real_min_max[self._tagged[rtag][0]] = min(pos), max(pos)
        for real_axis, positions in real_min_max.items():
            for pos in set(positions):
                try:
749
                    real_axis.controller.check_limits((real_axis, pos))
750
751
                except ValueError as e:
                    message = e.args[0]
752
                    new_message = f"{', '.join([axis.name for axis in axes])} move to {positions} error:\n{message}"
753
                    raise ValueError(new_message)
754

755
    def _get_complementary_pseudos_pos_dict(self, axes):
Valentin Valls's avatar
Valentin Valls committed
756
757
758
759
        """Find the other pseudos which are not in 'axes' and get their actual position.

        This complementary axes are necessary to compute the reals positions
        via the 'calc_to_real' method.
760

Valentin Valls's avatar
Valentin Valls committed
761
762
763
        Arguments:
            axes: list of Axis objects
        Return: {axis_tag:dial_pos, ...}
764
765
766
767
768
769
770
771
        """

        return {
            self._axis_tag(axis): axis.user2dial(axis._set_position)
            for axis in self.pseudos
            if axis not in axes
        }

772
    def _do_calc_from_real(self):
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
773
        real_positions_by_axis = self._reals_group.position
774
775
776
        real_positions = dict(
            [
                (self._axis_tag(axis), pos)
777
                for axis, pos in real_positions_by_axis.items()
778
779
            ]
        )
780
781
        return self.calc_from_real(real_positions)

782
    def _calc_from_real(self, *args):
783
        new_positions = self._do_calc_from_real()
Matias Guijarro's avatar
Matias Guijarro committed
784

Vincent Michel's avatar
Vincent Michel committed
785
        for tagged_axis_name, dial_pos in new_positions.items():
Matias Guijarro's avatar
Matias Guijarro committed
786
787
            axis = self._tagged[tagged_axis_name][0]
            if axis in self.pseudos:
788
                user_pos = axis.dial2user(dial_pos)
789
790
                axis.settings.set("dial_position", dial_pos)
                axis.settings.set("position", user_pos)
Matias Guijarro's avatar
Matias Guijarro committed
791
            else:
Matias Guijarro's avatar
Matias Guijarro committed
792
                raise RuntimeError("Cannot assign position to real motor")
793
        return new_positions
Matias Guijarro's avatar
Matias Guijarro committed
794
795
796
797
798

    def calc_from_real(self, real_positions):
        """Return a dict { pseudo motor tag: new position, ... }"""
        raise NotImplementedError

799
    def _real_move_done(self, done):
800
        self._real_move_is_done = done
801
        if done:
802
            self._calc_from_real()
803
804
805
806
807
808
809
            for axis in self.pseudos:
                if axis.encoder:
                    # check position and raise RuntimeError if encoder
                    # position doesn't correspond to axis position
                    # (MAXE_E)
                    axis._do_encoder_reading()

810
    def start_one(self, motion):
811
812
        self.start_all(motion)

Perceval Guillou's avatar
Perceval Guillou committed
813
    def _get_motion_pos_dict(self, motion_list):
Valentin Valls's avatar
Valentin Valls committed
814
        """Get all necessary pseudos with their positions to compute calc_to_real"""
Perceval Guillou's avatar
Perceval Guillou committed
815
816
        return self._get_set_positions()

817
    def start_all(self, *motion_list):
Perceval Guillou's avatar
Perceval Guillou committed
818
        positions_dict = self._get_motion_pos_dict(motion_list)
819
        move_dict = dict()
Vincent Michel's avatar
Vincent Michel committed
820
        for tag, target_pos in self.calc_to_real(positions_dict).items():
821
            real_axis = self._tagged[tag][0]
Matias Guijarro's avatar
Matias Guijarro committed
822
            move_dict[real_axis] = target_pos
823

824
825
        # force a global position update in case phys motors never move
        self._calc_from_real()
Matias Guijarro's avatar
Matias Guijarro committed
826
        self._reals_group.move(move_dict, wait=False)
Matias Guijarro's avatar
Matias Guijarro committed
827

828
    def calc_to_real(self, positions_dict):
Matias Guijarro's avatar
Matias Guijarro committed
829
830
831
        raise NotImplementedError

    def stop(self, axis):
832
        self._reals_group.stop()
Matias Guijarro's avatar
Matias Guijarro committed
833

834
    def read_position(self, axis):
Matias Guijarro's avatar
Matias Guijarro committed
835
836
        pos = axis.settings.get("dial_position")
        if pos is None:
837
838
839
            new_positions = self._calc_from_real()
            pos = new_positions[self._axis_tag(axis)]

Matias Guijarro's avatar
Matias Guijarro committed
840
        return pos
Matias Guijarro's avatar
Matias Guijarro committed
841
842

    def state(self, axis, new_state=None):
843
        return self._reals_group.state
844

845
    def set_position(self, axis, new_pos):
846
        if axis not in self.pseudos:
847
848
849
            raise RuntimeError(
                "Cannot set dial position on motor '%s` from CalcController" % axis.name
            )
850
851

        positions = self._get_set_positions()
852
853
        positions[self._axis_tag(axis)] = new_pos
        real_positions = self.calc_to_real(positions)
Vincent Michel's avatar
Vincent Michel committed
854
        for real_axis_tag, user_pos in real_positions.items():
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
855
            self._tagged[real_axis_tag][0].position = user_pos
856

857
        new_positions = self._calc_from_real()
858

859
        return new_positions[self._axis_tag(axis)]
Matias Guijarro's avatar
Matias Guijarro committed
860

861
    @object_method(types_info=(("float", "float", "int", "float"), "object"))
862
863
864
865
866
867
868
869
870
    def scan_on_trajectory(
        self,
        calc_axis,
        start_point,
        end_point,
        nb_points,
        time_per_point,
        interpolation_factor=1,
    ):
871
872
873
        """
        helper to create a trajectories handler for a scan.

874
875
876
877
878
879
        It will check the **trajectory_minimum_resolution** and
        **trajectory_maximum_resolution** axis property.
        If the trajectory resolution asked is lower than the trajectory_minimum_resolution,
        the trajectory will be over sampled.
        And if the trajectory resolution asked is higher than the trajectory_maximum_resolution
        the trajectory will be down sampled.
880
881
882
883
884
885
        Args:
            start -- first point of the trajectory
            end -- the last point of the trajectory
            nb_points -- the number of point created for this trajectory
            time_per_point -- the time between each points.
        """
886
        # check if real motor has trajectory capability
887
        real_axes = list()
888
        real_involved = self.calc_to_real(
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
889
            {self._axis_tag(caxis): caxis.position for caxis in self.pseudos}
890
        )
891
        for real in self.reals:
892
893
894
            if self._axis_tag(real) in real_involved:
                axis, raxes = self._check_trajectory(real)
                real_axes.append((axis, raxes))
895

896
897
898
899
900
901
        trajectory_minimum_resolution = calc_axis.config.get(
            "trajectory_minimum_resolution", floatOrNone, None
        )
        trajectory_maximum_resolution = calc_axis.config.get(
            "trajectory_maximum_resolution", floatOrNone, None
        )
902

903
        # Check if the resolution is enough
904
905
906
907
        total_distance = abs(end_point - start_point)
        trajectory_resolution = total_distance / float(nb_points)
        used_resolution = None

908
909
910
911
912
913
914
915
916
        if (
            trajectory_minimum_resolution is not None
            and trajectory_maximum_resolution is not None
        ):
            if not (
                trajectory_maximum_resolution
                >= trajectory_resolution
                >= trajectory_minimum_resolution
            ):
917
918
919
920
                if trajectory_resolution > trajectory_minimum_resolution:
                    used_resolution = trajectory_minimum_resolution
                else:
                    used_resolution = trajectory_maximum_resolution
921
922
923
924
925
926
        elif trajectory_minimum_resolution is not None:
            if trajectory_resolution > trajectory_minimum_resolution:
                used_resolution = trajectory_minimum_resolution
        elif trajectory_maximum_resolution is not None:
            if trajectory_resolution < trajectory_maximum_resolution:
                used_resolution = trajectory_maximum_resolution
927
928
929
930
931
932
933

        if used_resolution is not None:
            new_nb_points = int(round(total_distance / used_resolution))
            new_time_point = float(time_per_point * nb_points) / new_nb_points
            nb_points = new_nb_points
            time_per_point = new_time_point

934
        calc_positions = numpy.linspace(start_point, end_point, nb_points)
935
936
        positions = {self._axis_tag(calc_axis): calc_positions}
        # other virtual axis stays at the same position
937
938
939
        for caxis in self.pseudos:
            if caxis is calc_axis:
                continue
940
            cpos = numpy.zeros(len(calc_positions), dtype=float)
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
941
            cpos[:] = caxis.position
942
943
            positions[self._axis_tag(caxis)] = cpos

Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
944
        time = numpy.linspace(0.0, nb_points * time_per_point, nb_points)
945
946
        real_positions = self.calc_to_real(positions)
        final_real_axes_position = dict()
947
        self._get_real_position(real_axes, real_positions, final_real_axes_position)
948
949

        pt = trajectory.PointTrajectory()
950
951
952
953
954
955
956
        spline_nb_points = (
            0 if interpolation_factor == 1 else len(time) * interpolation_factor
        )
        pt.build(
            time,
            {
                axis.name: position
Vincent Michel's avatar
Vincent Michel committed
957
                for axis, position in iter(final_real_axes_position.items())
958
959
960
961
            },
            spline_nb_points=spline_nb_points,
        )
        # check velocity and acceleration
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
962
963
964
        max_velocity = pt.max_velocity
        max_acceleration = pt.max_acceleration
        limits = pt.limits
965
966
967
        error_list = list()
        start_stop_acceleration = dict()
        for axis in final_real_axes_position:
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
968
969
970
            vel = axis.velocity
            acc = axis.acceleration
            axis_limits = axis.limits
971
972
973
974
            traj_vel = max_velocity[axis.name]
            traj_acc = max_acceleration[axis.name]
            traj_limits = limits[axis.name]
            if traj_acc > acc:
975
976
977
978
                error_list.append(
                    "Axis %s reach %f acceleration on this trajectory,"
                    "max acceleration is %f" % (axis.name, traj_acc, acc)
                )
979
            if traj_vel > vel:
980
981
982
983
                error_list.append(
                    "Axis %s reach %f velocity on this trajectory,"
                    "max velocity is %f" % (axis.name, traj_vel, vel)
                )
984
985
            for lm in traj_limits:
                if not axis_limits[0] <= lm <= axis_limits[1]:
986
987
988
989
                    error_list.append(
                        "Axis %s go beyond limits (%f <= %f <= %f)"
                        % (axis.name<