shexapod.py 11.6 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
8
9
10
11
12
13
14
15
16
# Distributed under the GNU LGPLv3. See LICENSE for more info.

"""\
Symetrie hexapod

YAML_ configuration example:

.. code-block:: yaml

    plugin: emotion
    class: SHexapod
17
    version: 2                           # (1)
18
19
    tcp:
      url: id99hexa1
20
21
    user_origin: 0 0 328.83 0 0 0        # (2)
    object_origin: 0 0 328.83 0 0 0
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
    axes:
      - name: h1tx
        role: tx
        unit: mm
      - name: h1ty
        role: ty
        unit: mm
      - name: h1tz
        role: tz
        unit: mm
      - name: h1rx
        role: rx
        unit: deg
      - name: h1ry
        role: ry
        unit: deg
      - name: h1rz
        role: rz
        unit: deg

42
43
44
1. API version: valid values: 1 or 2 (optional. If no version is given, it
   tries to discover the API version). Authors recommend to put the version
   whenever possible.
45

46
47
2. User/objects origins are optional, they are set at startup
"""
48
49

import gevent.lock
50
51
52

from collections import namedtuple
from math import pi
53
54
55
56
57

from bliss.comm.util import get_comm, TCP
from bliss.comm.tcp import SocketTimeout
from bliss.common.axis import AxisState
from bliss.controllers.motor import Controller
58
from bliss import global_map
59
from bliss.common.logtools import user_print
60

61
62
63
ROLES = "tx", "ty", "tz", "rx", "ry", "rz"
Pose = namedtuple("Pose", ROLES)

64
65
66
67
68
69
70
71
72
# Symetrie hexapods work only with mm and deg, but mrad and microns are more useful units
CUNIT_TO_UNIT = {
    "mrad": pi / 180.0 * 1000,
    "rad": pi / 180.0,
    "micron": 1 / 1000.0,
    "mm": 1,
    "deg": 1,
}

73

74
class BaseHexapodProtocol:
75
76
77
78
79
80
81

    DEFAULT_PORT = None

    Pose = Pose

    def __init__(self, config):
        self.config = config
82
83
        self.eol = "\r\n"
        self.comm = get_comm(config, ctype=TCP, port=self.DEFAULT_PORT, eol=self.eol)
84
85
86
87
88

    #
    # To be overwritten by sub-class
    #

89
    def _homing(self, async_=False):
90
91
        raise NotImplementedError

92
    def _move(self, pose, async_=False):
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
136
137
138
139
140
141
142
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
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
        raise NotImplementedError

    def _stop(self):
        raise NotImplementedError

    def _reset(self):
        raise NotImplementedError

    #
    # API to Hexapod emotion controller
    #

    # Fist, the ones that must me overwritten sub-class

    @property
    def object_pose(self):
        """
        Return a sequence of tx, ty, tz, rx, ry, rz.
        Translation in mm; Rotation in degrees.
        """
        raise NotImplementedError

    @property
    def system_status(self):
        """
        Return object with (at least) members (bool):
        - control (true if control is active)
        - error (true if there is an error)
        - moving (true if hexapod is moving)
        """
        raise NotImplementedError

    @property
    def tspeed(self):
        """
        Returns translation speed (mm/s)
        """
        raise NotImplementedError

    @tspeed.setter
    def tspeed(self, tspeed):
        """
        Set translation speed (mm/s)
        """
        raise NotImplementedError

    @property
    def rspeed(self):
        """
        Returns rotational speed (deg/s)
        """
        raise NotImplementedError

    @rspeed.setter
    def rspeed(self, rspeed):
        """
        Set rotational speed (mm/s)
        """
        raise NotImplementedError

    @property
    def tacceleration(self):
        """
        Returns translation acceleration (mm/s)
        """
        raise NotImplementedError

    @tacceleration.setter
    def tacceleration(self, taccel):
        """
        Set translation acceleration (mm/s)
        """
        raise NotImplementedError

    @property
    def racceleration(self):
        """
        Returns rotational acceleration (deg/s)
        """
        raise NotImplementedError

    @racceleration.setter
    def racceleration(self, raccel):
        """
        Set rotational acceleration (mm/s)
        """
        raise NotImplementedError

    def start_move(self, pose):
        """
        Start absolute motion to pose (user coordinates)

        Returns:
            AsyncResult: handler which can be used to wait for the end of the
                         motion
        """
189
        return self._move(pose, async_=True)
190
191
192
193
194
195
196
197

    def move(self, pose):
        """
        Move to given pose (user coordinates) and wait for motion to finish
        """
        return self._move(pose)

    def start_homing(self):
198
        return self._homing(async_=True)
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216

    def homing(self):
        return self._homing()

    def stop(self):
        return self._stop()

    def reset(self):
        return self._reset()


class BaseHexapodError(Exception):
    pass


class SHexapod(Controller):
    """Symetrie hexapod controller"""

217
    def __init__(self, *args, **kwargs):
bliss administrator's avatar
bliss administrator committed
218
        super().__init__(*args, **kwargs)
219
220
221
222
223
224

        user_origin = self.config.get("user_origin", "")
        object_origin = self.config.get("object_origin", "")

        if user_origin and object_origin:
            self.set_origin(user_origin.split(), object_origin.split())
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253

    def set_origin(self, user_origin, object_origin):
        if (len(user_origin) + len(object_origin)) != 12:
            raise ValueError(
                "Wrong parameter number: need 12 values to define user and object origin"
            )

        try:
            cmd = (
                "Q80=%f Q81=%f Q82=%f Q83=%f Q84=%f Q85=%f \
Q86=%f Q87=%f Q88=%f Q89=%f Q90=%f Q91=%f Q20=21"
                % (
                    float(user_origin[0]),
                    float(user_origin[1]),
                    float(user_origin[2]),
                    float(user_origin[3]),
                    float(user_origin[4]),
                    float(user_origin[5]),
                    float(object_origin[0]),
                    float(object_origin[1]),
                    float(object_origin[2]),
                    float(object_origin[3]),
                    float(object_origin[4]),
                    float(object_origin[5]),
                )
            )
        except ValueError:
            raise TypeError("Need float values to define user and object origin")

254
255
256
257
258
        prot = self.protocol()
        if isinstance(prot, HexapodProtocolV1):
            prot.pmac(cmd)
        else:
            raise NotImplementedError
259
260
261
262

    def __info__(self):
        return self.get_info(None)

263
    def protocol(self):
264
        if hasattr(self, "_protocol"):
265
            return self._protocol
Emmanuel Papillon's avatar
Emmanuel Papillon committed
266

267
        version = self.config.config_dict.get("version", None)
Emmanuel Papillon's avatar
Emmanuel Papillon committed
268
269
270
271
272
273
        if version == 1:
            all_klass = (HexapodProtocolV1,)
        elif version == 2:
            all_klass = (HexapodProtocolV2,)
        else:
            all_klass = (HexapodProtocolV2, HexapodProtocolV1)
274

Emmanuel Papillon's avatar
Emmanuel Papillon committed
275
        for klass in all_klass:
276
277
            try:
                protocol = klass(self.config.config_dict)
278
                global_map.register(
279
280
                    protocol, parents_list=[self], children_list=[protocol.comm]
                )
281
282
283
284
285
                protocol.comm.open()
                self._protocol = protocol
                break
            except gevent.socket.error:
                pass
286
287
            except SocketTimeout:
                pass
288
        else:
289
            raise ValueError("Could not find Hexapod (is it connected?)")
290
291
        return self._protocol

292
293
294
295
296
    def initialize(self):
        # velocity and acceleration are not mandatory in config
        self.axis_settings.config_setting["velocity"] = False
        self.axis_settings.config_setting["acceleration"] = False

297
298
299
300
301
302
    def initialize_hardware(self):
        self.protocol().control = True

    def initialize_axis(self, axis):
        role = self.__get_axis_role(axis)
        if role not in ROLES:
303
            raise ValueError("Invalid role {0!r} for axis {1}".format(role, axis.name))
304

305
306
307
308
309
310
311
312
313
314
315
316
317
318
        # on this controller the homing procedure is particular so here
        # we replace the *axis.home* by the specific homing procedure.
        def _home():
            protocol = self.protocol()
            # start homing
            protocol.homing()
            # Wait the procedure to starts
            gevent.sleep(1)
            while protocol.system_status.moving:
                gevent.sleep(0.1)
            # home_done is not synchronous with moving!!!
            # Wait a little bit
            gevent.sleep(0.5)
            if not protocol.system_status.homing_done:
319
                user_print("Home failed check status for more info")
320
321
322
323
324
325
326
327
328
329
330
            # Wait that all axis are in position
            while True:
                gevent.sleep(0.2)
                if protocol.system_status.in_position:
                    break
            # Synchronize all hexapod axes.
            for axis in self.axes.values():
                axis.sync_hard()

        axis.home = _home

331
    def __get_axis_role(self, axis):
332
        return axis.config.get("role")
333
334

    def __get_hw_set_position(self, axis):
Sebastien Petitdemange's avatar
Sebastien Petitdemange committed
335
        user_set_pos = axis._set_position
336
337
338
339
340
        dial_set_pos = axis.user2dial(user_set_pos)
        hw_set_pos = dial_set_pos * axis.steps_per_unit
        return hw_set_pos

    def __get_hw_set_positions(self):
341
342
        return dict(
            (
343
344
                (
                    self.__get_axis_role(axis),
Benoit Formet's avatar
Benoit Formet committed
345
                    self.__get_hw_set_position(axis) / CUNIT_TO_UNIT[axis.unit],
346
                )
347
                for axis in self.axes.values()
348
349
            )
        )
350
351
352
353
354

    def start_one(self, motion):
        return self.start_all(motion)

    def start_all(self, *motion_list):
355
        pose_dict = dict(((r, None) for r in ROLES))
356
        pose_dict.update(self.__get_hw_set_positions())
357
358
359
        pose_dict.update(
            dict(
                (
360
361
362
363
                    (
                        self.__get_axis_role(motion.axis),
                        motion.target_pos / CUNIT_TO_UNIT[motion.axis.unit],
                    )
364
365
366
367
                    for motion in motion_list
                )
            )
        )
368
369
370
371
372
373
374
375
376
377
378
379
        pose = Pose(**pose_dict)

        self.protocol().start_move(pose)

    def stop(self, axis):
        self.protocol().stop()

    def stop_all(self, *motions):
        self.protocol().stop()

    def state(self, axis):
        status = self.protocol().system_status
380
        state = "READY"
381
        if status.moving:
382
            state = "MOVING"
383
        if not status.control:
384
            state = "OFF"
385
        if status.error:
386
            state = "FAULT"
387
388
389
        state = AxisState(state)
        return state

390
    def get_axis_info(self, axis):
391
392
393
        return self.protocol().dump()

    def read_position(self, axis):
394
395
396
        return CUNIT_TO_UNIT[axis.unit] * getattr(
            self.protocol().object_pose, self.__get_axis_role(axis)
        )
397
398
399
400
401
402
403
404
405
406
407

    def set_position(self, axis, new_position):
        raise NotImplementedError

    def set_on(self, axis):
        self.protocol().control = True

    def set_off(self, axis):
        self.protocol().control = False

    def read_velocity(self, axis):
408
        if self.__get_axis_role(axis).startswith("t"):
409
410
411
412
            return self.protocol().tspeed
        else:
            return self.protocol().rspeed

413
414
    #    def set_velocity(self, axis, new_velocity):
    #        raise NotImplementedError
415
416

    def read_acceleration(self, axis):
417
        if self.__get_axis_role(axis).startswith("t"):
418
419
420
421
            return self.protocol().tacceleration
        else:
            return self.protocol().racceleration

422
423
    #    def set_acceleration(self, axis, new_acc):
    #        raise NotImplementedError
424

425
426
    def make_ref(self):
        self.protocol().start_homing()  # async or not?
427

428
429
    def reset(self):
        self.protocol().reset()
Emmanuel Papillon's avatar
Emmanuel Papillon committed
430
431


bliss administrator's avatar
bliss administrator committed
432
# at end of file to avoid circular import
Benoit Formet's avatar
Benoit Formet committed
433
434
from bliss.controllers.motors.shexapodV1 import HexapodProtocolV1  # noqa E402
from bliss.controllers.motors.shexapodV2 import HexapodProtocolV2  # noqa E402