Measure dynamic limits and mark nominal timing in the NMG benchmark

scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md

@@ -139,10 +139,23 @@ physical task success.
 - updates rollout state through FK, which is a kinematic model loop rather than
   sensor-driven simulation or real-robot recovery;
 - reports a fixed nominal `dt`, with velocity and acceleration estimated from
-  joint-position finite differences;
+  joint-position finite differences. `PlanResult` requires `dt` whenever it
+  carries positions, so the nominal value cannot be withheld; adapters instead
+  declare `native_timing`, and a row that does not solve timing reports no
+  comparable duration. The default is nominal: only cuRobo and TOPPRA declare
+  solved timing, while IK interpolation assigns a configured constant interval
+  and stays nominal like NMG;
 - has no explicit obstacle, collision, multimodal-sampling, or APG-refinement
   input/output interface yet.
 
+The simulator does not carry the asset's joint velocity limits: a measured
+Franka run reports the float32 maximum on all seven joints, and effort limits
+as a flat 1e5, rather than the URDF's 2.62-5.26 rad/s and 12-87 Nm. A limit at
+that magnitude is a placeholder, so the suites state the published FR3 values
+and the metrics treat an unusable asset limit as not applicable rather than as
+satisfied. A velocity check that silently always passes is worse than one
+reported as unavailable.
+
 The v1 benchmark must run within these constraints while reserving
 capability-gated tracks for future features. An unsupported capability must be
 reported as `unsupported`, not silently converted into success or failure.
@@ -161,7 +174,7 @@ However, it currently uses one Franka start state, fixed waypoint offsets, and
 one environment. Repeating a deterministic case mostly measures runtime
 variance, not workspace coverage or generalization. It also lacks:
 
-- joint-limit, velocity, acceleration, jerk, collision, and clearance checks;
+- collision and clearance checks;
 - path length, path efficiency, and smoothness;
 - physics execution and controller tracking;
 - Atomic Action task completion;
@@ -560,6 +573,14 @@ not select unrelated best samples.
 - time-indexed integrated squared acceleration and jerk;
 - endpoint settling error and hold stability.
 
+Velocity, acceleration and jerk are recomputed from each planner's positions
+and reported timing with compact divided differences, not read from the
+planner. A central difference averages neighboring steps and reads a path that
+reverses every step as nearly at rest; the compact second difference equals the
+true acceleration somewhere in its window, so it neither overstates nor hides
+the peak. A derivative the path is too short to resolve is reported as not
+applicable rather than zero.
+
 Define `motion_valid` independently:
 
 ```text

scripts/benchmark/motion_generation/aggregation.py

@@ -38,6 +38,26 @@ def _mean(values: Iterable[float | None]) -> float | None:
     return sum(finite) / len(finite) if finite else None
 
 
+def _mean_unbounded(values: Iterable[float | None]) -> float | None:
+    """Return the mean of measured values, keeping an unbounded one unbounded.
+
+    A position change across a zero interval is recorded as an infinite peak.
+    :func:`_mean` would drop it as non-finite and summarize the remaining
+    trajectories as if it had never happened, so here any infinite value makes
+    the mean infinite. NaN still carries no measurement and is ignored.
+    """
+    measured = [
+        float(value)
+        for value in values
+        if value is not None and not math.isnan(float(value))
+    ]
+    if not measured:
+        return None
+    if any(math.isinf(value) for value in measured):
+        return math.inf
+    return sum(measured) / len(measured)
+
+
 def _case_macro_rate(
     measured: list[TrialRecord],
     track_cases: list[BenchmarkCase],
@@ -70,6 +90,42 @@ def _case_macro_rate(
     return sum(case_rates) / len(case_rates)
 
 
+def _case_macro_rate_optional(
+    measured: list[TrialRecord],
+    track_cases: list[BenchmarkCase],
+    attribute: str,
+) -> float | None:
+    """Macro-average a tri-state outcome attribute, reporting N/A as ``None``.
+
+    Unlike :func:`_case_macro_rate`, a ``None`` outcome means the check did not
+    apply rather than that it passed, so it is excluded from the average and a
+    case with no applicable outcome is skipped entirely. When no case applies
+    the metric is ``None``, which keeps an unconfigured limit from being
+    reported as a zero violation rate.
+    """
+    if not track_cases:
+        return None
+
+    outcomes_by_case: dict[str, list[CaseOutcome]] = defaultdict(list)
+    for record in measured:
+        outcomes_by_case[record.case_id].extend(record.outcomes)
+
+    case_rates: list[float] = []
+    for case in track_cases:
+        applicable = [
+            outcome
+            for outcome in outcomes_by_case.get(case.case_id, [])
+            if getattr(outcome, attribute) is not None
+        ]
+        if not applicable:
+            continue
+        case_rates.append(
+            sum(bool(getattr(outcome, attribute)) for outcome in applicable)
+            / len(applicable)
+        )
+    return sum(case_rates) / len(case_rates) if case_rates else None
+
+
 def _case_supports_attribute(case: BenchmarkCase, attribute: str) -> bool:
     """Return whether a staged outcome applies to one case protocol."""
     if attribute == "task_success":
@@ -325,8 +381,13 @@ def _performance_rows(
                 "end_to_end_time_ms": _mean(
                     record.end_to_end_time_ms for record in group
                 ),
-                "trajectory_duration_s": _mean(
-                    record.trajectory_duration_s for record in group
+                "native_timing": info.native_timing,
+                # A planner that never solves timing reports a nominal dt, so
+                # its duration is not comparable with a solved one.
+                "trajectory_duration_s": (
+                    _mean(record.trajectory_duration_s for record in group)
+                    if info.native_timing
+                    else None
                 ),
                 "trajectory_waypoints": _mean(
                     (
@@ -589,6 +650,38 @@ def _metric_rows(
                     "joint_violation_rate": _case_macro_rate(
                         measured, group_cases, "joint_limit_violation"
                     ),
+                    "velocity_violation_rate": _case_macro_rate_optional(
+                        measured, group_cases, "velocity_limit_violation"
+                    ),
+                    "acceleration_violation_rate": _case_macro_rate_optional(
+                        measured, group_cases, "acceleration_limit_violation"
+                    ),
+                    "jerk_violation_rate": _case_macro_rate_optional(
+                        measured, group_cases, "jerk_limit_violation"
+                    ),
+                    # Dynamics are summarized over every measured outcome, not
+                    # the motion-valid subset: exceeding a limit can make an
+                    # outcome invalid, so restricting the population here would
+                    # drop exactly the trajectories with the largest peaks and
+                    # pair a modest peak with a nonzero violation rate.
+                    "max_joint_velocity_rad_s": _mean_unbounded(
+                        outcome.max_joint_velocity_rad_s for outcome in outcomes
+                    ),
+                    "max_joint_acceleration_rad_s2": _mean_unbounded(
+                        outcome.max_joint_acceleration_rad_s2 for outcome in outcomes
+                    ),
+                    "max_joint_jerk_rad_s3": _mean_unbounded(
+                        outcome.max_joint_jerk_rad_s3 for outcome in outcomes
+                    ),
+                    "velocity_utilization": _mean_unbounded(
+                        outcome.velocity_utilization for outcome in outcomes
+                    ),
+                    "acceleration_utilization": _mean_unbounded(
+                        outcome.acceleration_utilization for outcome in outcomes
+                    ),
+                    "jerk_utilization": _mean_unbounded(
+                        outcome.jerk_utilization for outcome in outcomes
+                    ),
                     "joint_path_length_rad": _mean(
                         outcome.joint_path_length_rad for outcome in valid_outcomes
                     ),

scripts/benchmark/motion_generation/config.py

@@ -111,6 +111,28 @@ class ProtocolCfg:
     position_threshold_m: float = 0.01
     rotation_threshold_rad: float = 0.1
     joint_limit_tolerance_rad: float = 1.0e-5
+    joint_velocity_limit_rad_s: float | list[float] | None = None
+    """Per-joint or scalar velocity limit overriding the asset's.
+
+    The asset is the preferred source, but a backend that does not carry the
+    URDF's limits reports a placeholder instead, which would make the velocity
+    check vacuous. Stating the limit here restores it. FR3 publishes
+    2.62 / 2.62 / 2.62 / 2.62 / 5.26 / 4.18 / 5.26 rad/s.
+    """
+    joint_acceleration_limit_rad_s2: float | list[float] | None = None
+    """Per-joint or scalar acceleration limit, or ``None`` to report N/A.
+
+    URDF carries no acceleration field and :class:`Robot` exposes no
+    ``qacc_limits``, so this limit has no asset source and must be stated by the
+    suite. FR3 publishes 10 rad/s^2 uniformly across its seven joints.
+    """
+    joint_jerk_limit_rad_s3: float | list[float] | None = None
+    """Per-joint or scalar jerk limit, or ``None`` to report N/A.
+
+    FR3 publishes 5000 rad/s^3 uniformly across its seven joints.
+    """
+    dynamic_limit_tolerance: float = 1.0e-3
+    """Relative slack on dynamic-limit utilization before a violation is flagged."""
 
 
 @configclass
@@ -237,6 +259,33 @@ def validate_benchmark(self) -> None:
             raise ValueError("rotation_threshold_rad must be > 0.")
         if self.protocol.joint_limit_tolerance_rad < 0.0:
             raise ValueError("joint_limit_tolerance_rad must be >= 0.")
+        for field_name in (
+            "joint_velocity_limit_rad_s",
+            "joint_acceleration_limit_rad_s2",
+            "joint_jerk_limit_rad_s3",
+        ):
+            limit = getattr(self.protocol, field_name)
+            if limit is None:
+                continue
+            values = limit if isinstance(limit, (list, tuple)) else [limit]
+            if not values or any(
+                not isinstance(value, (int, float))
+                or isinstance(value, bool)
+                or not math.isfinite(float(value))
+                or float(value) <= 0.0
+                for value in values
+            ):
+                raise ValueError(f"{field_name} entries must be finite and > 0.")
+        # NaN fails every comparison, so `tolerance < 0` would let it through
+        # and `utilization > 1 + nan` would then never report a violation.
+        tolerance = self.protocol.dynamic_limit_tolerance
+        if (
+            not isinstance(tolerance, (int, float))
+            or isinstance(tolerance, bool)
+            or not math.isfinite(float(tolerance))
+            or float(tolerance) < 0.0
+        ):
+            raise ValueError("dynamic_limit_tolerance must be finite and >= 0.")
         if any(track.scenario == _FREE_SPACE_SCENARIO for track in self.tracks):
             _validate_free_space(self.free_space)
         for track in self.tracks: