OpenSMI-Core 0.4.1__tar.gz → 0.6__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (47) hide show
  1. opensmi_core-0.6/.github/workflows/checks.yml +72 -0
  2. {opensmi_core-0.4.1 → opensmi_core-0.6}/.github/workflows/python-publish.yml +4 -1
  3. {opensmi_core-0.4.1 → opensmi_core-0.6}/PKG-INFO +1 -1
  4. {opensmi_core-0.4.1 → opensmi_core-0.6}/pyproject.toml +11 -5
  5. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/OpenSMI_Core.egg-info/PKG-INFO +1 -1
  6. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/OpenSMI_Core.egg-info/SOURCES.txt +3 -0
  7. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/OpenSMI_Core.egg-info/scm_file_list.json +3 -0
  8. opensmi_core-0.6/src/OpenSMI_Core.egg-info/scm_version.json +8 -0
  9. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/base_ua_object.py +1 -3
  10. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/logging.py +74 -36
  11. opensmi_core-0.6/src/opensmi/core/rsl.py +271 -0
  12. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/subscription_manager.py +139 -38
  13. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/ua_enums.py +1 -1
  14. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/ua_node_util.py +4 -2
  15. opensmi_core-0.6/tests/test_rsl.py +495 -0
  16. {opensmi_core-0.4.1 → opensmi_core-0.6}/tools/auto_generate_units.py +3 -0
  17. opensmi_core-0.4.1/src/OpenSMI_Core.egg-info/scm_version.json +0 -8
  18. {opensmi_core-0.4.1 → opensmi_core-0.6}/.github/workflows/api-docs.yml +0 -0
  19. {opensmi_core-0.4.1 → opensmi_core-0.6}/.gitignore +0 -0
  20. {opensmi_core-0.4.1 → opensmi_core-0.6}/.gitlab-ci.yml +0 -0
  21. {opensmi_core-0.4.1 → opensmi_core-0.6}/LICENSE +0 -0
  22. {opensmi_core-0.4.1 → opensmi_core-0.6}/LICENSES/MIT.txt +0 -0
  23. {opensmi_core-0.4.1 → opensmi_core-0.6}/README.md +0 -0
  24. {opensmi_core-0.4.1 → opensmi_core-0.6}/setup.cfg +0 -0
  25. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/OpenSMI_Core.egg-info/dependency_links.txt +0 -0
  26. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/OpenSMI_Core.egg-info/requires.txt +0 -0
  27. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/OpenSMI_Core.egg-info/top_level.txt +0 -0
  28. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/__init__.py +0 -0
  29. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/async_task_mixin.py +0 -0
  30. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/base_server.py +0 -0
  31. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/config.py +0 -0
  32. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/enums.py +0 -0
  33. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/errors.py +0 -0
  34. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/frozen.py +0 -0
  35. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/hook.py +0 -0
  36. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/lifecycle_mixin.py +0 -0
  37. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/meta.py +0 -0
  38. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/mixin_helpers.py +0 -0
  39. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/protocols.py +0 -0
  40. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/repr_mixin.py +0 -0
  41. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/signal.py +0 -0
  42. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/ua.py +0 -0
  43. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/unit_all.py +0 -0
  44. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/unit_base.py +0 -0
  45. {opensmi_core-0.4.1 → opensmi_core-0.6}/src/opensmi/core/unset.py +0 -0
  46. {opensmi_core-0.4.1 → opensmi_core-0.6}/tests/test_signal.py +0 -0
  47. {opensmi_core-0.4.1 → opensmi_core-0.6}/tests/test_unit.py +0 -0
@@ -0,0 +1,72 @@
1
+ name: Checks
2
+
3
+ on:
4
+ push:
5
+ branches: [ main ]
6
+ pull_request:
7
+ workflow_call: # lets the publishing workflow reuse this one
8
+ secrets:
9
+ CODECOV_TOKEN:
10
+ required: false
11
+
12
+ permissions:
13
+ contents: read
14
+
15
+ jobs:
16
+ lint:
17
+ name: Ruff and basedpyright
18
+ runs-on: ubuntu-latest
19
+
20
+ steps:
21
+ - uses: actions/checkout@v4
22
+
23
+ - uses: astral-sh/setup-uv@v7
24
+ with:
25
+ python-version: "3.12"
26
+ enable-cache: true
27
+
28
+ - name: Install dependencies
29
+ run: uv sync --group dev --all-extras
30
+
31
+ # - name: Ruff lint
32
+ # run: uv run ruff check .
33
+
34
+ - name: Ruff format check
35
+ run: uv run ruff format --check .
36
+
37
+ - name: basedpyright
38
+ run: uv run basedpyright
39
+
40
+ test:
41
+ runs-on: ubuntu-latest
42
+ strategy:
43
+ fail-fast: false
44
+ matrix:
45
+ python-version: [ "3.11", "3.12", "3.13", "3.14" ]
46
+
47
+ steps:
48
+ - uses: actions/checkout@v4
49
+
50
+ - uses: astral-sh/setup-uv@v7
51
+ with:
52
+ python-version: ${{ matrix.python-version }}
53
+ enable-cache: true
54
+
55
+ - name: Install dependencies
56
+ run: uv sync --group dev
57
+
58
+ - name: Run tests with coverage
59
+ run: >
60
+ uv run pytest
61
+ -n auto
62
+ --cov
63
+ --cov-report=term-missing
64
+ --cov-report=xml
65
+
66
+ - name: Upload coverage to Codecov
67
+ uses: codecov/codecov-action@v5
68
+ with:
69
+ token: ${{ secrets.CODECOV_TOKEN }}
70
+ files: coverage.xml
71
+ flags: py${{ matrix.python-version }}
72
+ fail_ci_if_error: false # don't block on Codecov outages or fork PRs without the token
@@ -10,12 +10,15 @@ name: Upload Python Package
10
10
 
11
11
  on:
12
12
  release:
13
- types: [published]
13
+ types: [ published ]
14
14
 
15
15
  permissions:
16
16
  contents: read
17
17
 
18
18
  jobs:
19
+ checks:
20
+ uses: ./.github/workflows/checks.yml
21
+
19
22
  release-build:
20
23
  runs-on: ubuntu-latest
21
24
 
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: OpenSMI-Core
3
- Version: 0.4.1
3
+ Version: 0.6
4
4
  Summary: Smart Machine Interface core functionality for server and client applications.
5
5
  Author-email: Carsten Harms <Carsten.Harms@dfki.de>, Simon Jungbluth <Simon.Jungbluth@smartfactory.de>
6
6
  License-Expression: MIT
@@ -61,7 +61,9 @@ dev = [
61
61
  "pytest-xdist>=3",
62
62
  "pytest-html>=4",
63
63
  "pytest-timeout>=2",
64
- "openpyxl" # to load UNECE Excel to autogenerate units
64
+ "openpyxl", # to load UNECE Excel to autogenerate units
65
+ "scipy~=1.17.1", # for testing, serves as reference implementation. newer versions dropped Python 3.11
66
+ "numpy~=2.4.6" # for scipy, newer versions dropped Python 3.11
65
67
  ]
66
68
 
67
69
  [tool.setuptools]
@@ -133,10 +135,10 @@ author = "OpenSMI Contributors"
133
135
 
134
136
  [tool.ruff.lint.per-file-ignores]
135
137
  "tests/**" = ["ANN", "D", "T", "SLF001"]
136
- "src/opensmi/core/unit_all.py" = [ # auto-generated
137
- "E501", # line too long
138
- "RUF001", # String contains ambiguous ~
139
- "CPY", # copyright
138
+ "src/opensmi/core/unit_all.py" = [# auto-generated
139
+ "E501", # line too long
140
+ "RUF001", # String contains ambiguous ~
141
+ "CPY", # copyright
140
142
  ]
141
143
 
142
144
  [tool.ruff.format]
@@ -180,3 +182,7 @@ theme = "readthedocs"
180
182
  # verbose = 1
181
183
  # warnings-as-errors = true
182
184
  intersphinx = ["https://opcua-asyncio.readthedocs.io/en/latest/objects.inv"] # TODO(CaHa): does not work
185
+
186
+ [tool.coverage.run]
187
+ source = ["src/opensmi/core"]
188
+ branch = true
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: OpenSMI-Core
3
- Version: 0.4.1
3
+ Version: 0.6
4
4
  Summary: Smart Machine Interface core functionality for server and client applications.
5
5
  Author-email: Carsten Harms <Carsten.Harms@dfki.de>, Simon Jungbluth <Simon.Jungbluth@smartfactory.de>
6
6
  License-Expression: MIT
@@ -4,6 +4,7 @@ LICENSE
4
4
  README.md
5
5
  pyproject.toml
6
6
  .github/workflows/api-docs.yml
7
+ .github/workflows/checks.yml
7
8
  .github/workflows/python-publish.yml
8
9
  LICENSES/MIT.txt
9
10
  src/OpenSMI_Core.egg-info/PKG-INFO
@@ -28,6 +29,7 @@ src/opensmi/core/meta.py
28
29
  src/opensmi/core/mixin_helpers.py
29
30
  src/opensmi/core/protocols.py
30
31
  src/opensmi/core/repr_mixin.py
32
+ src/opensmi/core/rsl.py
31
33
  src/opensmi/core/signal.py
32
34
  src/opensmi/core/subscription_manager.py
33
35
  src/opensmi/core/ua.py
@@ -36,6 +38,7 @@ src/opensmi/core/ua_node_util.py
36
38
  src/opensmi/core/unit_all.py
37
39
  src/opensmi/core/unit_base.py
38
40
  src/opensmi/core/unset.py
41
+ tests/test_rsl.py
39
42
  tests/test_signal.py
40
43
  tests/test_unit.py
41
44
  tools/auto_generate_units.py
@@ -1,6 +1,7 @@
1
1
  {
2
2
  "files": [
3
3
  ".github/workflows/api-docs.yml",
4
+ ".github/workflows/checks.yml",
4
5
  ".github/workflows/python-publish.yml",
5
6
  ".gitignore",
6
7
  ".gitlab-ci.yml",
@@ -23,6 +24,7 @@
23
24
  "src/opensmi/core/mixin_helpers.py",
24
25
  "src/opensmi/core/protocols.py",
25
26
  "src/opensmi/core/repr_mixin.py",
27
+ "src/opensmi/core/rsl.py",
26
28
  "src/opensmi/core/signal.py",
27
29
  "src/opensmi/core/subscription_manager.py",
28
30
  "src/opensmi/core/ua.py",
@@ -31,6 +33,7 @@
31
33
  "src/opensmi/core/unit_all.py",
32
34
  "src/opensmi/core/unit_base.py",
33
35
  "src/opensmi/core/unset.py",
36
+ "tests/test_rsl.py",
34
37
  "tests/test_signal.py",
35
38
  "tests/test_unit.py",
36
39
  "tools/auto_generate_units.py"
@@ -0,0 +1,8 @@
1
+ {
2
+ "tag": "0.6",
3
+ "distance": 0,
4
+ "node": "g242780f6570d8852355c9ab5fe3335ccbba3e1e9",
5
+ "dirty": false,
6
+ "branch": "HEAD",
7
+ "node_date": "2026-11-02"
8
+ }
@@ -50,9 +50,7 @@ class BaseUaObject(Generic[_ServerType]):
50
50
  if parent is not None:
51
51
  self.parent = parent # via setter to allow optional runtime type checking
52
52
  self._ua_node: Node | None = None
53
- self._logger: structlog.BoundLogger = structlog.get_logger(
54
- f"opensmi.{self.__class__.__name__}", name=self.name
55
- )
53
+ self._logger: structlog.BoundLogger = structlog.get_logger(f"opensmi.{self.__class__.__name__}", name=self.name)
56
54
 
57
55
  super().__init__(**kwargs) # cooperative __init__() after we are done
58
56
 
@@ -10,7 +10,8 @@ import shutil
10
10
  import sys
11
11
  from collections.abc import Mapping, MutableMapping
12
12
  from enum import Enum
13
- from typing import Any
13
+ from pathlib import PurePath
14
+ from typing import Any, Final
14
15
 
15
16
  import structlog
16
17
 
@@ -31,8 +32,17 @@ def _elapsed_to_ms(_, __, event_dict: MutableMapping[str, Any]) -> MutableMappin
31
32
  return event_dict
32
33
 
33
34
 
35
+ def _path_to_str(_, __, event_dict: MutableMapping[str, Any]) -> MutableMapping[str, Any]: # noqa: ANN001
36
+ """Convert ``path`` entry in structlog processor event dict to simple string."""
37
+ if "path" in event_dict:
38
+ path = event_dict["path"]
39
+ if isinstance(path, PurePath):
40
+ event_dict["path"] = str(path)
41
+ return event_dict
42
+
43
+
34
44
  _logging_initialized = False
35
- """ Global flag indicated whether logging is initialized. """
45
+ """Global flag indicated whether logging is initialized."""
36
46
 
37
47
 
38
48
  class LogFormat(Enum):
@@ -46,11 +56,15 @@ class LogFormat(Enum):
46
56
  """JSON formatted logging output"""
47
57
 
48
58
 
49
- def _setup_console_logging() -> None:
59
+ def _setup_key_value_logging(
60
+ *,
61
+ min_level: int | str = logging.NOTSET,
62
+ enable_rich_tracebacks: bool = True,
63
+ ) -> None:
50
64
  from importlib.util import find_spec
51
65
 
52
66
  # see https://www.structlog.org/en/stable/logging-best-practices.html#pretty-printing-vs-structured-output
53
- if find_spec("rich") is not None:
67
+ if find_spec("rich") is not None and enable_rich_tracebacks:
54
68
  console_renderer = structlog.dev.ConsoleRenderer(
55
69
  sort_keys=False,
56
70
  exception_formatter=structlog.dev.RichTracebackFormatter(
@@ -62,10 +76,10 @@ def _setup_console_logging() -> None:
62
76
 
63
77
  processors = [
64
78
  structlog.contextvars.merge_contextvars,
65
- structlog.stdlib.filter_by_level,
66
79
  structlog.stdlib.add_logger_name,
67
80
  structlog.stdlib.add_log_level,
68
81
  _elapsed_to_ms,
82
+ _path_to_str,
69
83
  structlog.processors.TimeStamper(fmt="iso", utc=False),
70
84
  structlog.processors.UnicodeDecoder(),
71
85
  console_renderer,
@@ -73,67 +87,91 @@ def _setup_console_logging() -> None:
73
87
 
74
88
  structlog.configure(
75
89
  processors=processors,
76
- wrapper_class=structlog.make_filtering_bound_logger(logging.NOTSET),
90
+ wrapper_class=structlog.make_filtering_bound_logger(min_level=min_level),
77
91
  logger_factory=structlog.stdlib.LoggerFactory(),
78
92
  cache_logger_on_first_use=True,
79
93
  )
80
94
 
81
95
 
82
- def _setup_key_value_logging() -> None:
96
+ def _setup_json_logging(
97
+ *,
98
+ min_level: int | str = logging.NOTSET,
99
+ ) -> None:
100
+
101
+ processors = [
102
+ structlog.contextvars.merge_contextvars,
103
+ structlog.stdlib.add_logger_name,
104
+ structlog.stdlib.add_log_level,
105
+ _path_to_str,
106
+ structlog.processors.dict_tracebacks,
107
+ structlog.processors.TimeStamper(fmt="iso", utc=True),
108
+ ]
109
+
83
110
  try:
84
111
  import orjson
85
112
 
86
113
  _LOGGER.info("Using orjson serialization")
87
- processors = [
88
- structlog.contextvars.merge_contextvars,
89
- # structlog.stdlib.filter_by_level,
90
- # structlog.stdlib.add_logger_name,
91
- structlog.stdlib.add_log_level,
92
- _elapsed_to_ms,
93
- structlog.processors.dict_tracebacks,
94
- structlog.processors.JSONRenderer(serializer=orjson.dumps),
95
- ]
114
+
115
+ processors.append(structlog.processors.JSONRenderer(serializer=orjson.dumps))
96
116
  logger_factory = structlog.BytesLoggerFactory()
97
117
  except ImportError:
98
- _LOGGER.warning("Could not import orjson, fallback to buildin serializer.")
99
-
100
- processors = [
101
- structlog.contextvars.merge_contextvars,
102
- # structlog.stdlib.filter_by_level,
103
- # structlog.stdlib.add_logger_name,
104
- structlog.stdlib.add_log_level,
105
- _elapsed_to_ms,
106
- structlog.processors.dict_tracebacks,
107
- structlog.processors.JSONRenderer(),
108
- ]
118
+ _LOGGER.warning("Could not import orjson, fallback to builtin serializer.")
119
+
120
+ processors.append(structlog.processors.JSONRenderer())
109
121
  logger_factory = structlog.stdlib.LoggerFactory()
110
122
 
111
123
  structlog.configure(
112
124
  processors=processors,
113
- wrapper_class=structlog.make_filtering_bound_logger(logging.NOTSET),
125
+ wrapper_class=structlog.make_filtering_bound_logger(min_level=min_level),
114
126
  logger_factory=logger_factory,
115
127
  cache_logger_on_first_use=True,
116
128
  )
117
129
 
118
130
 
131
+ DEFAULT_LOG_LEVELS: Final[dict[str, int | str]] = {
132
+ "opensmi": logging.INFO,
133
+ "asyncua": logging.ERROR,
134
+ }
135
+
136
+
119
137
  def setup_logging(
120
138
  *,
139
+ log_min_level: int | str = logging.NOTSET,
121
140
  log_levels: Mapping[str, int | str] | None = None,
122
141
  log_format: LogFormat = LogFormat.AUTOMATIC,
142
+ enable_rich_tracebacks: bool = True,
143
+ force_basic_config: bool = False,
123
144
  ) -> None:
124
- """Set up structured logging."""
145
+ """Set up structured logging.
146
+
147
+ ``log_format`` can be overridden with the ``LOG_FORMAT`` environment variable.
148
+
149
+ :param log_min_level: Minimum logging level for structlog loggers.
150
+ :param log_levels: Logging levels for *all* loggers. Maps logger name to level. Defaults to `DEFAULT_LOG_LEVELS`.
151
+ :param log_format: Logging format for *all* loggers.
152
+ :param enable_rich_tracebacks: Enable Rich tracebacks if `KEY_VALUE` log format is used.
153
+ :param force_basic_config: ``force`` parameter of ``logging.basicConfig``.
154
+ """
125
155
  global _logging_initialized
126
156
  if _logging_initialized:
127
157
  return
128
158
 
129
- logging.basicConfig(force=True, format="")
159
+ logging.basicConfig(format="", force=force_basic_config)
160
+
161
+ if log_levels is None:
162
+ log_levels = DEFAULT_LOG_LEVELS
163
+
164
+ # structlog loggers created via structlog.stdlib.LoggerFactory are also gated by the stdlib level
165
+ if log_min_level != logging.NOTSET and (log_levels is None or "" not in log_levels):
166
+ logging.getLogger().setLevel(log_min_level)
167
+
130
168
  if log_levels is not None:
131
169
  for name, level in log_levels.items():
132
170
  try:
133
171
  logging.getLogger(name).setLevel(level)
134
- _LOGGER.debug(f"Set logger level of '{name}' to {level}")
172
+ _LOGGER.debug("Set logger level of %r to %s", name, level)
135
173
  except (TypeError, ValueError):
136
- _LOGGER.exception(f"Could not set logger level of '{name}' to {level}")
174
+ _LOGGER.exception("Could not set logger level of %r to %s", name, level)
137
175
  if _LOGGER.level == logging.NOTSET:
138
176
  _LOGGER.setLevel(logging.INFO)
139
177
 
@@ -142,8 +180,8 @@ def setup_logging(
142
180
  if env_log_format is not None:
143
181
  try:
144
182
  log_format = LogFormat[env_log_format.upper()]
145
- except ValueError:
146
- _LOGGER.warning(f"Invalid log format '{env_log_format}'")
183
+ except KeyError:
184
+ _LOGGER.warning("Invalid log format %r", env_log_format)
147
185
 
148
186
  if log_format == LogFormat.AUTOMATIC:
149
187
  if sys.stderr.isatty():
@@ -154,8 +192,8 @@ def setup_logging(
154
192
  log_format = LogFormat.JSON
155
193
 
156
194
  if log_format == LogFormat.KEY_VALUE:
157
- _setup_console_logging()
195
+ _setup_key_value_logging(min_level=log_min_level, enable_rich_tracebacks=enable_rich_tracebacks)
158
196
  else: # JSON
159
- _setup_key_value_logging()
197
+ _setup_json_logging(min_level=log_min_level)
160
198
 
161
199
  _logging_initialized = True
@@ -0,0 +1,271 @@
1
+ # SPDX-FileCopyrightText: 2026 OpenSMI Contributors
2
+ #
3
+ # SPDX-License-Identifier: MIT
4
+
5
+ """Pose types with unit handling for OPC UA, plus light-weight pose math.
6
+
7
+ Based on the OPC UA Relative Spatial Location (OPC 10000-210), see
8
+ https://reference.opcfoundation.org/specs/OPC-10000-210/full
9
+
10
+ .. warning::
11
+
12
+ The math (``@``, ``inverse()``, ``transform()``) is pure Python on Euler angles,
13
+ meant for occasional use only. It is slow in bulk and accumulates rounding error when
14
+ chained. Use a dedicated library (e.g. NumPy, SciPy) for anything heavier.
15
+
16
+ Euler angles are not unique, so results may differ from the inputs while describing the
17
+ same rotation. Compare with ``isclose()``, not ``==``.
18
+
19
+ Rotations are intrinsic z-y'-x'' (yaw, pitch, roll), equivalent to extrinsic x-y-z.
20
+ ``a @ b`` expresses ``b`` in the frame of ``a`` and uses the left operand's units.
21
+ """
22
+
23
+ from __future__ import annotations
24
+
25
+ import math
26
+ from collections.abc import Callable
27
+ from dataclasses import dataclass, replace
28
+ from functools import cache
29
+ from typing import Final
30
+
31
+ from asyncua import ua
32
+
33
+ from opensmi.core import Unit
34
+
35
+ _Vec = tuple[float, float, float]
36
+ _Mat = tuple[float, float, float, float, float, float, float, float, float] # row-major
37
+
38
+ _GIMBAL_EPS: Final = 1e-12
39
+
40
+
41
+ def _matrix_of(o: Orientation) -> _Mat:
42
+ """Rotation matrix R = Rz(c) @ Ry(b) @ Rx(a) (intrinsic z-y'-x'')."""
43
+ k = _angle_factor(o.unit, Unit.RADIAN)
44
+ sa, ca = math.sin(o.a * k), math.cos(o.a * k)
45
+ sb, cb = math.sin(o.b * k), math.cos(o.b * k)
46
+ sc, cc = math.sin(o.c * k), math.cos(o.c * k)
47
+ return (
48
+ cb * cc, sa * sb * cc - ca * sc, ca * sb * cc + sa * sc,
49
+ cb * sc, sa * sb * sc + ca * cc, ca * sb * sc - sa * cc,
50
+ -sb, sa * cb, ca * cb,
51
+ ) # fmt: skip
52
+
53
+
54
+ def _orientation_from(m: _Mat, unit: Unit) -> Orientation:
55
+ """Inverse of ``_matrix_of``. At pitch ±90° (gimbal lock) yaw is set to 0."""
56
+ r00, _, _, r10, r11, r12, r20, r21, r22 = m
57
+ b = math.asin(max(-1.0, min(1.0, -r20)))
58
+ if abs(r20) < 1.0 - _GIMBAL_EPS:
59
+ a, c = math.atan2(r21, r22), math.atan2(r10, r00)
60
+ else:
61
+ a, c = math.atan2(-r12, r11), 0.0
62
+ k = _angle_factor(Unit.RADIAN, unit)
63
+ return Orientation(a * k, b * k, c * k, unit)
64
+
65
+
66
+ def _matmul(p: _Mat, q: _Mat) -> _Mat:
67
+ return tuple( # type: ignore
68
+ sum(p[3 * i + k] * q[3 * k + j] for k in range(3)) for i in range(3) for j in range(3)
69
+ )
70
+
71
+
72
+ def _transpose(m: _Mat) -> _Mat:
73
+ return m[0], m[3], m[6], m[1], m[4], m[7], m[2], m[5], m[8]
74
+
75
+
76
+ def _rotate(m: _Mat, v: _Vec) -> _Vec:
77
+ x, y, z = v
78
+ return (
79
+ m[0] * x + m[1] * y + m[2] * z,
80
+ m[3] * x + m[4] * y + m[5] * z,
81
+ m[6] * x + m[7] * y + m[8] * z,
82
+ )
83
+
84
+
85
+ # TODO(CaHa): Maybe attach conversion factor to Unit. UNECE table has this information
86
+ _LENGTH: Final = {
87
+ Unit.MILLIMETRE: 0.001,
88
+ Unit.CENTIMETRE: 0.01,
89
+ Unit.METRE: 1.0,
90
+ }
91
+ _ANGLE: Final = {
92
+ Unit.DEGREE_UNIT_OF_ANGLE: math.pi / 180,
93
+ Unit.RADIAN: 1.0,
94
+ }
95
+
96
+
97
+ def _require_supported(unit: Unit, table: dict[Unit, float]) -> None:
98
+ if unit not in table:
99
+ msg = f"unsupported unit {unit!r}, expected one of {list(table)}"
100
+ raise ValueError(msg)
101
+
102
+
103
+ def _make_factor(table: dict[Unit, float]) -> Callable[[Unit, Unit], float]:
104
+ @cache
105
+ def factor(src: Unit, dst: Unit) -> float:
106
+ _require_supported(src, table)
107
+ _require_supported(dst, table)
108
+ return table[src] / table[dst]
109
+
110
+ return factor
111
+
112
+
113
+ _length_factor: Final = _make_factor(_LENGTH)
114
+ _angle_factor: Final = _make_factor(_ANGLE)
115
+
116
+
117
+ @dataclass(frozen=True, slots=True)
118
+ class Position:
119
+ """Cartesian position defined by X, Y, and Z coordinates.
120
+
121
+ see https://reference.opcfoundation.org/specs/OPC-10000-210/b-1
122
+ """
123
+
124
+ x: float = 0.0
125
+ y: float = 0.0
126
+ z: float = 0.0
127
+ unit: Unit = Unit.MILLIMETRE
128
+
129
+ def __post_init__(self) -> None:
130
+ """Verify unit post initialization."""
131
+ _require_supported(self.unit, _LENGTH)
132
+
133
+ def to(self, unit: Unit) -> Position:
134
+ """Convert this position to one with given ``unit``."""
135
+ if unit == self.unit:
136
+ return self
137
+ f = _length_factor(self.unit, unit)
138
+ return Position(f * self.x, f * self.y, f * self.z, unit)
139
+
140
+ def as_tuple(self) -> tuple[float, float, float]:
141
+ """Return ``(x, y, z)`` in this position's unit."""
142
+ return self.x, self.y, self.z
143
+
144
+ def is_close(self, other: Position, *, rel_tol: float = 1e-9, abs_tol: float = 1e-9) -> bool:
145
+ """Approximate equality; ``other`` is converted to this position's unit."""
146
+ o = other.to(self.unit)
147
+ return all(
148
+ math.isclose(p, q, rel_tol=rel_tol, abs_tol=abs_tol)
149
+ for p, q in zip(self.as_tuple(), o.as_tuple(), strict=True)
150
+ )
151
+
152
+
153
+ @dataclass(frozen=True, slots=True)
154
+ class Orientation:
155
+ """Angular orientation defined by A=roll (X), B=pitch (Y), C=yaw (Z).
156
+
157
+ The rotations are applied as intrinsic ``z-y'-x''`` (yaw, then pitch, then roll),
158
+ which is equivalent to extrinsic ``x-y-z``.
159
+
160
+ see https://reference.opcfoundation.org/specs/OPC-10000-210/b-2
161
+ """
162
+
163
+ a: float = 0.0
164
+ """Rotation around x axis (roll)"""
165
+ b: float = 0.0
166
+ """Rotation around y axis (pitch)"""
167
+ c: float = 0.0
168
+ """Rotation around z axis (yaw)"""
169
+ unit: Unit = Unit.DEGREE_UNIT_OF_ANGLE
170
+
171
+ def __post_init__(self) -> None:
172
+ """Verify unit post initialization."""
173
+ _require_supported(self.unit, _ANGLE)
174
+
175
+ def to(self, unit: Unit) -> Orientation:
176
+ """Convert this orientation to one with given ``unit``."""
177
+ if unit == self.unit:
178
+ return self
179
+ f = _angle_factor(self.unit, unit)
180
+ return Orientation(f * self.a, f * self.b, f * self.c, unit)
181
+
182
+ def as_tuple(self) -> tuple[float, float, float]:
183
+ """Return ``(a, b, c)`` in this orientation's unit."""
184
+ return self.a, self.b, self.c
185
+
186
+ def is_close(self, other: Orientation, *, abs_tol: float = 1e-9) -> bool:
187
+ """Approximate equality of the *rotation*, regardless of Euler representation."""
188
+ return all(
189
+ math.isclose(p, q, rel_tol=0.0, abs_tol=abs_tol)
190
+ for p, q in zip(_matrix_of(self), _matrix_of(other), strict=True)
191
+ )
192
+
193
+
194
+ @dataclass(frozen=True, slots=True)
195
+ class Pose:
196
+ """Pose defined by Cartesian `Position` and angular `Orientation`.
197
+
198
+ Corresponds to OPC UA data type ``3DFrameType``.
199
+ """
200
+
201
+ position: Position = Position()
202
+ orientation: Orientation = Orientation()
203
+
204
+ def with_position(self, position: Position) -> Pose:
205
+ """Return a copy of this Pose with given ``position`` keeping the orientation."""
206
+ return replace(self, position=position)
207
+
208
+ def with_orientation(self, orientation: Orientation) -> Pose:
209
+ """Return a copy of this Pose with given ``orientation`` keeping the position."""
210
+ return replace(self, orientation=orientation)
211
+
212
+ def __matmul__(self, other: Pose) -> Pose:
213
+ """Compose two poses.
214
+
215
+ ``other`` is expressed in this pose's frame; the result is expressed in this
216
+ pose's parent frame, in this pose's units::
217
+
218
+ position = self.position + R_self · other.position
219
+ orientation = R_self · R_other
220
+
221
+ The resulting Euler angles describe an equivalent rotation, but not necessarily
222
+ the same triple one would compute by hand (gimbal lock, angle wrapping).
223
+ Use `is_close` to compare poses.
224
+ """
225
+ unit = self.position.unit
226
+ r = _matrix_of(self.orientation)
227
+ dx, dy, dz = _rotate(r, other.position.to(unit).as_tuple())
228
+ x, y, z = self.position.as_tuple()
229
+ return Pose(
230
+ Position(x + dx, y + dy, z + dz, unit),
231
+ _orientation_from(_matmul(r, _matrix_of(other.orientation)), self.orientation.unit),
232
+ )
233
+
234
+ def inverse(self) -> Pose:
235
+ """Return the pose that undoes this one, so ``p @ p.inverse()`` is the identity."""
236
+ rt = _transpose(_matrix_of(self.orientation))
237
+ x, y, z = _rotate(rt, self.position.as_tuple())
238
+ return Pose(
239
+ Position(-x, -y, -z, self.position.unit),
240
+ _orientation_from(rt, self.orientation.unit),
241
+ )
242
+
243
+ def transform(self, point: Position) -> Position:
244
+ """Express ``point`` (given in this pose's frame) in the parent frame."""
245
+ unit = self.position.unit
246
+ dx, dy, dz = _rotate(_matrix_of(self.orientation), point.to(unit).as_tuple())
247
+ x, y, z = self.position.as_tuple()
248
+ return Position(x + dx, y + dy, z + dz, unit)
249
+
250
+ def is_close(self, other: Pose, *, rel_tol: float = 1e-9, abs_tol: float = 1e-9) -> bool:
251
+ """Approximate equality, regardless of Euler representation."""
252
+ return self.position.is_close(other.position, rel_tol=rel_tol, abs_tol=abs_tol) and (
253
+ self.orientation.is_close(other.orientation, abs_tol=abs_tol)
254
+ )
255
+
256
+ def to_ua(self, *, length_unit: Unit, angle_unit: Unit) -> ua.ThreeDFrame:
257
+ """Convert to a OPC UA ``3DFrame``, expressed in the given units."""
258
+ p, o = self.position.to(length_unit), self.orientation.to(angle_unit)
259
+ return ua.ThreeDFrame(
260
+ CartesianCoordinates=ua.ThreeDCartesianCoordinates(X=p.x, Y=p.y, Z=p.z), # type: ignore
261
+ Orientation=ua.ThreeDOrientation(A=o.a, B=o.b, C=o.c), # type: ignore
262
+ )
263
+
264
+ @classmethod
265
+ def from_ua(cls, frame: ua.ThreeDFrame, *, length_unit: Unit, angle_unit: Unit) -> Pose:
266
+ """Convert from ``ThreeDFrame`` whose numbers are in the given units."""
267
+ p, o = frame.CartesianCoordinates, frame.Orientation
268
+ return cls(
269
+ position=Position(float(p.X), float(p.Y), float(p.Z), length_unit),
270
+ orientation=Orientation(float(o.A), float(o.B), float(o.C), angle_unit),
271
+ )