Add the kinematics metrics

autodata_interfaces/embodiments/absolute_pose_embodiment_adapter.py

@@ -83,7 +83,7 @@ def metric_context(self) -> dict[str, Any]:
         These entries are not clipped.
         """
         pose_action = {
-            name: {"action_slice": list(spec["pose_action_slice"]), "bound": None}
+            name: {"action_slice": list(spec["pose_action_slice"]), "bound": None, "absolute": True}
             for name, spec in self.eefs.items()
             if spec.get("pose_action_slice") is not None
         }

autodata_interfaces/embodiments/bimanual_embodiment_adapter.py

@@ -197,8 +197,8 @@ def metric_context(self) -> dict[str, Any]:
         Each holds a position, then a quaternion. These entries are not clipped.
         """
         pose_action = {
-            self.left.name: {"action_slice": list(self.left_pose_slice), "bound": None},
-            self.right.name: {"action_slice": list(self.right_pose_slice), "bound": None},
+            self.left.name: {"action_slice": list(self.left_pose_slice), "bound": None, "absolute": True},
+            self.right.name: {"action_slice": list(self.right_pose_slice), "bound": None, "absolute": True},
         }
         return {**super().metric_context(), "pose_action": pose_action}
 

autodata_interfaces/embodiments/embodiment_adapter.py

@@ -55,8 +55,9 @@ def metric_context(self) -> dict[str, Any]:
 
         The base returns the optional ``hardware_limits`` section of the embodiment config: datasheet joint limits
         keyed by joint-name pattern. Subclasses add ``pose_action``, which says for each end effector which part of
-        the action vector commands its pose (``action_slice``, a half-open ``[start, end)`` index range) and the
-        value the adapter clips those entries to (``bound``, or None when they are not clipped).
+        the action vector commands its pose (``action_slice``, a half-open ``[start, end)`` index range), the
+        value the adapter clips those entries to (``bound``, or None when they are not clipped), and whether they
+        command an absolute target pose or a change of pose (``absolute``).
         """
         hardware_limits = (self.raw_config or {}).get("hardware_limits")
         return {"hardware_limits": hardware_limits} if hardware_limits else {}

autodata_interfaces/embodiments/single_arm_embodiment_adapter.py

@@ -139,7 +139,7 @@ def metric_context(self) -> dict[str, Any]:
         They are clipped to ±1 when ``clip_pose_action_to_unit`` is set.
         """
         bound = 1.0 if self.clip_pose_action_to_unit else None
-        pose_action = {self.eef_name: {"action_slice": [0, _DELTA_POSE_ACTION_DIM], "bound": bound}}
+        pose_action = {self.eef_name: {"action_slice": [0, _DELTA_POSE_ACTION_DIM], "bound": bound, "absolute": False}}
         return {**super().metric_context(), "pose_action": pose_action}
 
     @property

autodata_interfaces/env/isaaclab_env_interface.py

@@ -46,8 +46,8 @@
 def collect_autodata_entry(env: Any, embodiment_adapter: EmbodimentAdapter, run_id: str) -> dict[str, Any]:
     """The ``autodata`` entry of the datasets this env exports, read while the env is alive.
 
-    It holds the run id, the embodiment's metric context, and the robot's joint names with env 0's simulator
-    position limits [rad or m] and velocity limits [rad/s or m/s]. Unbounded limits are stored as None.
+    It holds the run id, the embodiment's metric context, and the robot's asset name and joint names with env 0's
+    simulator position limits [rad or m] and velocity limits [rad/s or m/s]. Unbounded limits are stored as None.
     :func:`~autodata_utils.dataset_metadata.merge_autodata_entry` writes it once the env has closed the file.
 
     Args:
@@ -59,6 +59,7 @@ def collect_autodata_entry(env: Any, embodiment_adapter: EmbodimentAdapter, run_
     robot = env.scene.articulations.get(embodiment_adapter.robot_asset_name)
     if robot is not None:
         entry["joints"] = {
+            "asset": embodiment_adapter.robot_asset_name,
             "names": list(robot.joint_names),
             "position_limits": _finite_or_none(as_torch(robot.data.joint_pos_limits)[0].cpu()),
             "velocity_limits": _finite_or_none(as_torch(robot.data.joint_vel_limits)[0].cpu()),

autodata_metrics/evidence.py

@@ -5,6 +5,7 @@
 
 from __future__ import annotations
 
+import copy
 import h5py
 import json
 import numpy as np
@@ -118,6 +119,26 @@ def has(self, key: str) -> bool:
             return _has_path(self.run, rest)
         return bool(self.episodes) and all(key in episode for episode in self.episodes)
 
+    @property
+    def entry(self) -> Mapping[str, Any]:
+        """The ``autodata`` entry of ``env_args``, or empty."""
+        return self.env_args.get("autodata", {})
+
+    def as_baseline_for(self, run: Evidence) -> Evidence:
+        """This source dataset, with the run's ``autodata`` fields where its own entry lacks them, so a baseline
+        reads the robot layout and limits.
+
+        ``joints`` and ``hardware_limits`` fill in field by field. A ``pose_action`` the source records stays whole,
+        as a channel it leaves out is not a pose action.
+        """
+        baseline = copy.copy(self)
+        entry = {**run.entry, **self.entry}
+        for key in ("joints", "hardware_limits"):
+            if key in self.entry and key in run.entry:
+                entry[key] = {**run.entry[key], **self.entry[key]}
+        baseline.env_args = {**self.env_args, "autodata": entry}
+        return baseline
+
     @cached_property
     def camera_paths(self) -> frozenset[str]:
         """Paths of the camera streams, 4-D ``uint8`` arrays, that any episode records."""

autodata_metrics/records.py

@@ -45,9 +45,9 @@ class MapOf:
 
 
 NUMBER = "a finite number"
-COUNT = "a non-negative integer"
-DURATION = "a non-negative finite number"
+NON_NEGATIVE = "a non-negative finite number"
 POSITIVE = "a positive finite number"
+COUNT = "a non-negative integer"
 
 INPUT = {"path": str, "sha256": str}
 RUN = {
@@ -60,8 +60,8 @@ class MapOf:
     "cpu": Maybe(str),
     "num_gpus": Maybe(COUNT),
     "num_envs": Maybe(COUNT),
-    "setup_time_s": Maybe(DURATION),
-    "generation_time_s": Maybe(DURATION),
+    "setup_time_s": Maybe(NON_NEGATIVE),
+    "generation_time_s": Maybe(NON_NEGATIVE),
     "attempts_file": Maybe(str),
 }
 RESULT = {"num_success": COUNT, "num_failures": Maybe(COUNT), "num_attempts": COUNT, "run": Maybe(RUN)}
@@ -71,32 +71,35 @@ class MapOf:
     "success": bool,
     "start_time_s": NUMBER,
     "end_time_s": NUMBER,
-    "extras": Maybe({"planning_time_s": Maybe(DURATION), "planning_failed": Maybe(bool)}),
+    "extras": Maybe({"planning_time_s": Maybe(NON_NEGATIVE), "planning_failed": Maybe(bool)}),
 }
 ANNOTATION = {
     "mode": Maybe(("auto", "manual")),
     "input_episodes": COUNT,
     "processed_episodes": COUNT,
     "annotated_episodes": COUNT,
-    "setup_time_s": Maybe(DURATION),
-    "time_s": DURATION,
+    "setup_time_s": Maybe(NON_NEGATIVE),
+    "time_s": NON_NEGATIVE,
 }
 ENTRY = {
     "version": COUNT,
     "run_id": str,
     "exported_episodes": COUNT,
-    "pose_action": Maybe(MapOf({"action_slice": ListOf(int, 2), "bound": Maybe(NUMBER)})),
+    "pose_action": Maybe(MapOf({"action_slice": ListOf(COUNT, 2), "bound": Maybe(POSITIVE), "absolute": Maybe(bool)})),
     "annotation": Maybe(ANNOTATION),
+    "action_noise": Maybe(NUMBER),
+    "joints": Maybe({"asset": Maybe(str), "names": ListOf(str)}),
+    "hardware_limits": Maybe({"velocity": Maybe(MapOf(POSITIVE)), "acceleration": Maybe(MapOf(POSITIVE))}),
 }
 ENV_ARGS = {"autodata": Maybe(ENTRY), "sim_args": Maybe({"dt": Maybe(POSITIVE), "decimation": Maybe(POSITIVE)})}
 
 
 def parse(value: Any, shape: Any, where: str) -> Any:
     """``value`` checked against ``shape``, with its absent or null optional fields left out.
 
-    A shape is a type, a tuple of allowed values, :data:`NUMBER`, :data:`COUNT`, :data:`DURATION`, :data:`POSITIVE`,
-    a :class:`Maybe`, :class:`ListOf` or :class:`MapOf`, or a dict of field shapes; a dict's fields are required
-    unless their shape is a :class:`Maybe`.
+    A shape is a type, a tuple of allowed values, :data:`NUMBER`, :data:`NON_NEGATIVE`, :data:`POSITIVE`,
+    :data:`COUNT`, a :class:`Maybe`, :class:`ListOf` or :class:`MapOf`, or a dict of field shapes; a dict's fields
+    are required unless their shape is a :class:`Maybe`.
 
     Raises:
         InvalidRecord: Naming ``where`` and the field, if ``value`` does not have ``shape``.
@@ -123,8 +126,8 @@ def parse(value: Any, shape: Any, where: str) -> Any:
         return {key: item for key, item in parsed.items() if item is not None}
     if isinstance(shape, tuple):
         _require(value in shape, value, f"one of {list(shape)}", where)
-    elif shape in (NUMBER, DURATION, POSITIVE):
-        in_range = is_finite_number(value) and (shape is NUMBER or value > 0 or (shape is DURATION and value == 0))
+    elif shape in (NUMBER, NON_NEGATIVE, POSITIVE):
+        in_range = is_finite_number(value) and (shape is NUMBER or value > 0 or (shape is NON_NEGATIVE and value == 0))
         _require(in_range, value, shape, where)
     elif shape is COUNT:
         _require(isinstance(value, int) and not isinstance(value, bool) and value >= 0, value, shape, where)
@@ -141,6 +144,9 @@ def parse_env_args(env_args: Any, where: str) -> dict[str, Any]:
     if isinstance(version, int) and version > AUTODATA_ENTRY_VERSION:
         raise InvalidRecord(f"{where}.autodata was written by a newer AutoData (entry version {version}).")
     parsed = parse(env_args, ENV_ARGS, where)
+    for eef, layout in parsed.get("autodata", {}).get("pose_action", {}).items():
+        start, end = layout["action_slice"]
+        _require(start < end, [start, end], "a non-empty range", f"{where}.autodata.pose_action.{eef}.action_slice")
     annotation = parsed.get("autodata", {}).get("annotation")
     if annotation is not None:
         counts = [annotation[name] for name in ("annotated_episodes", "processed_episodes", "input_episodes")]

autodata_metrics/report.py

@@ -182,7 +182,7 @@ def evaluate(spec: MetricSpec, evidence: Evidence) -> dict[str, Any]:
         "details": _finite_or_none(value.details),
     }
     if spec.baseline and evidence.source is not None:
-        row["baseline"] = evaluate(spec, evidence.source).get("value")
+        row["baseline"] = evaluate(spec, evidence.source.as_baseline_for(evidence)).get("value")
     return row
 
 

autodata_metrics/validity.py

@@ -8,6 +8,7 @@
 import h5py
 import math
 import numpy as np
+import re
 
 from autodata_metrics.evidence import Episode, Evidence
 from autodata_metrics.registry import NotApplicable, Value, check, metric
@@ -26,6 +27,9 @@
 """Annotation's per-step subtask signals, stored as a ``(steps,)`` column or a ``(1, steps)`` row."""
 OBSERVED_POSE_PATHS = ("obs/datagen_info/eef_pose/", "obs/datagen_info/object_pose/")
 """Recorded poses that are observations; commanded targets are measured by ``action_rotation_error`` instead."""
+NEAR_LIMIT = 0.95
+"""Share of a datasheet limit from which an episode counts as running at the limit, which a simulator that clamps
+there never exceeds."""
 
 
 def _image_streams(episode: Episode, ev: Evidence) -> dict[str, h5py.Dataset]:
@@ -109,10 +113,17 @@ def valid_rotations(episode: Episode, ev: Evidence) -> str | None:
 
 @check("pose_action_bounds", requires=["env_args.autodata.pose_action"])
 def pose_action_bounds(episode: Episode, ev: Evidence) -> str | None:
-    """Fails an episode whose pose actions exceed the bound the embodiment clips them to."""
+    """Fails an episode whose pose actions exceed the bound the embodiment clips them to, or lie beyond its actions."""
     actions = episode["actions"]
+    outside = [
+        f"{eef} {layout['action_slice']}"
+        for eef, layout in ev.entry["pose_action"].items()
+        if layout["action_slice"][1] > actions.shape[-1]
+    ]
+    if outside:
+        return f"pose action slices beyond the {actions.shape[-1]} action columns: {', '.join(outside)}"
     bad = []
-    for eef, layout in ev.env_args["autodata"]["pose_action"].items():
+    for eef, layout in ev.entry["pose_action"].items():
         if layout.get("bound") is None:
             continue
         start, end = layout["action_slice"]
@@ -235,12 +246,156 @@ def action_rotation_error(ev: Evidence) -> Value:
     """p50 and p95 deviation of the commanded target rotations from a valid rotation, over every step and end effector."""
     errors = []
     for episode in ev.episodes:
-        for eef in episode.children("obs/datagen_info/target_eef_pose"):
+        for eef in _pose_channels(ev, episode, "obs/datagen_info/target_eef_pose"):
             targets = episode[f"obs/datagen_info/target_eef_pose/{eef}"]
-            # A channel that commands no pose records all-zero targets, which are not rotations.
-            errors.append(_rotation_errors(targets[np.abs(targets).reshape(len(targets), -1).max(axis=1) > 0]))
-    errors = np.concatenate(errors) if errors else np.empty(0)
-    if not len(errors):
-        raise NotApplicable("no end effector commands a pose")
-    p50, p95 = np.percentile(errors, [50, 95])
-    return Value({"p50": float(p50), "p95": float(p95)}, n=len(errors))
+            errors.append(_rotation_errors(targets[_commanded(targets)]))
+    return _percentiles(errors, "no end effector commands a pose")
+
+
+@metric(
+    "tracking_error",
+    area="validity",
+    signal="motion feasibility",
+    scope="dataset",
+    unit="mm",
+    sample="poses",
+    better="lower",
+    primary="p95",
+    requires=["env_args.autodata.pose_action", "obs/datagen_info/eef_pose", "obs/datagen_info/target_eef_pose"],
+    baseline=True,
+)
+def tracking_error(ev: Evidence) -> Value:
+    """p50 and p95 distance from each absolute end-effector target to the end effector one step later."""
+    absolute = [eef for eef, layout in ev.entry["pose_action"].items() if layout.get("absolute")]
+    distances = []
+    for episode in ev.episodes:
+        for eef in absolute:
+            target, observed = f"obs/datagen_info/target_eef_pose/{eef}", f"obs/datagen_info/eef_pose/{eef}"
+            if target not in episode or observed not in episode:
+                continue
+            targets, reached = episode[target][:-1], episode[observed][1:]
+            commanded = _commanded(targets)
+            distances.append(np.linalg.norm(targets[commanded, :3, 3] - reached[commanded, :3, 3], axis=1) * 1000)
+    return _percentiles(distances, "no pose action is recorded as absolute")
+
+
+@metric(
+    "action_saturation",
+    area="validity",
+    signal="motion feasibility",
+    scope="dataset",
+    unit="fraction",
+    sample="steps",
+    better="lower",
+    requires=["env_args.autodata.pose_action"],
+    baseline=True,
+)
+def action_saturation(ev: Evidence) -> Value:
+    """Share of steps at which a pose action sits at the bound the embodiment clips it to."""
+    layouts = [layout for layout in ev.entry["pose_action"].values() if "bound" in layout]
+    if not layouts:
+        raise NotApplicable("no pose action is clipped")
+    saturated = []
+    for episode in ev.episodes:
+        actions = np.abs(episode["actions"])
+        at_bound = [
+            actions[:, slice(*layout["action_slice"])].max(axis=1, initial=0.0) >= layout["bound"] * (1 - 1e-6)
+            for layout in layouts
+        ]
+        saturated.append(np.any(at_bound, axis=0))
+    saturated = np.concatenate(saturated)
+    return Value(float(saturated.mean()), n=len(saturated))
+
+
+@metric(
+    "motion_smoothness",
+    area="validity",
+    signal="motion quality",
+    scope="dataset",
+    unit="m/s³",
+    sample="poses",
+    better="lower",
+    primary="p95",
+    requires=["obs/datagen_info/eef_pose", "env_args.sim_args.dt", "env_args.sim_args.decimation"],
+    baseline=True,
+)
+def motion_smoothness(ev: Evidence) -> Value:
+    """p50 and p95 end-effector jerk, labelled with the run's largest action-noise scale."""
+    step = ev.env_args["sim_args"]["dt"] * ev.env_args["sim_args"]["decimation"]
+    jerks = [
+        np.linalg.norm(np.diff(episode[f"obs/datagen_info/eef_pose/{eef}"][:, :3, 3], n=3, axis=0), axis=1) / step**3
+        for episode in ev.episodes
+        for eef in _pose_channels(ev, episode, "obs/datagen_info/eef_pose")
+    ]
+    noise = {"action_noise": ev.entry.get("action_noise")}
+    return _percentiles(jerks, "no end effector that commands a pose records four steps", noise)
+
+
+@metric(
+    "joint_limit_ratio",
+    area="validity",
+    signal="motion feasibility",
+    scope="dataset",
+    unit="fraction of episodes",
+    sample="episodes",
+    better="lower",
+    primary="velocity",
+    requires=["env_args.autodata.joints.asset", "env_args.sim_args.dt", "env_args.sim_args.decimation"],
+    baseline=True,
+)
+def joint_limit_ratio(ev: Evidence) -> Value:
+    """Share of episodes that reach ``NEAR_LIMIT`` of a datasheet velocity or acceleration limit."""
+    joints, datasheet = ev.entry["joints"], ev.entry.get("hardware_limits", {})
+    limits = {kind: _datasheet(joints["names"], datasheet.get(kind, {})) for kind in ("velocity", "acceleration")}
+    limits = {kind: limit for kind, limit in limits.items() if np.isfinite(limit).any()}
+    if not limits:
+        raise NotApplicable("the embodiment config gives no hardware_limits for these joints")
+    path = f"states/articulation/{joints['asset']}/joint_velocity"
+    if not all(path in episode for episode in ev.episodes):
+        raise NotApplicable(f"not every episode records {path}")
+    step = ev.env_args["sim_args"]["dt"] * ev.env_args["sim_args"]["decimation"]
+    peaks = {kind: [] for kind in limits}
+    for episode in ev.episodes:
+        velocity = episode[path]
+        if velocity.shape[1:] != (len(joints["names"]),):
+            raise ValueError(
+                f"{episode.name} records joint velocities of shape {velocity.shape} for {len(joints['names'])} joints"
+            )
+        motion = {"velocity": velocity, "acceleration": np.diff(velocity, axis=0) / step}
+        for kind, limit in limits.items():
+            peaks[kind].append((np.abs(motion[kind]) / limit).max(axis=0, initial=0.0))
+    value, details = {}, {}
+    for kind, per_episode in peaks.items():
+        per_episode = np.array(per_episode)
+        value[kind] = float((per_episode.max(axis=1) >= NEAR_LIMIT).mean())
+        details[f"{kind}_peak_ratio"] = float(per_episode.max())
+        per_joint = np.where(np.isfinite(limits[kind]), per_episode.max(axis=0), -1.0)
+        details[f"{kind}_worst_joint"] = joints["names"][int(per_joint.argmax())]
+    return Value(value, n=len(ev.episodes), details=details)
+
+
+def _pose_channels(ev: Evidence, episode: Episode, group: str) -> list[str]:
+    """The end effectors under ``group`` that command a pose action, or every one when the entry does not say."""
+    commanding = ev.entry.get("pose_action")
+    return [eef for eef in episode.children(group) if commanding is None or eef in commanding]
+
+
+def _commanded(targets: np.ndarray) -> np.ndarray:
+    """Which steps of ``targets`` hold a pose: all-zero targets mark steps or channels that command none."""
+    return np.abs(targets).max(axis=tuple(range(1, targets.ndim))) > 0
+
+
+def _percentiles(samples: list[np.ndarray], empty: str, details: dict | None = None) -> Value:
+    values = np.concatenate(samples) if samples else np.empty(0)
+    if not len(values):
+        raise NotApplicable(empty)
+    p50, p95 = np.percentile(values, [50, 95])
+    return Value({"p50": float(p50), "p95": float(p95)}, n=len(values), details=details or {})
+
+
+def _datasheet(names: list[str], limits: dict[str, float]) -> np.ndarray:
+    """Each joint's limit from the first pattern its name fully matches, infinite where none does."""
+    return np.array(
+        [next((limit for pattern, limit in limits.items() if re.fullmatch(pattern, name)), np.inf) for name in names],
+        dtype=float,
+    )