motor.py 33.8 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
# Distributed under the GNU LGPLv3. See LICENSE for more info.
7

8
from os import name
9
import numpy
10
11
12
import functools
from gevent import lock

13
14
# absolute import to avoid circular import
import bliss.common.motor_group as motor_group
15
from bliss.common.motor_config import MotorConfig
16
from bliss.common.motor_settings import ControllerAxisSettings, floatOrNone
17
from bliss.common.axis import Trajectory
18
from bliss.common import event
19
from bliss.controllers.counter import SamplingCounterController
20
from bliss.physics import trajectory
21
from bliss.common.utils import set_custom_members, object_method, grouped
22
from bliss import global_map
23
from bliss.config.channels import Cache
Matias Guijarro's avatar
Matias Guijarro committed
24

25
26
from bliss.controllers.bliss_controller import BlissController

27

28
29
30
31
32
33
class EncoderCounterController(SamplingCounterController):
    def __init__(self, motor_controller):
        super().__init__("encoder")

        self.motor_controller = motor_controller

34
35
36
        # High frequency acquisition loop
        self.max_sampling_frequency = None

37
    def read_all(self, *encoders):
38
        steps_per_unit = numpy.array([enc.steps_per_unit for enc in encoders])
39
40
41
42
43
44
45
46
47
48
49
        try:
            positions_array = numpy.array(
                self.motor_controller.read_encoder_multiple(*encoders)
            )
        except NotImplementedError:
            positions_array = numpy.array(
                list(map(self.motor_controller.read_encoder, encoders))
            )
        return positions_array / steps_per_unit


50
51
52
53
54
55
56
57
58
59
def check_disabled(func):
    @functools.wraps(func)
    def func_wrapper(self, *args, **kwargs):
        if self._disabled:
            raise RuntimeError(f"Controller is disabled. Check hardware and restart.")
        return func(self, *args, **kwargs)

    return func_wrapper


60
class Controller(BlissController):
61
    """
62
    Motor controller base class
63
    """
64

65
66
67
68
69
70
71
72
    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]

73
74
75
        super().__init__(config)

        self.__motor_config = MotorConfig(config)
76
        self.__initialized_hw = Cache(self, "initialized", default_value=False)
77
        self.__initialized_hw_axis = dict()
78
79
        self.__initialized_encoder = dict()
        self.__initialized_axis = dict()
80
        self.__lock = lock.RLock()
81
        self._encoder_counter_controller = EncoderCounterController(self)
Matias Guijarro's avatar
Matias Guijarro committed
82
        self._axes = dict()
Matias Guijarro's avatar
Matias Guijarro committed
83
        self._encoders = dict()
84
85
        self._shutters = dict()
        self._switches = dict()
Matias Guijarro's avatar
Matias Guijarro committed
86
        self._tagged = dict()
87
        self._disabled = False
Matias Guijarro's avatar
Matias Guijarro committed
88

89
        self.axis_settings = ControllerAxisSettings()
90
        global_map.register(self, parents_list=["controllers"])
91

92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
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
    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
136
137
138
    def _create_subitem_from_config(
        self, name, cfg, parent_key, item_class, item_obj=None
    ):
139
140

        if parent_key == "axes":
141
142
            if item_class is None:  # it is a reference
                axis = item_obj
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
            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
174

175
    def _init(self):
176
177
        try:
            self.initialize()
Perceval Guillou's avatar
Perceval Guillou committed
178
            self._disabled = False
179
180
181
        except BaseException:
            self._disabled = True
            raise
182

183
184
185
186
    @property
    def config(self):
        return self.__motor_config

Matias Guijarro's avatar
Matias Guijarro committed
187
188
189
190
    @property
    def axes(self):
        return self._axes

191
192
193
194
    @property
    def encoders(self):
        return self._encoders

195
196
197
198
199
    @property
    def shutters(self):
        return self._shutters

    @property
Matias Guijarro's avatar
Matias Guijarro committed
200
    def switches(self):
201
202
        return self._switches

203
204
205
206
207
208
209
210
211
212
213
214
    @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)

215
    @check_disabled
216
    def get_switch(self, name):
217
        return self._get_subitem(name)
218

219
220
221
222
223
    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
224
          steps up to 6 digits
225
226
227
228
        * 1 means the motor controller can only deal with an integer number of steps
        """
        return 1e-6

229
230
231
232
    def check_limits(self, *axis_positions):
        """
        check limits for list of axis and positions
        """
233
234
235
236
237
238
239
240
        if len(axis_positions) == 1:
            # assuming axis_positions is just a grouper object with axis, positions
            for axis, positions in axis_positions[0]:
                self._check_limits(axis, positions)
        else:
            # backward compatibility
            for axis, positions in grouped(axis_positions, 2):
                self._check_limits(axis, positions)
241

242
    def _check_limits(self, axis, user_positions):
243
244
245
246
247
248
249
250
251
        try:
            min_pos = min(user_positions)
        except TypeError:
            min_pos = user_positions
        try:
            max_pos = max(user_positions)
        except TypeError:
            max_pos = user_positions

Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
252
        ll, hl = axis.limits
253
254
255
256
257
258
259
        if min_pos < ll:
            # get motion object, this will raise ValueError exception
            axis._get_motion(min_pos)
        elif max_pos > hl:
            # get motion object, this will raise ValueError exception
            axis._get_motion(max_pos)

Matias Guijarro's avatar
Matias Guijarro committed
260
261
    def initialize(self):
        pass
Matias Guijarro's avatar
Matias Guijarro committed
262

263
264
265
266
    def initialize_hardware(self):
        """
        This method should contain all commands needed to initialize the controller hardware.
        i.e: reset, power on....
267
        This initialization will be called once (by the first client).
268
269
270
        """
        pass

Matias Guijarro's avatar
Matias Guijarro committed
271
272
    def finalize(self):
        pass
273

274
275
276
277
278
    @check_disabled
    def encoder_initialized(self, encoder):
        return self.__initialized_encoder[encoder]

    @check_disabled
279
    def _initialize_encoder(self, encoder):
280
281
282
        with self.__lock:
            if self.__initialized_encoder[encoder]:
                return
283
            self.__initialized_encoder[encoder] = True
284
285
286
287
288
289
            self._initialize_hardware()
            try:
                self.initialize_encoder(encoder)
            except BaseException:
                self.__initialized_encoder[encoder] = False
                raise
290

291
292
293
294
    @check_disabled
    def axis_initialized(self, axis):
        return self.__initialized_axis[axis]

295
296
    def _initialize_hardware(self):
        # initialize controller hardware only once.
Cyril Guilloud's avatar
Cyril Guilloud committed
297

298
299
300
301
302
303
304
305
        if not self.__initialized_hw.value:
            try:
                self.initialize_hardware()
            except BaseException:
                self._disabled = True
                raise
            self.__initialized_hw.value = True

306
    @check_disabled
307
    def _initialize_axis(self, axis, *args, **kwargs):
308
        """
Cyril Guilloud's avatar
Cyril Guilloud committed
309
        Called by axis.lazy_init
310
        """
311
        with self.__lock:
312
313
314
            if self.__initialized_axis[axis]:
                return

315
            self._initialize_hardware()
316

Cyril Guilloud's avatar
Cyril Guilloud committed
317
318
            # Consider axis is initialized
            # => prevent re-entering  _initialize_axis()  in lazy_init
319
            self.__initialized_axis[axis] = True
320

321
            try:
322
323
324
325
326
327
328
329
                # 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)
330
331
                    axis.settings.check_config_settings()
                    axis.settings.init()  # get settings, from config or from cache, and apply to hardware
332
333
                    axis_initialized.value = 1

334
            except BaseException:
335
                # Failed to initialize
336
337
338
                self.__initialized_axis[axis] = False
                raise

339
340
341
342
343
344
345
346
    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
347
348
    def initialize_axis(self, axis):
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
349

350
351
    def initialize_hardware_axis(self, axis):
        """
352
353
        This method should contain all commands needed to initialize the
        hardware for this axis.
354
        i.e: power, closed loop configuration...
355
        This initialization will call only once (by the first client).
356
357
        """
        pass
358

359
360
361
    def finalize_axis(self, axis):
        raise NotImplementedError

362
363
364
    def get_class_name(self):
        return self.__class__.__name__

Matias Guijarro's avatar
Matias Guijarro committed
365
366
367
    def initialize_encoder(self, encoder):
        raise NotImplementedError

368
369
370
371
372
373
374
    def has_trajectory(self):
        """
        should return True if trajectory is available
        on this controller.
        """
        return False

375
376
377
378
379
    def has_trajectory_event(self):
        return False

    def _prepare_trajectory(self, *trajectories):
        for traj in trajectories:
380
            if traj.has_events() and not self.has_trajectory_event():
381
382
383
                raise NotImplementedError(
                    "Controller does not support trajectories with events"
                )
384
385
386
387
388
        else:
            self.prepare_trajectory(*trajectories)
            if self.has_trajectory_event():
                self.set_trajectory_events(*trajectories)

389
390
    def prepare_trajectory(self, *trajectories):
        pass
391

392
393
394
    def prepare_all(self, *motion_list):
        raise NotImplementedError

cyril@lid269's avatar
cyril@lid269 committed
395
    def prepare_move(self, motion):
Matias Guijarro's avatar
Matias Guijarro committed
396
        return
Matias Guijarro's avatar
Matias Guijarro committed
397

Damien Naudet's avatar
Damien Naudet committed
398
    def start_jog(self, axis, velocity, direction):
399
400
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
401
402
    def start_one(self, motion):
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
403

Matias Guijarro's avatar
Matias Guijarro committed
404
405
    def start_all(self, *motion_list):
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
406

407
    def move_to_trajectory(self, *trajectories):
408
409
410
411
412
        """
        Should go move to the first point of the trajectory
        """
        raise NotImplementedError

413
    def start_trajectory(self, *trajectories):
414
415
416
417
        """
        Should move to the last point of the trajectory
        """
        raise NotImplementedError
418
419
420
421
422
423
424

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

Matias Guijarro's avatar
Matias Guijarro committed
426
427
    def stop(self, axis):
        raise NotImplementedError
428

429
430
    def stop_jog(self, axis):
        return self.stop(axis)
431

432
    def stop_all(self, *motions):
433
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
434

435
436
    def stop_trajectory(self, *trajectories):
        raise NotImplementedError
437

438
    def state(self, axis):
Matias Guijarro's avatar
Matias Guijarro committed
439
        raise NotImplementedError
Matias Guijarro's avatar
Matias Guijarro committed
440

441
442
443
444
445
446
447
448
449
450
451
    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

452
453
454
    def get_info(self, axis):
        raise NotImplementedError

455
456
457
    def get_id(self, axis):
        raise NotImplementedError

458
    def raw_write(self, com):
459
460
        raise NotImplementedError

461
    def raw_write_read(self, com):
462
463
        raise NotImplementedError

464
    def home_search(self, axis, switch):
Cyril Guilloud's avatar
Cyril Guilloud committed
465
466
        raise NotImplementedError

467
    def home_state(self, axis):
Cyril Guilloud's avatar
Cyril Guilloud committed
468
469
        raise NotImplementedError

470
    def limit_search(self, axis, limit):
471
472
        raise NotImplementedError

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

476
    def set_position(self, axis, new_position):
477
478
479
        """Set the position of <axis> in controller to <new_position>.
        This method is called by `position` property of <axis>.
        """
480
481
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
482
    def read_encoder(self, encoder):
483
        """Return the encoder value in *encoder steps*.
Cyril Guilloud's avatar
Cyril Guilloud committed
484
        """
Matias Guijarro's avatar
Matias Guijarro committed
485
486
        raise NotImplementedError

487
488
489
490
491
    def read_encoder_multiple(self, *encoder):
        """Return the encoder value in *encoder steps*.
        """
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
492
    def set_encoder(self, encoder, new_value):
493
        """Set encoder value. <new_value> is in encoder steps.
Cyril Guilloud's avatar
Cyril Guilloud committed
494
        """
Matias Guijarro's avatar
Matias Guijarro committed
495
        raise NotImplementedError
496

497
498
499
500
501
502
    def read_velocity(self, axis):
        raise NotImplementedError

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

Matias Guijarro's avatar
Matias Guijarro committed
503
    def set_on(self, axis):
blissadm@ID16NI's avatar
blissadm@ID16NI committed
504
505
        raise NotImplementedError

Matias Guijarro's avatar
Matias Guijarro committed
506
    def set_off(self, axis):
blissadm@ID16NI's avatar
blissadm@ID16NI committed
507
508
        raise NotImplementedError

cyril@lid269's avatar
cyril@lid269 committed
509
510
511
512
513
514
    def read_acceleration(self, axis):
        raise NotImplementedError

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

515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
    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
530

Perceval Guillou's avatar
Perceval Guillou committed
531
532
533
534
535
536
537
538
    def _is_already_on_position(self, axis, delta):
        """ return True if the difference between current position
            and new position (delta) is smaller than the positioning precision 
        """
        if abs(delta) < (self.steps_position_precision(axis) / 2):
            return True  # Already in position
        return False

Matias Guijarro's avatar
Matias Guijarro committed
539

540
class CalcController(Controller):
Matias Guijarro's avatar
Matias Guijarro committed
541
    def __init__(self, *args, **kwargs):
Matias Guijarro's avatar
Matias Guijarro committed
542

543
        self._reals_group = None
Matias Guijarro's avatar
Matias Guijarro committed
544
        self.reals = []
545
        self.pseudos = []
546
547
        self._lock = lock.RLock()
        self._in_real_pos_update = False
548

549
550
551
552
553
554
555
556
557
        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

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

559
560
561
562
563
        for axis_name in self._axes_config.keys():
            self.get_axis(axis_name)

        super()._init()

564
    def initialize(self):
565
        for real_axis in self._tagged["real"]:
566
567
            # check if real axis is really from another controller
            if real_axis.controller == self:
568
                raise RuntimeError("Real axis '%s` doesn't exist" % real_axis.name)
Matias Guijarro's avatar
Matias Guijarro committed
569
            self.reals.append(real_axis)
570
571
            event.connect(real_axis, "internal_position", self._real_position_update)
            event.connect(real_axis, "internal__set_position", self._real_setpos_update)
572

573
        self._reals_group = motor_group.Group(*self.reals)
574
        event.connect(self._reals_group, "move_done", self._real_move_done)
575
        global_map.register(self, children_list=self.reals)
576

577
    def close(self):
578
        event.disconnect(self._reals_group, "move_done", self._real_move_done)
579
        for pseudo_axis in self.pseudos:
580
            event.disconnect(pseudo_axis, "sync_hard", self._pseudo_sync_hard)
581
582

        for real_axis in self.reals:
Matias Guijarro's avatar
Matias Guijarro committed
583
            event.disconnect(real_axis, "internal_position", self._real_position_update)
584
585
586
            event.disconnect(
                real_axis, "internal__set_position", self._real_setpos_update
            )
587
588
589
590
591

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

592
    def initialize_axis(self, axis):
593
594
595
596
597
        pass  # nothing to do

    def _add_axis(self, axis):
        self.pseudos.append(axis)
        event.connect(axis, "sync_hard", self._pseudo_sync_hard)
598
599
600
601

    def _pseudo_sync_hard(self):
        for real_axis in self.reals:
            real_axis.sync_hard()
Matias Guijarro's avatar
Matias Guijarro committed
602

603
    def _axis_tag(self, axis):
604
605
        return [
            tag
Vincent Michel's avatar
Vincent Michel committed
606
            for tag, axes in self._tagged.items()
607
608
            if tag != "real" and len(axes) == 1 and axis in axes
        ][0]
609

610
    def _get_set_positions(self):
611
612
        setpos_dict = dict()
        for axis in self.pseudos:
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
613
            setpos_dict[self._axis_tag(axis)] = axis.user2dial(axis._set_position)
614
        return setpos_dict
615

616
    def _real_position_update(self, pos, sender=None):
617
618
619
620
        with self._lock:
            # avoid recursion
            if self._in_real_pos_update:
                return
Matias Guijarro's avatar
Matias Guijarro committed
621

622
623
624
625
626
627
628
629
            for axis in self.pseudos:
                self._initialize_axis(axis)

            try:
                self._in_real_pos_update = True
                return self._calc_from_real()
            finally:
                self._in_real_pos_update = False
Matias Guijarro's avatar
Matias Guijarro committed
630

631
632
633
    def _real_setpos_update(self, _):
        real_setpos = dict()
        for axis in self.reals:
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
634
            real_setpos[self._axis_tag(axis)] = axis._set_position
635
636

        new_setpos = self.calc_from_real(real_setpos)
637

Vincent Michel's avatar
Vincent Michel committed
638
        for tagged_axis_name, setpos in new_setpos.items():
639
640
            axis = self._tagged[tagged_axis_name][0]
            axis.settings.set("_set_position", axis.dial2user(setpos))
641

642
    def _check_limits(self, axis, positions):
643
        self.check_limits(axis, positions)
644

645
    def check_limits(self, *axis_positions):
646
647
648
649
650
651
652
        if len(axis_positions) == 1:
            # assuming axis_positions is just a grouper object with axis, positions
            grouped_axis_positions = list(axis_positions[0])
        else:
            # backward compatibility
            grouped_axis_positions = list(grouped(axis_positions, 2))
        axes = set()
653
        positions_len = []
654
655
        for axis, pos in grouped_axis_positions:
            axes.add(axis)
656
657
658
659
            try:
                iter(pos)
            except TypeError:
                positions_len.append(1)
660
            else:
661
662
663
664
665
666
                positions_len.append(len(pos))
        try:
            left_axis = axes - set(self.pseudos)
            assert not left_axis
        except AssertionError:
            raise RuntimeError(f"Axes {left_axis} are not managed in this controller")
667

668
669
670
671
672
673
674
        # number of positions must be equals
        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"
            )
675

676
677
        axis_to_positions = self._get_complementary_pseudos_pos_dict(axes)

678
679
680
681
682
683
684
685
        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(
686
            {self._axis_tag(axis): pos for axis, pos in grouped_axis_positions}
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
        )
        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:
                    real_axis.controller._check_limits(real_axis, pos)
                except ValueError as e:
                    message = e.args[0]
703
                    new_message = f"{', '.join([axis.name for axis in axes])} move to {positions} error:\n{message}"
704
                    raise ValueError(new_message)
705

706
707
708
709
710
    def _get_complementary_pseudos_pos_dict(self, axes):
        """ 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. 

Perceval Guillou's avatar
Perceval Guillou committed
711
712
            Args: 
                axes: list of Axis objects
713
714
715
716
717
718
719
720
721
            Return: {axis_tag:dial_pos, ...}
        """

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

722
    def _do_calc_from_real(self):
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
723
        real_positions_by_axis = self._reals_group.position
724
725
726
        real_positions = dict(
            [
                (self._axis_tag(axis), pos)
727
                for axis, pos in real_positions_by_axis.items()
728
729
            ]
        )
730
731
        return self.calc_from_real(real_positions)

732
    def _calc_from_real(self, *args):
733
        new_positions = self._do_calc_from_real()
Matias Guijarro's avatar
Matias Guijarro committed
734

Vincent Michel's avatar
Vincent Michel committed
735
        for tagged_axis_name, dial_pos in new_positions.items():
Matias Guijarro's avatar
Matias Guijarro committed
736
737
            axis = self._tagged[tagged_axis_name][0]
            if axis in self.pseudos:
738
                user_pos = axis.dial2user(dial_pos)
739
740
                axis.settings.set("dial_position", dial_pos)
                axis.settings.set("position", user_pos)
Matias Guijarro's avatar
Matias Guijarro committed
741
            else:
Matias Guijarro's avatar
Matias Guijarro committed
742
                raise RuntimeError("Cannot assign position to real motor")
743
        return new_positions
Matias Guijarro's avatar
Matias Guijarro committed
744
745
746
747
748

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

749
750
751
752
753
754
755
756
757
    def _real_move_done(self, done):
        if done:
            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()

758
    def start_one(self, motion):
759
760
        self.start_all(motion)

Perceval Guillou's avatar
Perceval Guillou committed
761
762
763
764
    def _get_motion_pos_dict(self, motion_list):
        """ get all necessary pseudos with their positions to compute calc_to_real"""
        return self._get_set_positions()

765
    def start_all(self, *motion_list):
Perceval Guillou's avatar
Perceval Guillou committed
766
        positions_dict = self._get_motion_pos_dict(motion_list)
767
        move_dict = dict()
Vincent Michel's avatar
Vincent Michel committed
768
        for tag, target_pos in self.calc_to_real(positions_dict).items():
769
            real_axis = self._tagged[tag][0]
Matias Guijarro's avatar
Matias Guijarro committed
770
            move_dict[real_axis] = target_pos
771

772
773
        # force a global position update in case phys motors never move
        self._calc_from_real()
Matias Guijarro's avatar
Matias Guijarro committed
774
        self._reals_group.move(move_dict, wait=False)
Matias Guijarro's avatar
Matias Guijarro committed
775

776
    def calc_to_real(self, positions_dict):
Matias Guijarro's avatar
Matias Guijarro committed
777
778
779
        raise NotImplementedError

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

782
    def read_position(self, axis):
Matias Guijarro's avatar
Matias Guijarro committed
783
784
        pos = axis.settings.get("dial_position")
        if pos is None:
785
786
787
            new_positions = self._calc_from_real()
            pos = new_positions[self._axis_tag(axis)]

Matias Guijarro's avatar
Matias Guijarro committed
788
        return pos
Matias Guijarro's avatar
Matias Guijarro committed
789
790

    def state(self, axis, new_state=None):
791
        return self._reals_group.state
792

793
    def set_position(self, axis, new_pos):
794
        if axis not in self.pseudos:
795
796
797
            raise RuntimeError(
                "Cannot set dial position on motor '%s` from CalcController" % axis.name
            )
798
799

        positions = self._get_set_positions()
800
801
        positions[self._axis_tag(axis)] = new_pos
        real_positions = self.calc_to_real(positions)
Vincent Michel's avatar
Vincent Michel committed
802
        for real_axis_tag, user_pos in real_positions.items():
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
803
            self._tagged[real_axis_tag][0].position = user_pos
804

805
        new_positions = self._calc_from_real()
806

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

809
    @object_method(types_info=(("float", "float", "int", "float"), "object"))
810
811
812
813
814
815
816
817
818
    def scan_on_trajectory(
        self,
        calc_axis,
        start_point,
        end_point,
        nb_points,
        time_per_point,
        interpolation_factor=1,
    ):
819
820
821
        """
        helper to create a trajectories handler for a scan.

822
823
824
825
826
827
        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.
828
829
830
831
832
833
        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.
        """
834
        # check if real motor has trajectory capability
835
        real_axes = list()
836
        real_involved = self.calc_to_real(
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
837
            {self._axis_tag(caxis): caxis.position for caxis in self.pseudos}
838
        )
839
        for real in self.reals:
840
841
842
            if self._axis_tag(real) in real_involved:
                axis, raxes = self._check_trajectory(real)
                real_axes.append((axis, raxes))
843

844
845
846
847
848
849
        trajectory_minimum_resolution = calc_axis.config.get(
            "trajectory_minimum_resolution", floatOrNone, None
        )
        trajectory_maximum_resolution = calc_axis.config.get(
            "trajectory_maximum_resolution", floatOrNone, None
        )
850

851
        # Check if the resolution is enough
852
853
854
855
        total_distance = abs(end_point - start_point)
        trajectory_resolution = total_distance / float(nb_points)
        used_resolution = None

856
857
858
859
860
861
862
863
864
        if (
            trajectory_minimum_resolution is not None
            and trajectory_maximum_resolution is not None
        ):
            if not (
                trajectory_maximum_resolution
                >= trajectory_resolution
                >= trajectory_minimum_resolution
            ):
865
866
867
868
                if trajectory_resolution > trajectory_minimum_resolution:
                    used_resolution = trajectory_minimum_resolution
                else:
                    used_resolution = trajectory_maximum_resolution
869
870
871
872
873
874
        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
875
876
877
878
879
880
881

        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

882
        calc_positions = numpy.linspace(start_point, end_point, nb_points)
883
884
        positions = {self._axis_tag(calc_axis): calc_positions}
        # other virtual axis stays at the same position
885
886
887
        for caxis in self.pseudos:
            if caxis is calc_axis:
                continue
888
            cpos = numpy.zeros(len(calc_positions), dtype=float)
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
889
            cpos[:] = caxis.position
890
891
            positions[self._axis_tag(caxis)] = cpos

Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
892
        time = numpy.linspace(0.0, nb_points * time_per_point, nb_points)
893
894
        real_positions = self.calc_to_real(positions)
        final_real_axes_position = dict()
895
        self._get_real_position(real_axes, real_positions, final_real_axes_position)
896
897

        pt = trajectory.PointTrajectory()
898
899
900
901
902
903
904
        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
905
                for axis, position in iter(final_real_axes_position.items())
906
907
908
909
            },
            spline_nb_points=spline_nb_points,
        )
        # check velocity and acceleration
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
910
911
912
        max_velocity = pt.max_velocity
        max_acceleration = pt.max_acceleration
        limits = pt.limits
913
914
915
        error_list = list()
        start_stop_acceleration = dict()
        for axis in final_real_axes_position:
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
916
917
918
            vel = axis.velocity
            acc = axis.acceleration
            axis_limits = axis.limits
919
920
921
922
            traj_vel = max_velocity[axis.name]
            traj_acc = max_acceleration[axis.name]
            traj_limits = limits[axis.name]
            if traj_acc > acc:
923
924
925
926
                error_list.append(
                    "Axis %s reach %f acceleration on this trajectory,"
                    "max acceleration is %f" % (axis.name, traj_acc, acc)
                )
927
            if traj_vel > vel:
928
929
930
931
                error_list.append(
                    "Axis %s reach %f velocity on this trajectory,"
                    "max velocity is %f" % (axis.name, traj_vel, vel)
                )
932
933
            for lm in traj_limits:
                if not axis_limits[0] <= lm <= axis_limits[1]:
934
935
936
937
                    error_list.append(
                        "Axis %s go beyond limits (%f <= %f <= %f)"
                        % (axis.name, axis_limits[0], traj_limits[0], axis_limits[1])
                    )
938
939
940
941

            start_stop_acceleration[axis.name] = acc

        if error_list:
942
            error_message = (
943
                "Trajectory on calc axis **%s** cannot be done.\n" % calc_axis.name
944
945
            )
            error_message += "\n".join(error_list)
946
947
948
            raise ValueError(error_message)

        pvt = pt.pvt(acceleration_start_end=start_stop_acceleration)
949
950
951
        trajectories = [
            Trajectory(axis, pvt[axis.name]) for axis in final_real_axes_position
        ]
952

953
        return motor_group.TrajectoryGroup(*trajectories, calc_axis=calc_axis)
954
955
956
957

    def _check_trajectory(self, axis):
        if axis.controller.has_trajectory():
            return axis, []
958
        else:  # check if axis is part of calccontroller
959
960
            ctrl = axis.controller
            if isinstance(ctrl, CalcController):
961
                real_involved = ctrl.calc_to_real(
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
962
                    {ctrl._axis_tag(caxis): caxis.position for caxis in ctrl.pseudos}
963
                )
964
965
                real_axes = list()
                for real in ctrl.reals:
966
967
968
                    if ctrl._axis_tag(real) in real_involved:
                        raxis, axes = self._check_trajectory(real)
                        real_axes.append((raxis, axes))
969
970
                return axis, real_axes
            else:
971
972
973
974
                raise ValueError(
                    "Controller for axis %s does not support "
                    "trajectories" % axis.name
                )
975

976
    def _get_real_position(self, real_axes, real_positions, final_real_axes_position):
977
978
979
980
981
        local_real_positions = dict()
        for axis, dep_real_axes in real_axes:
            axis_position = real_positions.get(self._axis_tag(axis))
            if not dep_real_axes:
                if axis_position is None:
982
983
984
                    raise RuntimeError(
                        "Could not get position " "for axis %s" % axis.name
                    )
985
986
987
988
                else:
                    final_real_axes_position[axis] = axis_position
            else:
                ctrl = axis.controller
989
                local_real_positions = {ctrl._axis_tag(axis): axis_position}
990
991
                for caxis in ctrl.pseudos:
                    axis_tag = ctrl._axis_tag(caxis)
992
                    if caxis is axis or axis_tag in local_real_positions:
993
                        continue
994
                    cpos = numpy.zeros(len(axis_position), dtype=float)
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
995
                    cpos[:] = caxis.position
996
997
998
                    local_real_positions[ctrl._axis_tag(caxis)] = cpos

                dep_real_position = ctrl.calc_to_real(local_real_positions)
999
1000
                ctrl._get_real_position(
                    dep_real_axes, dep_real_position, final_real_axes_position