robot-msgs 0.1.0__py3-none-any.whl
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.
- robot_msgs/__init__.py +29 -0
- robot_msgs/_core.py +352 -0
- robot_msgs/compare/__init__.py +18 -0
- robot_msgs/compare/_dispatch.py +92 -0
- robot_msgs/compare/align.py +53 -0
- robot_msgs/compare/comparison.py +52 -0
- robot_msgs/compare/difference.py +116 -0
- robot_msgs/compare/error_stats.py +62 -0
- robot_msgs/compare/metrics.py +66 -0
- robot_msgs/geometry/__init__.py +31 -0
- robot_msgs/geometry/accel.py +52 -0
- robot_msgs/geometry/inertia.py +99 -0
- robot_msgs/geometry/point.py +79 -0
- robot_msgs/geometry/pose.py +90 -0
- robot_msgs/geometry/pose2d.py +33 -0
- robot_msgs/geometry/pose_stamped.py +46 -0
- robot_msgs/geometry/quaternion.py +177 -0
- robot_msgs/geometry/rotations.py +251 -0
- robot_msgs/geometry/transform.py +125 -0
- robot_msgs/geometry/transform_stamped.py +32 -0
- robot_msgs/geometry/twist.py +52 -0
- robot_msgs/geometry/vector3.py +115 -0
- robot_msgs/geometry/wrench.py +52 -0
- robot_msgs/ros/__init__.py +19 -0
- robot_msgs/ros/bag.py +57 -0
- robot_msgs/ros/convert.py +939 -0
- robot_msgs/ros/vision_msgs.py +46 -0
- robot_msgs/sensor/__init__.py +9 -0
- robot_msgs/sensor/camera_info.py +135 -0
- robot_msgs/sensor/image.py +151 -0
- robot_msgs/sensor/imu.py +36 -0
- robot_msgs/sensor/joint_state.py +69 -0
- robot_msgs/sensor/point_cloud.py +116 -0
- robot_msgs/std/__init__.py +5 -0
- robot_msgs/std/header.py +81 -0
- robot_msgs/trajectory/__init__.py +29 -0
- robot_msgs/trajectory/joint_limits.py +65 -0
- robot_msgs/trajectory/joint_trajectory.py +208 -0
- robot_msgs/trajectory/pose_trajectory.py +175 -0
- robot_msgs/trajectory/time_repair.py +145 -0
- robot_msgs/trajectory/trajectory.py +567 -0
- robot_msgs/vision/__init__.py +21 -0
- robot_msgs/vision/bounding_box2d.py +68 -0
- robot_msgs/vision/bounding_box3d.py +50 -0
- robot_msgs/vision/detection2d.py +43 -0
- robot_msgs/vision/detection2d_array.py +80 -0
- robot_msgs/vision/detection3d.py +43 -0
- robot_msgs/vision/detection3d_array.py +104 -0
- robot_msgs/vision/object_hypothesis.py +23 -0
- robot_msgs/vision/object_hypothesis_with_pose.py +29 -0
- robot_msgs/viz/__init__.py +21 -0
- robot_msgs/viz/_common.py +167 -0
- robot_msgs/viz/_dispatch.py +20 -0
- robot_msgs/viz/images.py +215 -0
- robot_msgs/viz/movie.py +280 -0
- robot_msgs/viz/spatial.py +277 -0
- robot_msgs/viz/trajectory.py +525 -0
- robot_msgs-0.1.0.dist-info/METADATA +122 -0
- robot_msgs-0.1.0.dist-info/RECORD +61 -0
- robot_msgs-0.1.0.dist-info/WHEEL +4 -0
- robot_msgs-0.1.0.dist-info/licenses/LICENSE +26 -0
robot_msgs/__init__.py
ADDED
|
@@ -0,0 +1,29 @@
|
|
|
1
|
+
"""robot_msgs — standard robot message types as frozen Python dataclasses, with helpers.
|
|
2
|
+
|
|
3
|
+
Import types from their domain, like ROS packages::
|
|
4
|
+
|
|
5
|
+
from robot_msgs.geometry import Pose, Quaternion
|
|
6
|
+
from robot_msgs.sensor import JointState, Image
|
|
7
|
+
from robot_msgs.trajectory import JointTrajectory
|
|
8
|
+
"""
|
|
9
|
+
|
|
10
|
+
from robot_msgs import compare, geometry, sensor, std, trajectory, vision
|
|
11
|
+
from robot_msgs._core import Msg, allclose, from_dict, load_npz, message_types, save_npz, to_dict
|
|
12
|
+
|
|
13
|
+
__version__ = "0.1.0"
|
|
14
|
+
|
|
15
|
+
__all__ = [
|
|
16
|
+
"compare",
|
|
17
|
+
"Msg",
|
|
18
|
+
"allclose",
|
|
19
|
+
"from_dict",
|
|
20
|
+
"geometry",
|
|
21
|
+
"load_npz",
|
|
22
|
+
"message_types",
|
|
23
|
+
"save_npz",
|
|
24
|
+
"sensor",
|
|
25
|
+
"std",
|
|
26
|
+
"to_dict",
|
|
27
|
+
"trajectory",
|
|
28
|
+
"vision",
|
|
29
|
+
]
|
robot_msgs/_core.py
ADDED
|
@@ -0,0 +1,352 @@
|
|
|
1
|
+
"""What every message shares: the `Msg` base, read-only arrays, value equality, dict round-trips.
|
|
2
|
+
|
|
3
|
+
A message is an immutable value. Its numbers live in read-only numpy arrays, so a message can
|
|
4
|
+
hand out views of its data (slices, rows, whole columns) without copying and without anyone
|
|
5
|
+
being able to change it underneath you.
|
|
6
|
+
"""
|
|
7
|
+
|
|
8
|
+
from __future__ import annotations
|
|
9
|
+
|
|
10
|
+
import dataclasses
|
|
11
|
+
import json
|
|
12
|
+
import math
|
|
13
|
+
import types
|
|
14
|
+
import typing
|
|
15
|
+
import weakref
|
|
16
|
+
from collections.abc import Mapping
|
|
17
|
+
from typing import Any, ClassVar, TypeVar
|
|
18
|
+
|
|
19
|
+
import numpy as np
|
|
20
|
+
|
|
21
|
+
Array = np.ndarray
|
|
22
|
+
"""Annotation alias for array fields. Arrays inside messages are always read-only."""
|
|
23
|
+
|
|
24
|
+
M = TypeVar("M", bound="Msg")
|
|
25
|
+
|
|
26
|
+
_REGISTRY: dict[str, type[Msg]] = {}
|
|
27
|
+
|
|
28
|
+
# Arrays this library allocated and froze itself. Only their views may skip the defensive copy:
|
|
29
|
+
# a caller's own read-only array could be flipped back to writeable by its owner.
|
|
30
|
+
_OWNED: weakref.WeakValueDictionary[int, np.ndarray] = weakref.WeakValueDictionary()
|
|
31
|
+
|
|
32
|
+
|
|
33
|
+
def frozen_array(value: Any, dtype: Any = np.float64) -> np.ndarray:
|
|
34
|
+
"""Return `value` as a read-only array nobody else can mutate.
|
|
35
|
+
|
|
36
|
+
Copies, unless `value` is already a view onto memory this library froze (a slice or row
|
|
37
|
+
of another message's array). WHY: slicing a trajectory must stay O(1), never a copy.
|
|
38
|
+
`dtype=None` keeps the input's dtype (labels, ints, bools in trajectory channels).
|
|
39
|
+
"""
|
|
40
|
+
if isinstance(value, np.ndarray) and (dtype is None or value.dtype == np.dtype(dtype)):
|
|
41
|
+
if not value.flags.writeable and _is_owned(value):
|
|
42
|
+
return value
|
|
43
|
+
array = np.array(value, dtype=dtype, copy=True)
|
|
44
|
+
return adopt(array)
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
def adopt(array: np.ndarray) -> np.ndarray:
|
|
48
|
+
"""Freeze an array this library just allocated, without copying it. Internal use only."""
|
|
49
|
+
array.setflags(write=False)
|
|
50
|
+
_OWNED[id(array)] = array
|
|
51
|
+
return array
|
|
52
|
+
|
|
53
|
+
|
|
54
|
+
def _is_owned(array: np.ndarray) -> bool:
|
|
55
|
+
root = array
|
|
56
|
+
while isinstance(root.base, np.ndarray):
|
|
57
|
+
root = root.base
|
|
58
|
+
return _OWNED.get(id(root)) is root
|
|
59
|
+
|
|
60
|
+
|
|
61
|
+
def optional_array(value: Any, dtype: Any = np.float64) -> np.ndarray | None:
|
|
62
|
+
return None if value is None else frozen_array(value, dtype)
|
|
63
|
+
|
|
64
|
+
|
|
65
|
+
class FrozenDict(Mapping):
|
|
66
|
+
"""A read-only dict for mapping-valued fields (`channels`).
|
|
67
|
+
|
|
68
|
+
WHY not `types.MappingProxyType`: a proxy can't be pickled, so messages holding one could
|
|
69
|
+
not cross a process boundary (multiprocessing, ray) or be deep-copied.
|
|
70
|
+
"""
|
|
71
|
+
|
|
72
|
+
__slots__ = ("_data",)
|
|
73
|
+
|
|
74
|
+
def __init__(self, data: Any = ()) -> None:
|
|
75
|
+
self._data = dict(data)
|
|
76
|
+
|
|
77
|
+
def __getitem__(self, key: Any) -> Any:
|
|
78
|
+
return self._data[key]
|
|
79
|
+
|
|
80
|
+
def __iter__(self) -> Any:
|
|
81
|
+
return iter(self._data)
|
|
82
|
+
|
|
83
|
+
def __len__(self) -> int:
|
|
84
|
+
return len(self._data)
|
|
85
|
+
|
|
86
|
+
def __repr__(self) -> str:
|
|
87
|
+
return f"FrozenDict({self._data!r})"
|
|
88
|
+
|
|
89
|
+
def __reduce__(self) -> tuple[Any, ...]:
|
|
90
|
+
return (FrozenDict, (self._data,))
|
|
91
|
+
|
|
92
|
+
|
|
93
|
+
def set_field(msg: Msg, name: str, value: Any) -> None:
|
|
94
|
+
"""Normalise a field inside `__post_init__` of a frozen dataclass."""
|
|
95
|
+
object.__setattr__(msg, name, value)
|
|
96
|
+
|
|
97
|
+
|
|
98
|
+
class Msg:
|
|
99
|
+
"""Base of every message. Subclasses are `@dataclass(frozen=True, eq=False, repr=False)`.
|
|
100
|
+
|
|
101
|
+
The base supplies array-aware `==` and `hash`, a compact `repr` that never dumps a large
|
|
102
|
+
array, `replace()`, and a lossless `to_dict()` / `from_dict()`.
|
|
103
|
+
"""
|
|
104
|
+
|
|
105
|
+
__slots__ = ()
|
|
106
|
+
|
|
107
|
+
type_name: ClassVar[str]
|
|
108
|
+
"""Stable name like ``"trajectory/JointTrajectory"``, stored in serialised dicts."""
|
|
109
|
+
|
|
110
|
+
def __init_subclass__(cls, **kwargs: Any) -> None:
|
|
111
|
+
super().__init_subclass__(**kwargs)
|
|
112
|
+
module = cls.__module__.split(".")
|
|
113
|
+
domain = module[1] if module[0] == "robot_msgs" and len(module) > 2 else cls.__module__
|
|
114
|
+
cls.type_name = f"{domain}/{cls.__name__}"
|
|
115
|
+
_REGISTRY[cls.type_name] = cls
|
|
116
|
+
|
|
117
|
+
def __eq__(self, other: object) -> bool:
|
|
118
|
+
if type(other) is not type(self):
|
|
119
|
+
return NotImplemented
|
|
120
|
+
return all(
|
|
121
|
+
values_equal(getattr(self, f.name), getattr(other, f.name))
|
|
122
|
+
for f in dataclasses.fields(self) # type: ignore[arg-type]
|
|
123
|
+
)
|
|
124
|
+
|
|
125
|
+
def __hash__(self) -> int:
|
|
126
|
+
fields = dataclasses.fields(self) # type: ignore[arg-type]
|
|
127
|
+
return hash((type(self), *(_hashable(getattr(self, f.name)) for f in fields)))
|
|
128
|
+
|
|
129
|
+
def __repr__(self) -> str:
|
|
130
|
+
parts = []
|
|
131
|
+
for f in dataclasses.fields(self): # type: ignore[arg-type]
|
|
132
|
+
if not f.repr:
|
|
133
|
+
continue
|
|
134
|
+
value = getattr(self, f.name)
|
|
135
|
+
if _is_unset(value, f):
|
|
136
|
+
continue
|
|
137
|
+
parts.append(f"{f.name}={short_repr(value)}")
|
|
138
|
+
return f"{type(self).__name__}({', '.join(parts)})"
|
|
139
|
+
|
|
140
|
+
def replace(self: M, **changes: Any) -> M:
|
|
141
|
+
"""A copy with some fields changed. Validation runs again."""
|
|
142
|
+
return dataclasses.replace(self, **changes) # type: ignore[type-var]
|
|
143
|
+
|
|
144
|
+
def to_dict(self) -> dict[str, Any]:
|
|
145
|
+
"""JSON-safe dict with a ``"_type"`` key; arrays keep their dtype and shape."""
|
|
146
|
+
return to_dict(self)
|
|
147
|
+
|
|
148
|
+
@classmethod
|
|
149
|
+
def from_dict(cls: type[M], data: Mapping[str, Any]) -> M:
|
|
150
|
+
return from_dict(data, cls)
|
|
151
|
+
|
|
152
|
+
|
|
153
|
+
# ── equality, hashing, repr ──────────────────────────────────────────────────
|
|
154
|
+
|
|
155
|
+
|
|
156
|
+
def values_equal(a: Any, b: Any) -> bool:
|
|
157
|
+
if isinstance(a, np.ndarray) or isinstance(b, np.ndarray):
|
|
158
|
+
if not (isinstance(a, np.ndarray) and isinstance(b, np.ndarray)):
|
|
159
|
+
return False
|
|
160
|
+
if a.shape != b.shape or a.dtype != b.dtype:
|
|
161
|
+
return False
|
|
162
|
+
return bool(np.array_equal(a, b, equal_nan=a.dtype.kind in "fc"))
|
|
163
|
+
if isinstance(a, Mapping) and isinstance(b, Mapping):
|
|
164
|
+
return a.keys() == b.keys() and all(values_equal(a[k], b[k]) for k in a)
|
|
165
|
+
if isinstance(a, (tuple, list)) and isinstance(b, (tuple, list)):
|
|
166
|
+
return len(a) == len(b) and all(values_equal(x, y) for x, y in zip(a, b, strict=True))
|
|
167
|
+
if isinstance(a, float) and isinstance(b, float) and math.isnan(a) and math.isnan(b):
|
|
168
|
+
return True
|
|
169
|
+
return bool(a == b)
|
|
170
|
+
|
|
171
|
+
|
|
172
|
+
def _hashable(value: Any) -> Any:
|
|
173
|
+
if isinstance(value, np.ndarray):
|
|
174
|
+
if value.dtype.kind in "fc":
|
|
175
|
+
value = value + 0.0 # -0.0 → 0.0 so equal arrays hash equally
|
|
176
|
+
value = np.where(np.isnan(value), np.nan, value)
|
|
177
|
+
return (value.shape, value.dtype.str, value.tobytes())
|
|
178
|
+
if isinstance(value, Mapping):
|
|
179
|
+
return tuple(sorted((k, _hashable(v)) for k, v in value.items()))
|
|
180
|
+
if isinstance(value, (tuple, list)):
|
|
181
|
+
return tuple(_hashable(v) for v in value)
|
|
182
|
+
if isinstance(value, float) and math.isnan(value):
|
|
183
|
+
return "nan"
|
|
184
|
+
return value
|
|
185
|
+
|
|
186
|
+
|
|
187
|
+
def _is_unset(value: Any, f: dataclasses.Field) -> bool:
|
|
188
|
+
if value is None:
|
|
189
|
+
return True
|
|
190
|
+
if isinstance(value, (Mapping, tuple)) and not value:
|
|
191
|
+
return True
|
|
192
|
+
if isinstance(value, Msg) and f.default is not dataclasses.MISSING:
|
|
193
|
+
return value == f.default
|
|
194
|
+
if (
|
|
195
|
+
isinstance(value, Msg) and f.default_factory is not dataclasses.MISSING # type: ignore[misc]
|
|
196
|
+
):
|
|
197
|
+
return value == f.default_factory() # type: ignore[misc]
|
|
198
|
+
return False
|
|
199
|
+
|
|
200
|
+
|
|
201
|
+
def short_repr(value: Any) -> str:
|
|
202
|
+
"""repr that summarises big arrays as ``float64[500×6]`` instead of printing them."""
|
|
203
|
+
if isinstance(value, np.ndarray):
|
|
204
|
+
if value.size <= 8:
|
|
205
|
+
return np.array2string(value, precision=4, separator=", ", suppress_small=True)
|
|
206
|
+
return f"{value.dtype}[{'×'.join(str(n) for n in value.shape)}]"
|
|
207
|
+
if isinstance(value, Mapping):
|
|
208
|
+
return "{" + ", ".join(f"{k!r}: {short_repr(v)}" for k, v in value.items()) + "}"
|
|
209
|
+
if isinstance(value, tuple) and len(value) > 6:
|
|
210
|
+
head = ", ".join(short_repr(v) for v in value[:3])
|
|
211
|
+
return f"({head}, … {len(value) - 3} more)"
|
|
212
|
+
if isinstance(value, float):
|
|
213
|
+
# WHY repr, not a rounded format: a stamp like 1761600000.25 must print as itself.
|
|
214
|
+
return repr(value)
|
|
215
|
+
return repr(value)
|
|
216
|
+
|
|
217
|
+
|
|
218
|
+
def allclose(a: Any, b: Any, *, rtol: float = 1e-9, atol: float = 1e-12) -> bool:
|
|
219
|
+
"""Structural approximate equality for messages (same types, numbers within tolerance)."""
|
|
220
|
+
if isinstance(a, Msg) or isinstance(b, Msg):
|
|
221
|
+
if type(a) is not type(b):
|
|
222
|
+
return False
|
|
223
|
+
return all(
|
|
224
|
+
allclose(getattr(a, f.name), getattr(b, f.name), rtol=rtol, atol=atol)
|
|
225
|
+
for f in dataclasses.fields(a) # type: ignore[arg-type]
|
|
226
|
+
)
|
|
227
|
+
if isinstance(a, np.ndarray) or isinstance(b, np.ndarray):
|
|
228
|
+
a, b = np.asarray(a), np.asarray(b)
|
|
229
|
+
if a.shape != b.shape:
|
|
230
|
+
return False
|
|
231
|
+
if a.dtype.kind in "fciu" and b.dtype.kind in "fciu":
|
|
232
|
+
return bool(np.allclose(a, b, rtol=rtol, atol=atol, equal_nan=True))
|
|
233
|
+
return bool(np.array_equal(a, b))
|
|
234
|
+
if isinstance(a, float) or isinstance(b, float):
|
|
235
|
+
return math.isclose(a, b, rel_tol=rtol, abs_tol=atol) or (math.isnan(a) and math.isnan(b))
|
|
236
|
+
if isinstance(a, Mapping) and isinstance(b, Mapping):
|
|
237
|
+
return a.keys() == b.keys() and all(allclose(a[k], b[k], rtol=rtol, atol=atol) for k in a)
|
|
238
|
+
if isinstance(a, (tuple, list)) and isinstance(b, (tuple, list)):
|
|
239
|
+
return len(a) == len(b) and all(
|
|
240
|
+
allclose(x, y, rtol=rtol, atol=atol) for x, y in zip(a, b, strict=True)
|
|
241
|
+
)
|
|
242
|
+
return values_equal(a, b)
|
|
243
|
+
|
|
244
|
+
|
|
245
|
+
# ── dict / JSON / npz round-trips ────────────────────────────────────────────
|
|
246
|
+
|
|
247
|
+
|
|
248
|
+
def to_dict(msg: Msg, *, arrays: dict[str, np.ndarray] | None = None) -> dict[str, Any]:
|
|
249
|
+
"""Encode a message as plain dicts and lists.
|
|
250
|
+
|
|
251
|
+
With `arrays`, array payloads go into that dict (for `save_npz`) and the encoded tree only
|
|
252
|
+
keeps a reference to them.
|
|
253
|
+
"""
|
|
254
|
+
out: dict[str, Any] = {"_type": msg.type_name}
|
|
255
|
+
for f in dataclasses.fields(msg): # type: ignore[arg-type]
|
|
256
|
+
out[f.name] = _encode(getattr(msg, f.name), arrays)
|
|
257
|
+
return out
|
|
258
|
+
|
|
259
|
+
|
|
260
|
+
def _encode(value: Any, arrays: dict[str, np.ndarray] | None) -> Any:
|
|
261
|
+
if isinstance(value, Msg):
|
|
262
|
+
return to_dict(value, arrays=arrays)
|
|
263
|
+
if isinstance(value, np.ndarray):
|
|
264
|
+
if arrays is not None:
|
|
265
|
+
key = f"a{len(arrays)}"
|
|
266
|
+
arrays[key] = value
|
|
267
|
+
return {"npz": key}
|
|
268
|
+
return {
|
|
269
|
+
"dtype": value.dtype.str,
|
|
270
|
+
"shape": list(value.shape),
|
|
271
|
+
"data": value.ravel().tolist(),
|
|
272
|
+
}
|
|
273
|
+
if isinstance(value, Mapping):
|
|
274
|
+
return {str(k): _encode(v, arrays) for k, v in value.items()}
|
|
275
|
+
if isinstance(value, (tuple, list)):
|
|
276
|
+
return [_encode(v, arrays) for v in value]
|
|
277
|
+
if isinstance(value, np.generic):
|
|
278
|
+
return value.item()
|
|
279
|
+
return value
|
|
280
|
+
|
|
281
|
+
|
|
282
|
+
def from_dict(
|
|
283
|
+
data: Mapping[str, Any],
|
|
284
|
+
cls: type[M] | None = None,
|
|
285
|
+
*,
|
|
286
|
+
arrays: Mapping[str, np.ndarray] | None = None,
|
|
287
|
+
) -> M:
|
|
288
|
+
"""Decode what `to_dict` produced. `cls` defaults to the class named by ``"_type"``."""
|
|
289
|
+
target = _REGISTRY[data["_type"]] if "_type" in data else cls
|
|
290
|
+
if target is None:
|
|
291
|
+
raise ValueError("from_dict needs a class or a '_type' key")
|
|
292
|
+
if cls is not None and not issubclass(target, cls):
|
|
293
|
+
raise TypeError(f"{data.get('_type')} is not a {cls.__name__}")
|
|
294
|
+
hints = typing.get_type_hints(target)
|
|
295
|
+
kwargs = {
|
|
296
|
+
f.name: _decode(data[f.name], hints[f.name], arrays)
|
|
297
|
+
for f in dataclasses.fields(target) # type: ignore[arg-type]
|
|
298
|
+
if f.init and f.name in data
|
|
299
|
+
}
|
|
300
|
+
return target(**kwargs) # type: ignore[return-value]
|
|
301
|
+
|
|
302
|
+
|
|
303
|
+
def _decode(value: Any, hint: Any, arrays: Mapping[str, np.ndarray] | None) -> Any:
|
|
304
|
+
if value is None:
|
|
305
|
+
return None
|
|
306
|
+
origin = typing.get_origin(hint)
|
|
307
|
+
args = typing.get_args(hint)
|
|
308
|
+
if origin in (typing.Union, types.UnionType):
|
|
309
|
+
options = [a for a in args if a is not type(None)]
|
|
310
|
+
return _decode(value, options[0], arrays) if len(options) == 1 else value
|
|
311
|
+
if hint is np.ndarray or origin is np.ndarray:
|
|
312
|
+
return _decode_array(value, arrays)
|
|
313
|
+
if isinstance(hint, type) and issubclass(hint, Msg):
|
|
314
|
+
return from_dict(value, hint, arrays=arrays)
|
|
315
|
+
if origin is tuple:
|
|
316
|
+
if len(args) == 2 and args[1] is Ellipsis:
|
|
317
|
+
return tuple(_decode(v, args[0], arrays) for v in value)
|
|
318
|
+
return tuple(_decode(v, a, arrays) for v, a in zip(value, args, strict=True))
|
|
319
|
+
if origin in (dict, Mapping) or origin is getattr(typing, "Mapping", None):
|
|
320
|
+
value_hint = args[1] if len(args) == 2 else Any
|
|
321
|
+
return {k: _decode(v, value_hint, arrays) for k, v in value.items()}
|
|
322
|
+
if hint is Any and isinstance(value, Mapping) and ("npz" in value or "dtype" in value):
|
|
323
|
+
return _decode_array(value, arrays)
|
|
324
|
+
return value
|
|
325
|
+
|
|
326
|
+
|
|
327
|
+
def _decode_array(value: Mapping[str, Any], arrays: Mapping[str, np.ndarray] | None) -> np.ndarray:
|
|
328
|
+
if "npz" in value:
|
|
329
|
+
if arrays is None:
|
|
330
|
+
raise ValueError("array reference without an npz archive")
|
|
331
|
+
return frozen_array(arrays[value["npz"]], None)
|
|
332
|
+
array = np.array(value["data"], dtype=np.dtype(value["dtype"])).reshape(value["shape"])
|
|
333
|
+
return adopt(array)
|
|
334
|
+
|
|
335
|
+
|
|
336
|
+
def save_npz(msg: Msg, path: Any) -> None:
|
|
337
|
+
"""Save a message compactly: arrays as raw npz entries, everything else as JSON."""
|
|
338
|
+
arrays: dict[str, np.ndarray] = {}
|
|
339
|
+
tree = to_dict(msg, arrays=arrays)
|
|
340
|
+
np.savez(path, __msg__=np.array(json.dumps(tree)), **arrays)
|
|
341
|
+
|
|
342
|
+
|
|
343
|
+
def load_npz(path: Any) -> Msg:
|
|
344
|
+
with np.load(path, allow_pickle=False) as archive:
|
|
345
|
+
tree = json.loads(str(archive["__msg__"]))
|
|
346
|
+
arrays = {k: archive[k] for k in archive.files if k != "__msg__"}
|
|
347
|
+
return from_dict(tree, arrays=arrays)
|
|
348
|
+
|
|
349
|
+
|
|
350
|
+
def message_types() -> dict[str, type[Msg]]:
|
|
351
|
+
"""Every registered message type by its `type_name`."""
|
|
352
|
+
return dict(_REGISTRY)
|
|
@@ -0,0 +1,18 @@
|
|
|
1
|
+
"""compare — one trajectory measured against another: the error signal and its numbers.
|
|
2
|
+
|
|
3
|
+
from robot_msgs.compare import compare
|
|
4
|
+
result = compare(target, real) # or target.compare(real)
|
|
5
|
+
result.error # same type as the inputs: reference - measured
|
|
6
|
+
result.metrics["positions/j2"].rms # ErrorStats per joint / axis / channel
|
|
7
|
+
|
|
8
|
+
Pure numpy, like the rest of robot_msgs. Viewers (HTML report, Rerun) live outside and read
|
|
9
|
+
`Comparison` values; see docs/DESIGN.md, "Comparing trajectories".
|
|
10
|
+
"""
|
|
11
|
+
|
|
12
|
+
from robot_msgs.compare._dispatch import compare
|
|
13
|
+
from robot_msgs.compare.align import align
|
|
14
|
+
from robot_msgs.compare.comparison import Comparison
|
|
15
|
+
from robot_msgs.compare.difference import track_errors
|
|
16
|
+
from robot_msgs.compare.error_stats import ErrorStats, error_stats
|
|
17
|
+
|
|
18
|
+
__all__ = ["Comparison", "ErrorStats", "align", "compare", "error_stats", "track_errors"]
|
|
@@ -0,0 +1,92 @@
|
|
|
1
|
+
"""`compare`: one generic function, one implementation per trajectory type (like `viz.plot`)."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from functools import singledispatch
|
|
6
|
+
from typing import Any
|
|
7
|
+
|
|
8
|
+
from robot_msgs.compare.align import align
|
|
9
|
+
from robot_msgs.compare.comparison import Comparison
|
|
10
|
+
from robot_msgs.compare.difference import joint_error, pose_error, trajectory_error
|
|
11
|
+
from robot_msgs.compare.metrics import channel_metrics, joint_metrics, pose_metrics
|
|
12
|
+
from robot_msgs.trajectory import JointTrajectory, PoseTrajectory, Trajectory
|
|
13
|
+
|
|
14
|
+
|
|
15
|
+
def _require(measured: Any, kind: type[Trajectory], reference: Trajectory) -> None:
|
|
16
|
+
if not isinstance(measured, kind):
|
|
17
|
+
raise TypeError(
|
|
18
|
+
f"cannot compare a {type(measured).__name__} against a {type(reference).__name__} "
|
|
19
|
+
f"reference; both must be {kind.__name__}s"
|
|
20
|
+
)
|
|
21
|
+
|
|
22
|
+
|
|
23
|
+
@singledispatch
|
|
24
|
+
def compare(reference: Any, measured: Any, *, time_offset_s: float = 0.0) -> Comparison:
|
|
25
|
+
"""`measured` against `reference`: the error trajectory and its metrics.
|
|
26
|
+
|
|
27
|
+
- ``error = reference - measured`` (ROS 2 ``JointTrajectoryControllerState``), on the
|
|
28
|
+
measured run's own samples inside the reference's span; see `align` for the clock rules.
|
|
29
|
+
- The type picks the analysis: `JointTrajectory` → per-joint errors; `PoseTrajectory` →
|
|
30
|
+
position / rotation / per-axis / lag / cross-track; plain `Trajectory` → shared channels.
|
|
31
|
+
Joint ↔ cartesian is not this function's job: build a `PoseTrajectory` per point you care
|
|
32
|
+
about (``PoseTrajectory.from_matrices(t, fk(q), child_frame_id="L2 tip")``) and compare
|
|
33
|
+
those.
|
|
34
|
+
- Pairwise on purpose: N runs, or sim vs real with no target involved, is a loop.
|
|
35
|
+
"""
|
|
36
|
+
raise TypeError(f"robot_msgs.compare has no comparison for {type(reference).__name__}")
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
@compare.register
|
|
40
|
+
def _compare_trajectory(
|
|
41
|
+
reference: Trajectory, measured: Any, *, time_offset_s: float = 0.0
|
|
42
|
+
) -> Comparison:
|
|
43
|
+
_require(measured, Trajectory, reference)
|
|
44
|
+
reference_at, inside, dropped, coverage = align(
|
|
45
|
+
reference, measured, time_offset_s=time_offset_s
|
|
46
|
+
)
|
|
47
|
+
error = trajectory_error(reference_at, inside)
|
|
48
|
+
return Comparison(
|
|
49
|
+
error=error, metrics=channel_metrics(error), time_offset_s=time_offset_s,
|
|
50
|
+
dropped=dropped, coverage=coverage,
|
|
51
|
+
) # fmt: skip
|
|
52
|
+
|
|
53
|
+
|
|
54
|
+
@compare.register
|
|
55
|
+
def _compare_joints(
|
|
56
|
+
reference: JointTrajectory, measured: Any, *, time_offset_s: float = 0.0
|
|
57
|
+
) -> Comparison:
|
|
58
|
+
_require(measured, JointTrajectory, reference)
|
|
59
|
+
missing = [name for name in reference.joint_names if name not in measured.joint_names]
|
|
60
|
+
extra = [name for name in measured.joint_names if name not in reference.joint_names]
|
|
61
|
+
if missing or extra:
|
|
62
|
+
raise ValueError(
|
|
63
|
+
f"joints differ: measured lacks {missing}, reference lacks {extra}; "
|
|
64
|
+
"use .select(names) on either side to compare a subset"
|
|
65
|
+
)
|
|
66
|
+
reference = reference.select(measured.joint_names) # match by name, in measured's order
|
|
67
|
+
reference_at, inside, dropped, coverage = align(
|
|
68
|
+
reference, measured, time_offset_s=time_offset_s
|
|
69
|
+
)
|
|
70
|
+
error = joint_error(reference_at, inside)
|
|
71
|
+
return Comparison(
|
|
72
|
+
error=error, metrics=joint_metrics(error), time_offset_s=time_offset_s,
|
|
73
|
+
dropped=dropped, coverage=coverage,
|
|
74
|
+
) # fmt: skip
|
|
75
|
+
|
|
76
|
+
|
|
77
|
+
@compare.register
|
|
78
|
+
def _compare_poses(
|
|
79
|
+
reference: PoseTrajectory, measured: Any, *, time_offset_s: float = 0.0
|
|
80
|
+
) -> Comparison:
|
|
81
|
+
_require(measured, PoseTrajectory, reference)
|
|
82
|
+
if reference.linear_velocities is None and len(reference) >= 2:
|
|
83
|
+
# Lag / cross-track need the reference's direction of travel, taken on its own samples.
|
|
84
|
+
reference = reference.with_derivatives()
|
|
85
|
+
reference_at, inside, dropped, coverage = align(
|
|
86
|
+
reference, measured, time_offset_s=time_offset_s
|
|
87
|
+
)
|
|
88
|
+
error = pose_error(reference_at, inside)
|
|
89
|
+
return Comparison(
|
|
90
|
+
error=error, metrics=pose_metrics(error), time_offset_s=time_offset_s,
|
|
91
|
+
dropped=dropped, coverage=coverage,
|
|
92
|
+
) # fmt: skip
|
|
@@ -0,0 +1,53 @@
|
|
|
1
|
+
"""Lining a measured trajectory up with its reference: one clock, no extrapolation."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from typing import TypeVar
|
|
6
|
+
|
|
7
|
+
import numpy as np
|
|
8
|
+
|
|
9
|
+
from robot_msgs.trajectory import Trajectory
|
|
10
|
+
|
|
11
|
+
TrajectoryT = TypeVar("TrajectoryT", bound=Trajectory)
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
def align(
|
|
15
|
+
reference: TrajectoryT, measured: TrajectoryT, *, time_offset_s: float = 0.0
|
|
16
|
+
) -> tuple[TrajectoryT, TrajectoryT, int, float]:
|
|
17
|
+
"""The reference sampled at the measured run's own instants, and those measured samples.
|
|
18
|
+
|
|
19
|
+
Returns ``(reference_at, measured_inside, dropped, coverage)``.
|
|
20
|
+
|
|
21
|
+
- Both clocks are absolute (``header.stamp + t``), so runs recorded with their own stamps line
|
|
22
|
+
up. `time_offset_s` is how late the measured clock runs: measured time ``τ`` is compared
|
|
23
|
+
with reference time ``τ - time_offset_s`` (a camera's latency, a one-tick-stale reply).
|
|
24
|
+
- Measured samples outside the reference's span are dropped, never compared against an
|
|
25
|
+
extrapolated or held reference. WHY: an error measured against a frozen last value is
|
|
26
|
+
invented, not observed (data-ui's ``difference(a, b)`` contract, evo's ``max_diff``).
|
|
27
|
+
- Errors are taken on the measured run's native samples, never on a shared resampled grid:
|
|
28
|
+
a 1 kHz joint's ringing disappears on a 100 Hz grid.
|
|
29
|
+
"""
|
|
30
|
+
reference_times = reference.absolute_times
|
|
31
|
+
measured_times = measured.absolute_times - float(time_offset_s)
|
|
32
|
+
tolerance = 1e-9 * max(1.0, float(np.max(np.abs(reference_times))))
|
|
33
|
+
inside = (measured_times >= reference_times[0] - tolerance) & (
|
|
34
|
+
measured_times <= reference_times[-1] + tolerance
|
|
35
|
+
)
|
|
36
|
+
if not inside.any():
|
|
37
|
+
raise ValueError(
|
|
38
|
+
f"no overlap: measured spans [{measured_times[0]:.6g}, {measured_times[-1]:.6g}] s, "
|
|
39
|
+
f"reference [{reference_times[0]:.6g}, {reference_times[-1]:.6g}] s "
|
|
40
|
+
f"(time_offset_s={time_offset_s})"
|
|
41
|
+
)
|
|
42
|
+
measured_inside = measured[inside]
|
|
43
|
+
query = measured_times[inside] - reference.header.stamp
|
|
44
|
+
reference_at = reference.resample(query, extrapolate="clamp") # clamp only absorbs `tolerance`
|
|
45
|
+
span = reference_times[-1] - reference_times[0]
|
|
46
|
+
if span > 0.0:
|
|
47
|
+
covered = min(measured_times[inside][-1], reference_times[-1]) - max(
|
|
48
|
+
measured_times[inside][0], reference_times[0]
|
|
49
|
+
)
|
|
50
|
+
coverage = float(np.clip(covered / span, 0.0, 1.0))
|
|
51
|
+
else:
|
|
52
|
+
coverage = 1.0
|
|
53
|
+
return reference_at, measured_inside, int(np.count_nonzero(~inside)), coverage
|
|
@@ -0,0 +1,52 @@
|
|
|
1
|
+
"""Comparison: one trajectory measured against a reference — the error signal and its numbers."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from collections.abc import Mapping
|
|
6
|
+
from dataclasses import dataclass, field
|
|
7
|
+
from typing import Any
|
|
8
|
+
|
|
9
|
+
from robot_msgs._core import FrozenDict, Msg, set_field
|
|
10
|
+
from robot_msgs.compare.error_stats import ErrorStats
|
|
11
|
+
from robot_msgs.trajectory import Trajectory
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
@dataclass(frozen=True, eq=False, repr=False, kw_only=True)
|
|
15
|
+
class Comparison(Msg):
|
|
16
|
+
"""What `robot_msgs.compare.compare(reference, measured)` returns.
|
|
17
|
+
|
|
18
|
+
- `error` is a trajectory of the *same type* as the inputs, on the measured run's own
|
|
19
|
+
samples, holding ``reference - measured`` (the ROS 2 controller-state convention): joint
|
|
20
|
+
error is a `JointTrajectory`, pose error a `PoseTrajectory`. Derived error forms (rotation
|
|
21
|
+
vector, lag, cross-track, …) ride along as `channels`. WHY one type, not a parallel
|
|
22
|
+
"error" schema: plots, resampling, slicing and serialisation already work on it.
|
|
23
|
+
- `metrics` maps a stable name (``"positions/j2"``, ``"position"``, ``"lag"``,
|
|
24
|
+
``"channels/closure_gap"``) to its `ErrorStats`.
|
|
25
|
+
- `time_offset_s`, `dropped` and `coverage` record how the two were lined up, so a number is
|
|
26
|
+
never separated from the decision that produced it: the offset applied to the measured
|
|
27
|
+
clock, how many measured samples fell outside the reference (never extrapolated), and the
|
|
28
|
+
fraction of the reference's duration the measured run covers.
|
|
29
|
+
"""
|
|
30
|
+
|
|
31
|
+
error: Trajectory
|
|
32
|
+
metrics: Mapping[str, ErrorStats] = field(default_factory=dict)
|
|
33
|
+
time_offset_s: float = 0.0
|
|
34
|
+
dropped: int = 0
|
|
35
|
+
coverage: float = 1.0
|
|
36
|
+
|
|
37
|
+
def __post_init__(self) -> None:
|
|
38
|
+
if not isinstance(self.error, Trajectory):
|
|
39
|
+
raise TypeError(
|
|
40
|
+
f"Comparison.error must be a Trajectory, got {type(self.error).__name__}"
|
|
41
|
+
)
|
|
42
|
+
for name, stats in self.metrics.items():
|
|
43
|
+
if not isinstance(stats, ErrorStats):
|
|
44
|
+
raise TypeError(f"Comparison.metrics[{name!r}] must be ErrorStats")
|
|
45
|
+
set_field(self, "metrics", FrozenDict({str(k): v for k, v in self.metrics.items()}))
|
|
46
|
+
set_field(self, "time_offset_s", float(self.time_offset_s))
|
|
47
|
+
set_field(self, "dropped", int(self.dropped))
|
|
48
|
+
set_field(self, "coverage", float(self.coverage))
|
|
49
|
+
|
|
50
|
+
def rows(self) -> list[dict[str, Any]]:
|
|
51
|
+
"""The metrics as tidy rows — one dict per metric — for tables and data-ui."""
|
|
52
|
+
return [{"metric": name, **stats.as_dict()} for name, stats in self.metrics.items()]
|