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.
Files changed (61) hide show
  1. robot_msgs/__init__.py +29 -0
  2. robot_msgs/_core.py +352 -0
  3. robot_msgs/compare/__init__.py +18 -0
  4. robot_msgs/compare/_dispatch.py +92 -0
  5. robot_msgs/compare/align.py +53 -0
  6. robot_msgs/compare/comparison.py +52 -0
  7. robot_msgs/compare/difference.py +116 -0
  8. robot_msgs/compare/error_stats.py +62 -0
  9. robot_msgs/compare/metrics.py +66 -0
  10. robot_msgs/geometry/__init__.py +31 -0
  11. robot_msgs/geometry/accel.py +52 -0
  12. robot_msgs/geometry/inertia.py +99 -0
  13. robot_msgs/geometry/point.py +79 -0
  14. robot_msgs/geometry/pose.py +90 -0
  15. robot_msgs/geometry/pose2d.py +33 -0
  16. robot_msgs/geometry/pose_stamped.py +46 -0
  17. robot_msgs/geometry/quaternion.py +177 -0
  18. robot_msgs/geometry/rotations.py +251 -0
  19. robot_msgs/geometry/transform.py +125 -0
  20. robot_msgs/geometry/transform_stamped.py +32 -0
  21. robot_msgs/geometry/twist.py +52 -0
  22. robot_msgs/geometry/vector3.py +115 -0
  23. robot_msgs/geometry/wrench.py +52 -0
  24. robot_msgs/ros/__init__.py +19 -0
  25. robot_msgs/ros/bag.py +57 -0
  26. robot_msgs/ros/convert.py +939 -0
  27. robot_msgs/ros/vision_msgs.py +46 -0
  28. robot_msgs/sensor/__init__.py +9 -0
  29. robot_msgs/sensor/camera_info.py +135 -0
  30. robot_msgs/sensor/image.py +151 -0
  31. robot_msgs/sensor/imu.py +36 -0
  32. robot_msgs/sensor/joint_state.py +69 -0
  33. robot_msgs/sensor/point_cloud.py +116 -0
  34. robot_msgs/std/__init__.py +5 -0
  35. robot_msgs/std/header.py +81 -0
  36. robot_msgs/trajectory/__init__.py +29 -0
  37. robot_msgs/trajectory/joint_limits.py +65 -0
  38. robot_msgs/trajectory/joint_trajectory.py +208 -0
  39. robot_msgs/trajectory/pose_trajectory.py +175 -0
  40. robot_msgs/trajectory/time_repair.py +145 -0
  41. robot_msgs/trajectory/trajectory.py +567 -0
  42. robot_msgs/vision/__init__.py +21 -0
  43. robot_msgs/vision/bounding_box2d.py +68 -0
  44. robot_msgs/vision/bounding_box3d.py +50 -0
  45. robot_msgs/vision/detection2d.py +43 -0
  46. robot_msgs/vision/detection2d_array.py +80 -0
  47. robot_msgs/vision/detection3d.py +43 -0
  48. robot_msgs/vision/detection3d_array.py +104 -0
  49. robot_msgs/vision/object_hypothesis.py +23 -0
  50. robot_msgs/vision/object_hypothesis_with_pose.py +29 -0
  51. robot_msgs/viz/__init__.py +21 -0
  52. robot_msgs/viz/_common.py +167 -0
  53. robot_msgs/viz/_dispatch.py +20 -0
  54. robot_msgs/viz/images.py +215 -0
  55. robot_msgs/viz/movie.py +280 -0
  56. robot_msgs/viz/spatial.py +277 -0
  57. robot_msgs/viz/trajectory.py +525 -0
  58. robot_msgs-0.1.0.dist-info/METADATA +122 -0
  59. robot_msgs-0.1.0.dist-info/RECORD +61 -0
  60. robot_msgs-0.1.0.dist-info/WHEEL +4 -0
  61. 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()]