From b68f3de83b20945417f3782a5f165736a2212c6e Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 3 Aug 2026 09:02:44 +0000 Subject: [PATCH 01/17] feat(benchmark): add free-space motion generation suite Refactor the neural planner benchmark into configurable adapters, fixed scenarios, external metrics, artifacts, aggregation, and reporting. Add cuRobo as the primary baseline, optional IK/TOPPRA diagnostics, and a configurable NMG stub. --- .../lab/sim/planners/curobo/curobo_planner.py | 48 + pyproject.toml | 1 + scripts/benchmark/__main__.py | 86 +- .../planners/neural_planner/__init__.py | 21 + .../planners/neural_planner/aggregation.py | 348 ++++ .../planners/neural_planner/artifacts.py | 160 ++ .../planners/neural_planner/compat.py | 159 ++ .../planners/neural_planner/config.py | 226 +++ .../neural_planner/metrics/__init__.py | 36 + .../neural_planner/metrics/performance.py | 85 + .../neural_planner/metrics/trajectory.py | 487 ++++++ .../planners/neural_planner/models.py | 147 ++ .../neural_planner/planners/__init__.py | 34 + .../planners/neural_planner/planners/base.py | 92 ++ .../neural_planner/planners/curobo.py | 143 ++ .../neural_planner/planners/ik_interpolate.py | 76 + .../neural_planner/planners/neural.py | 55 + .../neural_planner/planners/toppra.py | 91 ++ .../planners/neural_planner/registry.py | 62 + .../planners/neural_planner/reporting.py | 156 ++ .../planners/neural_planner/run_benchmark.py | 1426 +++-------------- .../planners/neural_planner/runner.py | 456 ++++++ .../neural_planner/scenarios/__init__.py | 23 + .../neural_planner/scenarios/free_space.py | 216 +++ .../neural_planner/suites/coverage.yaml | 62 + .../planners/neural_planner/suites/smoke.yaml | 62 + .../test_motion_generation_benchmark.py | 317 ++++ 27 files changed, 3800 insertions(+), 1275 deletions(-) create mode 100644 scripts/benchmark/planners/neural_planner/__init__.py create mode 100644 scripts/benchmark/planners/neural_planner/aggregation.py create mode 100644 scripts/benchmark/planners/neural_planner/artifacts.py create mode 100644 scripts/benchmark/planners/neural_planner/compat.py create mode 100644 scripts/benchmark/planners/neural_planner/config.py create mode 100644 scripts/benchmark/planners/neural_planner/metrics/__init__.py create mode 100644 scripts/benchmark/planners/neural_planner/metrics/performance.py create mode 100644 scripts/benchmark/planners/neural_planner/metrics/trajectory.py create mode 100644 scripts/benchmark/planners/neural_planner/models.py create mode 100644 scripts/benchmark/planners/neural_planner/planners/__init__.py create mode 100644 scripts/benchmark/planners/neural_planner/planners/base.py create mode 100644 scripts/benchmark/planners/neural_planner/planners/curobo.py create mode 100644 scripts/benchmark/planners/neural_planner/planners/ik_interpolate.py create mode 100644 scripts/benchmark/planners/neural_planner/planners/neural.py create mode 100644 scripts/benchmark/planners/neural_planner/planners/toppra.py create mode 100644 scripts/benchmark/planners/neural_planner/registry.py create mode 100644 scripts/benchmark/planners/neural_planner/reporting.py create mode 100644 scripts/benchmark/planners/neural_planner/runner.py create mode 100644 scripts/benchmark/planners/neural_planner/scenarios/__init__.py create mode 100644 scripts/benchmark/planners/neural_planner/scenarios/free_space.py create mode 100644 scripts/benchmark/planners/neural_planner/suites/coverage.yaml create mode 100644 scripts/benchmark/planners/neural_planner/suites/smoke.yaml create mode 100644 tests/benchmark/planners/test_motion_generation_benchmark.py diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index 47b9f5f7b..ccdf1a8de 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -799,6 +799,54 @@ def with_motion_context( options.control_part = control_part return options + def prepare_backend( + self, + *, + control_part: str, + batch_size: int, + move_type: MoveType = MoveType.EEF_MOVE, + ) -> dict[str, object]: + """Materialize and warm one lazy cuRobo backend without planning a case. + + This explicit lifecycle hook lets deployment tooling and benchmarks + separate one-time robot/world YAML generation, collision-sphere setup, + CUDA graph capture, and cuRobo warmup from the first real planning call. + Repeated calls for the same backend key reuse the cached backend. + + Args: + control_part: Robot control part to prepare. + batch_size: Goal batch size used by the future planning calls. + move_type: Goal type whose cuRobo buffers and graph are prepared. + + Returns: + Metadata describing the resolved backend and actual CUDA graph mode. + + Raises: + ValueError: If the batch size or move type is unsupported. + """ + if batch_size < 1: + logger.log_error("batch_size must be >= 1.", ValueError) + if move_type not in self.supported_move_types: + logger.log_error( + f"cuRobo cannot prepare unsupported move type {move_type}.", + ValueError, + ) + robot_batch_size = int(getattr(self.robot, "num_instances", 1)) + if batch_size not in (1, robot_batch_size): + logger.log_error( + f"batch_size={batch_size} must be 1 or robot.num_instances=" + f"{robot_batch_size}.", + ValueError, + ) + backend = self._get_backend(control_part, batch_size, move_type) + return { + "control_part": backend.control_part, + "batch_size": backend.batch_size, + "move_type": backend.planning_mode.name, + "multi_env": bool(self.cfg.world.multi_env), + "use_cuda_graph": backend.use_cuda_graph, + } + @validate_plan_options(options_cls=CuroboPlanOptions) def plan( self, diff --git a/pyproject.toml b/pyproject.toml index e7542405b..6f4952ecf 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -43,6 +43,7 @@ dependencies = [ "tensorboard>=2.20.0", "ortools", "prettytable", + "psutil>=5.9", "black==26.3.1", "fvcore", "h5py", diff --git a/scripts/benchmark/__main__.py b/scripts/benchmark/__main__.py index 07aab4930..e25ec2463 100644 --- a/scripts/benchmark/__main__.py +++ b/scripts/benchmark/__main__.py @@ -51,23 +51,12 @@ def _run_rl_cli(_: argparse.Namespace) -> None: def _run_neural_planner_cli(args: argparse.Namespace) -> None: - """Run NeuralPlanner benchmark with forwarded CLI args.""" + """Run the free-space motion-generation benchmark.""" from scripts.benchmark.planners.neural_planner.run_benchmark import ( - run_all_benchmarks, + run_from_args, ) - run_all_benchmarks( - num_waypoints_list=args.num_waypoints, - sim_device=args.device, - headless=args.headless, - checkpoint_path=args.checkpoint_path, - num_trials=args.num_trials, - warmup_trials=args.warmup_trials, - sample_interval=args.sample_interval, - compare_ik=args.compare_ik, - compare_toppra=args.compare_toppra, - include_trial_details=args.save_trial_details, - ) + run_from_args(args) def _run_atomic_action_cli(_: argparse.Namespace) -> None: @@ -127,72 +116,13 @@ def main(argv: Sequence[str] | None = None) -> None: # -- planners-neural-planner -------------------------------------------- neural_planner_parser = subparsers.add_parser( "planners-neural-planner", - help="Benchmark NeuralPlanner planning latency and quality on Franka.", - ) - neural_planner_parser.add_argument( - "--device", - choices=("auto", "cpu", "cuda"), - default="auto", - help="Simulation and planner device. Auto uses CUDA when available.", - ) - neural_planner_parser.add_argument( - "--num-waypoints", - nargs="+", - type=int, - default=[1, 3, 5], - help="Number of EEF waypoints to sweep.", - ) - neural_planner_parser.add_argument( - "--num-trials", - type=int, - default=8, - help="Measured trials per (impl, num_waypoints) configuration.", - ) - neural_planner_parser.add_argument( - "--warmup-trials", - type=int, - default=1, - help="Warmup trials per configuration; excluded from summary aggregation.", - ) - neural_planner_parser.add_argument( - "--sample-interval", - type=int, - default=20, - help="Resampled trajectory length for ik_interpolate and ik_toppra.", + help="Benchmark free-space motion generation with cuRobo as baseline.", ) - neural_planner_parser.add_argument( - "--compare-ik", - action="store_true", - help="Also benchmark sequential IK plus joint interpolation.", - ) - neural_planner_parser.add_argument( - "--compare-toppra", - action="store_true", - help="Also benchmark EEF IK interpolation followed by TOPPRA.", - ) - neural_planner_parser.add_argument( - "--save-trial-details", - action="store_true", - help="Include per-trial rows in the markdown report.", - ) - neural_planner_parser.add_argument( - "--checkpoint-path", - type=str, - default=None, - help="Local neural planner checkpoint path. Skips HuggingFace download.", - ) - neural_planner_parser.add_argument( - "--headless", - action="store_true", - default=True, - help="Run simulation headlessly (default: True).", - ) - neural_planner_parser.add_argument( - "--no-headless", - action="store_false", - dest="headless", - help="Open the simulation viewer window.", + from scripts.benchmark.planners.neural_planner.run_benchmark import ( + add_parser_arguments, ) + + add_parser_arguments(neural_planner_parser) neural_planner_parser.set_defaults(func=_run_neural_planner_cli) # -- atomic-action ------------------------------------------------------- diff --git a/scripts/benchmark/planners/neural_planner/__init__.py b/scripts/benchmark/planners/neural_planner/__init__.py new file mode 100644 index 000000000..70862ba73 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Extensible free-space motion-generation benchmark.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/scripts/benchmark/planners/neural_planner/aggregation.py b/scripts/benchmark/planners/neural_planner/aggregation.py new file mode 100644 index 000000000..eac057b88 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/aggregation.py @@ -0,0 +1,348 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Aggregate raw planner trials and build a complete benchmark leaderboard.""" + +from __future__ import annotations + +import math +from collections import Counter, defaultdict +from collections.abc import Iterable + +from .models import BenchmarkCase, CaseOutcome, PlannerMetadata, TrialPhase, TrialRecord + +__all__ = ["aggregate_results"] + + +def _mean(values: Iterable[float | None]) -> float | None: + """Return the mean of finite values or ``None`` when unavailable.""" + finite = [ + float(value) + for value in values + if value is not None and math.isfinite(float(value)) + ] + return sum(finite) / len(finite) if finite else None + + +def _percentile(values: Iterable[float | None], percentile: float) -> float | None: + """Return a nearest-rank percentile over finite values.""" + finite = sorted( + float(value) + for value in values + if value is not None and math.isfinite(float(value)) + ) + if not finite: + return None + index = max( + 0, min(len(finite) - 1, math.ceil(percentile / 100.0 * len(finite)) - 1) + ) + return finite[index] + + +def _rate(values: Iterable[bool]) -> float | None: + """Return a boolean rate or ``None`` for an empty sequence.""" + materialized = list(values) + return sum(materialized) / len(materialized) if materialized else None + + +def _top_failure(outcomes: list[CaseOutcome]) -> str | None: + """Return the most frequent non-empty failure code.""" + failures = Counter( + outcome.failure_code for outcome in outcomes if outcome.failure_code + ) + return failures.most_common(1)[0][0] if failures else None + + +def _lifecycle_value( + records: list[TrialRecord], + algorithm_id: str, + batch_size: int, + phase: TrialPhase, +) -> float | None: + """Return the first lifecycle cost for one algorithm and batch size.""" + for record in records: + if ( + record.algorithm_id == algorithm_id + and record.batch_size == batch_size + and record.phase is phase + ): + return record.cost_time_ms + return None + + +def _performance_rows( + records: list[TrialRecord], metadata: list[PlannerMetadata] +) -> list[dict[str, object]]: + """Aggregate steady-state time and memory by algorithm and input shape.""" + measured_groups: dict[tuple[str, int, int], list[TrialRecord]] = defaultdict(list) + for record in records: + if record.phase is TrialPhase.MEASURED: + measured_groups[ + (record.algorithm_id, record.batch_size, record.waypoint_count) + ].append(record) + + metadata_by_id = {item.algorithm_id: item for item in metadata} + rows: list[dict[str, object]] = [] + for key in sorted(measured_groups): + algorithm_id, batch_size, waypoint_count = key + group = measured_groups[key] + info = metadata_by_id[algorithm_id] + costs = [record.cost_time_ms for record in group] + mean_cost = _mean(costs) + rows.append( + { + "track": "free-space-common", + "algorithm": algorithm_id, + "algorithm_role": info.algorithm_role.value, + "batch_size": batch_size, + "waypoint_count": waypoint_count, + "num_trials": len(group), + "planner_construct_ms": _lifecycle_value( + records, algorithm_id, batch_size, TrialPhase.CONSTRUCT + ), + "backend_prepare_ms": _lifecycle_value( + records, algorithm_id, batch_size, TrialPhase.PREPARE + ), + "cold_plan_ms": _lifecycle_value( + records, algorithm_id, batch_size, TrialPhase.COLD + ), + "cost_time_ms": mean_cost, + "warm_plan_ms_p50": _percentile(costs, 50.0), + "warm_plan_ms_p95": _percentile(costs, 95.0), + "latency_per_env_ms": ( + mean_cost / batch_size if mean_cost is not None else None + ), + "cost_time_per_segment_ms": ( + mean_cost / waypoint_count if mean_cost is not None else None + ), + "trajectories_per_second": ( + batch_size * 1000.0 / mean_cost + if mean_cost is not None and mean_cost > 0.0 + else None + ), + "cpu_delta_mb": _mean(record.cpu_delta_mb for record in group), + "gpu_delta_mb": _mean(record.gpu_delta_mb for record in group), + "peak_gpu_mb": max( + (record.peak_gpu_mb or 0.0 for record in group), default=0.0 + ), + } + ) + + present_algorithms = {row["algorithm"] for row in rows} + for info in metadata: + if info.algorithm_id in present_algorithms: + continue + rows.append( + { + "track": "free-space-common", + "algorithm": info.algorithm_id, + "algorithm_role": info.algorithm_role.value, + "batch_size": None, + "waypoint_count": None, + "num_trials": 0, + "planner_construct_ms": None, + "backend_prepare_ms": None, + "cold_plan_ms": None, + "cost_time_ms": None, + "warm_plan_ms_p50": None, + "warm_plan_ms_p95": None, + "latency_per_env_ms": None, + "cost_time_per_segment_ms": None, + "trajectories_per_second": None, + "cpu_delta_mb": None, + "gpu_delta_mb": None, + "peak_gpu_mb": None, + } + ) + return sorted( + rows, + key=lambda row: ( + str(row["algorithm"]), + int(row["batch_size"] or 0), + int(row["waypoint_count"] or 0), + ), + ) + + +def _metric_rows( + records: list[TrialRecord], + metadata: list[PlannerMetadata], + cases: list[BenchmarkCase], + measured_trials: int, +) -> list[dict[str, object]]: + """Aggregate external success and quality metrics by scenario condition.""" + outcome_groups: dict[tuple[str, str, int, int, str], list[CaseOutcome]] = ( + defaultdict(list) + ) + for record in records: + if record.phase is not TrialPhase.MEASURED: + continue + key = ( + record.algorithm_id, + record.scenario_id, + record.batch_size, + record.waypoint_count, + record.path_shape, + ) + outcome_groups[key].extend(record.outcomes) + + expected_by_group: Counter[tuple[str, int, int, str]] = Counter() + unique_cases_by_group: Counter[tuple[str, int, int, str]] = Counter() + for case in cases: + key = (case.scenario_id, case.batch_size, case.num_waypoints, case.path_shape) + expected_by_group[key] += case.batch_size * measured_trials + unique_cases_by_group[key] += case.batch_size + + rows: list[dict[str, object]] = [] + for info in metadata: + for group_key in sorted(expected_by_group): + scenario_id, batch_size, waypoint_count, path_shape = group_key + outcomes = outcome_groups.get( + ( + info.algorithm_id, + scenario_id, + batch_size, + waypoint_count, + path_shape, + ), + [], + ) + valid_outcomes = [outcome for outcome in outcomes if outcome.motion_valid] + expected = expected_by_group[group_key] + rows.append( + { + "track": "free-space-common", + "scenario": scenario_id, + "algorithm": info.algorithm_id, + "algorithm_role": info.algorithm_role.value, + "batch_size": batch_size, + "waypoint_count": waypoint_count, + "path_shape": path_shape, + "cases": unique_cases_by_group[group_key], + "coverage_rate": min(1.0, len(outcomes) / max(expected, 1)), + "success_rate": _rate(outcome.motion_valid for outcome in outcomes), + "planning_success_rate": _rate( + outcome.planning_success for outcome in outcomes + ), + "ordered_waypoint_success_rate": _rate( + outcome.ordered_waypoints_reached for outcome in outcomes + ), + "motion_valid_rate": _rate( + outcome.motion_valid for outcome in outcomes + ), + "waypoint_completion_rate": _mean( + outcome.completed_waypoint_ratio for outcome in outcomes + ), + "final_pos_err_mm": _mean( + outcome.final_translation_err_mm for outcome in valid_outcomes + ), + "final_rot_err_deg": _mean( + outcome.final_rotation_err_deg for outcome in valid_outcomes + ), + "waypoint_pos_err_mm_p95": _mean( + outcome.waypoint_translation_err_mm_p95 + for outcome in valid_outcomes + ), + "waypoint_rot_err_deg_p95": _mean( + outcome.waypoint_rotation_err_deg_p95 + for outcome in valid_outcomes + ), + "joint_violation_rate": _rate( + outcome.joint_limit_violation for outcome in outcomes + ), + "joint_path_length_rad": _mean( + outcome.joint_path_length_rad for outcome in valid_outcomes + ), + "cartesian_path_length_m": _mean( + outcome.cartesian_path_length_m for outcome in valid_outcomes + ), + "path_efficiency": _mean( + outcome.path_efficiency for outcome in valid_outcomes + ), + "top_failure": _top_failure(outcomes), + } + ) + return rows + + +def _leaderboard_rows( + records: list[TrialRecord], + metadata: list[PlannerMetadata], + cases: list[BenchmarkCase], + measured_trials: int, +) -> list[dict[str, object]]: + """Build a complete success/coverage/latency ordered leaderboard.""" + expected_outcomes = sum(case.batch_size for case in cases) * measured_trials + entries: list[dict[str, object]] = [] + for info in metadata: + measured = [ + record + for record in records + if record.algorithm_id == info.algorithm_id + and record.phase is TrialPhase.MEASURED + ] + outcomes = [outcome for record in measured for outcome in record.outcomes] + coverage = min(1.0, len(outcomes) / max(expected_outcomes, 1)) + motion_rate = _rate(outcome.motion_valid for outcome in outcomes) or 0.0 + planning_rate = _rate(outcome.planning_success for outcome in outcomes) or 0.0 + latency_p95 = _percentile((record.cost_time_ms for record in measured), 95.0) + peak_gpu = max((record.peak_gpu_mb or 0.0 for record in measured), default=None) + entries.append( + { + "track": "free-space-common", + "algorithm": info.algorithm_id, + "algorithm_role": info.algorithm_role.value, + "model_revision": info.model_revision, + "planner_config_hash": info.config_hash[:12], + "eligible": coverage >= 1.0 - 1.0e-12, + "coverage_rate": coverage, + "overall_success_rate": motion_rate, + "planning_success_rate": planning_rate, + "motion_valid_rate": motion_rate, + "task_success_rate": None, + "latency_p95_ms": latency_p95, + "peak_gpu_mb": peak_gpu, + } + ) + + entries.sort( + key=lambda row: ( + not bool(row["eligible"]), + -float(row["overall_success_rate"]), + -float(row["coverage_rate"]), + ( + float(row["latency_p95_ms"]) + if row["latency_p95_ms"] is not None + else math.inf + ), + str(row["algorithm"]), + ) + ) + return [{"rank": rank, **entry} for rank, entry in enumerate(entries, start=1)] + + +def aggregate_results( + records: list[TrialRecord], + metadata: list[PlannerMetadata], + cases: list[BenchmarkCase], + measured_trials: int, +) -> dict[str, list[dict[str, object]]]: + """Build all three report datasets from raw numeric records.""" + return { + "time_and_memory": _performance_rows(records, metadata), + "success_and_metrics": _metric_rows(records, metadata, cases, measured_trials), + "leaderboard": _leaderboard_rows(records, metadata, cases, measured_trials), + } diff --git a/scripts/benchmark/planners/neural_planner/artifacts.py b/scripts/benchmark/planners/neural_planner/artifacts.py new file mode 100644 index 000000000..49ec54437 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/artifacts.py @@ -0,0 +1,160 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Reproducibility and raw-result artifacts for planner benchmarks.""" + +from __future__ import annotations + +import importlib.metadata +import json +import platform +import subprocess +from datetime import datetime, timezone +from pathlib import Path +from typing import Any + +import torch +import yaml + +from .config import SuiteCfg, suite_to_dict +from .models import BenchmarkCase, TrialRecord + +__all__ = [ + "TrialJsonlWriter", + "create_run_directory", + "environment_metadata", + "write_case_manifest", + "write_json", + "write_resolved_suite", +] + + +def create_run_directory(output_root: str | Path, suite_name: str) -> Path: + """Create one timestamped benchmark run directory.""" + timestamp = datetime.now(timezone.utc).strftime("%Y%m%d_%H%M%S_%f") + run_dir = Path(output_root) / suite_name / timestamp + run_dir.mkdir(parents=True, exist_ok=False) + return run_dir + + +def _package_version(package: str) -> str | None: + """Return an installed distribution version when available.""" + try: + return importlib.metadata.version(package) + except importlib.metadata.PackageNotFoundError: + return None + + +def _git_commit() -> str | None: + """Return the current repository commit without failing outside git.""" + result = subprocess.run( + ["git", "rev-parse", "HEAD"], + check=False, + capture_output=True, + text=True, + ) + return result.stdout.strip() or None + + +def environment_metadata() -> dict[str, object]: + """Collect software and hardware metadata needed to interpret results.""" + gpu_name = None + if torch.cuda.is_available(): + gpu_name = torch.cuda.get_device_name(torch.cuda.current_device()) + return { + "generated_at": datetime.now(timezone.utc).isoformat(timespec="seconds"), + "git_commit": _git_commit(), + "platform": platform.platform(), + "python": platform.python_version(), + "processor": platform.processor() or None, + "torch": torch.__version__, + "cuda_runtime": torch.version.cuda, + "cuda_available": torch.cuda.is_available(), + "gpu": gpu_name, + "curobo": _package_version("nvidia-curobo"), + "dexsim_engine": _package_version("dexsim-engine"), + "embodichain": _package_version("embodichain"), + } + + +def write_json(path: str | Path, value: object) -> Path: + """Write one UTF-8 JSON artifact with stable formatting.""" + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text( + json.dumps(value, indent=2, sort_keys=True, ensure_ascii=False, default=str) + + "\n", + encoding="utf-8", + ) + return output + + +def write_resolved_suite(path: str | Path, suite: SuiteCfg) -> Path: + """Write the fully resolved suite as YAML.""" + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text( + yaml.safe_dump(suite_to_dict(suite), sort_keys=False), encoding="utf-8" + ) + return output + + +def _case_to_dict(case: BenchmarkCase) -> dict[str, Any]: + """Serialize a fixed case without losing tensor numeric values.""" + return { + "suite_version": case.suite_version, + "track": case.track, + "scenario_id": case.scenario_id, + "case_id": case.case_id, + "seed": case.seed, + "batch_size": case.batch_size, + "num_waypoints": case.num_waypoints, + "path_shape": case.path_shape, + "start_state_bins": list(case.start_state_bins), + "start_qpos": case.start_qpos.detach().cpu().tolist(), + "target_waypoints": case.target_waypoints.detach().cpu().tolist(), + "validity_evidence": { + "method": "reference_qpos_fk", + "reference_qpos": case.reference_qpos.detach().cpu().tolist(), + }, + } + + +def write_case_manifest(path: str | Path, cases: list[BenchmarkCase]) -> Path: + """Write the algorithm-independent case manifest.""" + return write_json( + path, + { + "case_schema_version": 1, + "cases": [_case_to_dict(case) for case in cases], + }, + ) + + +class TrialJsonlWriter: + """Append numeric raw trial records to one JSONL artifact.""" + + def __init__(self, path: str | Path) -> None: + self.path = Path(path) + self.path.parent.mkdir(parents=True, exist_ok=True) + self.path.write_text("", encoding="utf-8") + + def append(self, record: TrialRecord) -> None: + """Append one record and flush it immediately for recoverability.""" + with self.path.open("a", encoding="utf-8") as file: + file.write( + json.dumps(record.to_dict(), ensure_ascii=False, default=str) + "\n" + ) diff --git a/scripts/benchmark/planners/neural_planner/compat.py b/scripts/benchmark/planners/neural_planner/compat.py new file mode 100644 index 000000000..ba4a1e6de --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/compat.py @@ -0,0 +1,159 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Temporary helpers retained for callers of the pre-refactor benchmark module.""" + +from __future__ import annotations + +import math +from collections import defaultdict + +__all__ = [ + "IMPL_IK", + "IMPL_NEURAL", + "IMPL_TOPPRA", + "QUALITY_SUMMARY_COLUMNS", + "aggregate_legacy_rows", + "format_waypoint_grouped_tables", +] + +IMPL_NEURAL = "neural_planner" +IMPL_IK = "ik_interpolate" +IMPL_TOPPRA = "ik_toppra" + +QUALITY_SUMMARY_COLUMNS = ( + "impl", + "num_trials", + "success_rate", + "final_translation_err_mm_mean", + "final_rotation_err_deg_mean", + "mean_waypoint_pos_err_mm_mean", + "max_waypoint_pos_err_mm_mean", + "mean_waypoint_rot_err_deg_mean", + "max_waypoint_rot_err_deg_mean", +) + +_IMPL_REPORT_ORDER = {IMPL_NEURAL: 0, IMPL_IK: 1, IMPL_TOPPRA: 2} + + +def _percentile(values: list[float], percentile: float) -> float: + """Return the legacy nearest-rank percentile.""" + ordered = sorted(values) + index = max( + 0, + min( + len(ordered) - 1, + math.ceil(percentile / 100.0 * len(ordered)) - 1, + ), + ) + return ordered[index] + + +def _mean_finite(rows: list[dict[str, object]], key: str) -> str: + """Format the mean of legacy finite numeric values.""" + values = [float(row[key]) for row in rows if math.isfinite(float(row[key]))] + return f"{sum(values) / len(values):.6f}" if values else "inf" + + +def aggregate_legacy_rows( + trial_rows: list[dict[str, object]], +) -> list[dict[str, object]]: + """Aggregate the legacy row schema during the CLI migration window.""" + groups: dict[tuple[str, int], list[dict[str, object]]] = defaultdict(list) + for row in trial_rows: + if not bool(row["warmup"]): + groups[(str(row["impl"]), int(row["num_waypoints"]))].append(row) + + summaries: list[dict[str, object]] = [] + for (impl, waypoint_count), rows in groups.items(): + costs = [float(row["cost_time_ms"]) for row in rows] + summaries.append( + { + "impl": impl, + "num_waypoints": waypoint_count, + "num_trials": len(rows), + "success_rate": f"{sum(bool(row['success']) for row in rows) / len(rows):.2%}", + "cost_time_ms_mean": f"{sum(costs) / len(costs):.6f}", + "cost_time_ms_p95": f"{_percentile(costs, 95.0):.6f}", + "rollout_steps_mean": f"{sum(int(row['rollout_steps']) for row in rows) / len(rows):.2f}", + "cpu_delta_mb_mean": f"{sum(float(row['cpu_delta_mb']) for row in rows) / len(rows):.6f}", + "gpu_delta_mb_mean": f"{sum(float(row['gpu_delta_mb']) for row in rows) / len(rows):.6f}", + "peak_gpu_mb_mean": f"{sum(float(row['peak_gpu_mb']) for row in rows) / len(rows):.6f}", + "peak_gpu_mb_max": f"{max(float(row['peak_gpu_mb']) for row in rows):.6f}", + "final_translation_err_mm_mean": _mean_finite( + rows, "final_translation_err_mm" + ), + "final_rotation_err_deg_mean": _mean_finite( + rows, "final_rotation_err_deg" + ), + "mean_waypoint_pos_err_mm_mean": _mean_finite( + rows, "mean_waypoint_pos_err_mm" + ), + "max_waypoint_pos_err_mm_mean": _mean_finite( + rows, "max_waypoint_pos_err_mm" + ), + "mean_waypoint_rot_err_deg_mean": _mean_finite( + rows, "mean_waypoint_rot_err_deg" + ), + "max_waypoint_rot_err_deg_mean": _mean_finite( + rows, "max_waypoint_rot_err_deg" + ), + } + ) + return sorted( + summaries, + key=lambda row: ( + int(row["num_waypoints"]), + _IMPL_REPORT_ORDER.get(str(row["impl"]), 99), + ), + ) + + +def _format_table(rows: list[dict[str, object]]) -> list[str]: + """Render the small legacy table used by compatibility tests.""" + if not rows: + return ["No data."] + headers = list(rows[0]) + lines = [ + "| " + " | ".join(headers) + " |", + "| " + " | ".join(["---"] * len(headers)) + " |", + ] + lines.extend( + "| " + " | ".join(str(row[header]) for header in headers) + " |" for row in rows + ) + return lines + + +def format_waypoint_grouped_tables( + summary_rows: list[dict[str, object]], columns: tuple[str, ...] +) -> list[str]: + """Render legacy summaries grouped by waypoint count.""" + groups: dict[int, list[dict[str, object]]] = defaultdict(list) + for row in summary_rows: + groups[int(row["num_waypoints"])].append(row) + lines: list[str] = [] + for group_index, waypoint_count in enumerate(sorted(groups)): + if group_index: + lines.append("") + rows = sorted( + groups[waypoint_count], + key=lambda row: _IMPL_REPORT_ORDER.get(str(row["impl"]), 99), + ) + lines.extend([f"### num_waypoints = {waypoint_count}", ""]) + lines.extend( + _format_table([{column: row[column] for column in columns} for row in rows]) + ) + return lines or ["No data."] diff --git a/scripts/benchmark/planners/neural_planner/config.py b/scripts/benchmark/planners/neural_planner/config.py new file mode 100644 index 000000000..ff200b16b --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/config.py @@ -0,0 +1,226 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Configuration loading and hashing for free-space planner benchmark suites.""" + +from __future__ import annotations + +import hashlib +import json +from dataclasses import asdict +from pathlib import Path +from typing import Any + +import yaml + +from embodichain.utils import configclass + +from .models import AlgorithmRole + +__all__ = [ + "BENCHMARK_ROOT", + "FreeSpaceTrackCfg", + "PlannerSpecCfg", + "ProtocolCfg", + "SuiteCfg", + "load_suite", + "stable_hash", + "suite_to_dict", +] + +BENCHMARK_ROOT = Path(__file__).resolve().parent +_SUPPORTED_PATH_SHAPES = { + "direct", + "l_turn", + "s_curve", + "orientation_only", + "combined", +} +_SUPPORTED_START_STATE_BINS = { + "nominal", + "random_reachable", + "near_limit", + "near_singularity", +} + + +@configclass +class PlannerSpecCfg: + """Configuration for one registered planner adapter.""" + + id: str = "" + adapter: str = "" + role: str = AlgorithmRole.DIAGNOSTIC_BASELINE.value + enabled: bool = False + config: dict[str, Any] = {} + + +@configclass +class ProtocolCfg: + """Common timing and external-validation protocol.""" + + warmup_trials: int = 1 + measured_trials: int = 3 + sample_interval: int = 40 + validation_samples: int = 128 + position_threshold_m: float = 0.05 + rotation_threshold_rad: float = 0.3 + joint_limit_tolerance_rad: float = 1.0e-5 + + +@configclass +class FreeSpaceTrackCfg: + """Case matrix for the ``free-space-common`` track.""" + + batch_sizes: list[int] = [1] + waypoint_counts: list[int] = [1, 3, 5] + path_shapes: list[str] = ["direct", "l_turn", "s_curve"] + start_state_bins: list[str] = ["nominal"] + seeds: list[int] = [11] + + +@configclass +class SuiteCfg: + """Resolved benchmark suite configuration.""" + + schema_version: int = 1 + name: str = "free_space_common" + suite_version: str = "free_space_common_v1" + profile: str = "smoke" + planners: list[PlannerSpecCfg] = [] + protocol: ProtocolCfg = ProtocolCfg() + free_space: FreeSpaceTrackCfg = FreeSpaceTrackCfg() + + @classmethod + def from_dict(cls, data: dict[str, Any]) -> "SuiteCfg": + """Build and validate a suite from a YAML-compatible mapping.""" + planners = [PlannerSpecCfg(**item) for item in data.get("planners", [])] + suite = cls( + schema_version=int(data.get("schema_version", 1)), + name=str(data.get("name", "free_space_common")), + suite_version=str(data.get("suite_version", "free_space_common_v1")), + profile=str(data.get("profile", "smoke")), + planners=planners, + protocol=ProtocolCfg(**data.get("protocol", {})), + free_space=FreeSpaceTrackCfg(**data.get("free_space", {})), + ) + suite.validate_benchmark() + return suite + + def validate_benchmark(self) -> None: + """Validate values that affect benchmark correctness.""" + missing_fields = self.validate() + if missing_fields: + raise ValueError( + "Benchmark suite has missing required fields: " + + ", ".join(missing_fields) + ) + if self.schema_version != 1: + raise ValueError( + f"Unsupported suite schema_version={self.schema_version}; expected 1." + ) + if not self.planners: + raise ValueError("The benchmark suite must declare at least one planner.") + planner_ids = [spec.id for spec in self.planners] + if len(planner_ids) != len(set(planner_ids)): + raise ValueError("Planner ids must be unique within a suite.") + for spec in self.planners: + if not spec.id or not spec.adapter: + raise ValueError( + "Every planner must define a non-empty id and adapter." + ) + AlgorithmRole(spec.role) + if self.protocol.warmup_trials < 0: + raise ValueError("warmup_trials must be >= 0.") + if self.protocol.measured_trials < 1: + raise ValueError("measured_trials must be >= 1.") + if self.protocol.sample_interval < 2: + raise ValueError("sample_interval must be >= 2.") + if self.protocol.validation_samples < 2: + raise ValueError("validation_samples must be >= 2.") + if self.protocol.position_threshold_m <= 0.0: + raise ValueError("position_threshold_m must be > 0.") + if self.protocol.rotation_threshold_rad <= 0.0: + 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.") + if not self.free_space.batch_sizes or any( + value < 1 for value in self.free_space.batch_sizes + ): + raise ValueError("batch_sizes must contain positive integers.") + if not self.free_space.waypoint_counts or any( + value < 1 for value in self.free_space.waypoint_counts + ): + raise ValueError("waypoint_counts must contain positive integers.") + if not self.free_space.seeds: + raise ValueError("seeds must not be empty.") + if not self.free_space.path_shapes: + raise ValueError("path_shapes must not be empty.") + unknown_shapes = set(self.free_space.path_shapes) - _SUPPORTED_PATH_SHAPES + if unknown_shapes: + raise ValueError(f"Unsupported path_shapes: {sorted(unknown_shapes)}.") + if not self.free_space.start_state_bins: + raise ValueError("start_state_bins must not be empty.") + unknown_bins = ( + set(self.free_space.start_state_bins) - _SUPPORTED_START_STATE_BINS + ) + if unknown_bins: + raise ValueError(f"Unsupported start_state_bins: {sorted(unknown_bins)}.") + for name, values in ( + ("batch_sizes", self.free_space.batch_sizes), + ("waypoint_counts", self.free_space.waypoint_counts), + ("path_shapes", self.free_space.path_shapes), + ("start_state_bins", self.free_space.start_state_bins), + ("seeds", self.free_space.seeds), + ): + if len(values) != len(set(values)): + raise ValueError(f"{name} must not contain duplicate values.") + nmg = next((spec for spec in self.planners if spec.id == "nmg"), None) + if nmg is not None: + if float(nmg.config.get("pos_eps", 0.05)) <= 0.0: + raise ValueError("NMG pos_eps must be > 0.") + if float(nmg.config.get("rot_eps", 0.3)) <= 0.0: + raise ValueError("NMG rot_eps must be > 0.") + + +def load_suite(name_or_path: str = "smoke") -> SuiteCfg: + """Load a suite by short name or explicit YAML path.""" + requested = Path(name_or_path) + path = ( + requested + if requested.is_file() + else BENCHMARK_ROOT / "suites" / f"{name_or_path}.yaml" + ) + if not path.is_file(): + raise FileNotFoundError(f"Benchmark suite not found: {path}") + with path.open("r", encoding="utf-8") as file: + data = yaml.safe_load(file) or {} + if not isinstance(data, dict): + raise TypeError( + f"Expected a mapping in suite {path}, got {type(data).__name__}." + ) + return SuiteCfg.from_dict(data) + + +def suite_to_dict(suite: SuiteCfg) -> dict[str, Any]: + """Convert a resolved suite to plain YAML/JSON-compatible values.""" + return asdict(suite) + + +def stable_hash(value: object) -> str: + """Return a stable SHA256 hash for JSON-compatible configuration data.""" + payload = json.dumps(value, sort_keys=True, separators=(",", ":"), default=str) + return hashlib.sha256(payload.encode("utf-8")).hexdigest() diff --git a/scripts/benchmark/planners/neural_planner/metrics/__init__.py b/scripts/benchmark/planners/neural_planner/metrics/__init__.py new file mode 100644 index 000000000..ed41b292f --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/metrics/__init__.py @@ -0,0 +1,36 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Metric evaluators for motion-generation benchmarks.""" + +from __future__ import annotations + +from .performance import TimedCall, timed_call +from .trajectory import ( + compute_case_outcomes, + compute_waypoint_errors, + get_pose_err, + match_ordered_waypoints, +) + +__all__ = [ + "TimedCall", + "compute_case_outcomes", + "compute_waypoint_errors", + "get_pose_err", + "match_ordered_waypoints", + "timed_call", +] diff --git a/scripts/benchmark/planners/neural_planner/metrics/performance.py b/scripts/benchmark/planners/neural_planner/metrics/performance.py new file mode 100644 index 000000000..692ec29d3 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/metrics/performance.py @@ -0,0 +1,85 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Wall-clock and process-memory measurement helpers.""" + +from __future__ import annotations + +import os +import time +from collections.abc import Callable +from dataclasses import dataclass +from typing import Generic, TypeVar + +import psutil +import torch + +__all__ = ["TimedCall", "timed_call"] + +_T = TypeVar("_T") + + +@dataclass(frozen=True) +class TimedCall(Generic[_T]): + """Result and resource deltas captured around one callable.""" + + result: _T + cost_time_ms: float + cpu_delta_mb: float + gpu_delta_mb: float + peak_gpu_mb: float + + +def _sync_cuda() -> None: + """Synchronize CUDA before and after timed operations when available.""" + if torch.cuda.is_available(): + torch.cuda.synchronize() + + +def _memory_snapshot() -> tuple[float, float]: + """Return current process RSS and PyTorch GPU allocation in MB.""" + cpu_mb = psutil.Process(os.getpid()).memory_info().rss / 1024**2 + gpu_mb = ( + torch.cuda.memory_allocated() / 1024**2 if torch.cuda.is_available() else 0.0 + ) + return cpu_mb, gpu_mb + + +def timed_call(callable_fn: Callable[[], _T]) -> TimedCall[_T]: + """Time only ``callable_fn`` and capture CPU/GPU memory deltas.""" + if torch.cuda.is_available(): + torch.cuda.reset_peak_memory_stats() + cpu_before, gpu_before = _memory_snapshot() + _sync_cuda() + + start = time.perf_counter() + result = callable_fn() + _sync_cuda() + elapsed_ms = (time.perf_counter() - start) * 1000.0 + + cpu_after, gpu_after = _memory_snapshot() + peak_gpu_mb = ( + torch.cuda.max_memory_allocated() / 1024**2 + if torch.cuda.is_available() + else 0.0 + ) + return TimedCall( + result=result, + cost_time_ms=elapsed_ms, + cpu_delta_mb=cpu_after - cpu_before, + gpu_delta_mb=gpu_after - gpu_before, + peak_gpu_mb=peak_gpu_mb, + ) diff --git a/scripts/benchmark/planners/neural_planner/metrics/trajectory.py b/scripts/benchmark/planners/neural_planner/metrics/trajectory.py new file mode 100644 index 000000000..e3fa71a6f --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/metrics/trajectory.py @@ -0,0 +1,487 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Ordered waypoint and free-space trajectory validity metrics.""" + +from __future__ import annotations + +import math +from typing import TYPE_CHECKING + +import torch + +from embodichain.lab.sim.planners.utils import PlanResult + +from ..models import BenchmarkCase, CaseOutcome + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + +__all__ = [ + "compute_case_outcomes", + "compute_waypoint_errors", + "get_pose_err", + "make_failure_outcomes", + "match_ordered_waypoints", +] + + +def _percentile(values: list[float], percentile: float) -> float: + """Return a nearest-rank percentile for a non-empty list.""" + if not values: + return float("inf") + ordered = sorted(values) + index = max( + 0, + min( + len(ordered) - 1, + math.ceil(percentile / 100.0 * len(ordered)) - 1, + ), + ) + return float(ordered[index]) + + +def _pose_error_matrices( + waypoints: torch.Tensor, trajectory_poses: torch.Tensor +) -> tuple[torch.Tensor, torch.Tensor]: + """Return waypoint-by-sample translation and geodesic rotation errors.""" + waypoints = torch.as_tensor(waypoints, dtype=torch.float64) + trajectory_poses = torch.as_tensor( + trajectory_poses, dtype=torch.float64, device=waypoints.device + ) + pos_error = torch.linalg.norm( + waypoints[:, None, :3, 3] - trajectory_poses[None, :, :3, 3], dim=-1 + ) + waypoint_rot = waypoints[:, None, :3, :3] + trajectory_rot = trajectory_poses[None, :, :3, :3] + relative = waypoint_rot.transpose(-1, -2) @ trajectory_rot + trace = torch.diagonal(relative, dim1=-2, dim2=-1).sum(dim=-1) + rotation_error = torch.arccos(torch.clamp((trace - 1.0) * 0.5, -1.0, 1.0)) + return pos_error, rotation_error + + +def get_pose_err(matrix_a: torch.Tensor, matrix_b: torch.Tensor) -> tuple[float, float]: + """Return translation (m) and geodesic rotation (rad) pose errors.""" + tensor_a = torch.as_tensor(matrix_a, dtype=torch.float64) + tensor_b = torch.as_tensor(matrix_b, dtype=torch.float64, device=tensor_a.device) + if tensor_a.ndim == 2: + tensor_a = tensor_a.unsqueeze(0) + if tensor_b.ndim == 2: + tensor_b = tensor_b.unsqueeze(0) + translation = torch.linalg.norm(tensor_a[:, :3, 3] - tensor_b[:, :3, 3], dim=-1) + relative = tensor_a[:, :3, :3].transpose(-1, -2) @ tensor_b[:, :3, :3] + trace = torch.diagonal(relative, dim1=-2, dim2=-1).sum(dim=-1) + rotation = torch.arccos(torch.clamp((trace - 1.0) * 0.5, -1.0, 1.0)) + return float(translation.mean().item()), float(rotation.mean().item()) + + +def _minimum_cost_monotonic_indices(cost: torch.Tensor) -> list[int]: + """Match each waypoint to one strictly later sample with minimum total cost.""" + waypoint_count, sample_count = cost.shape + if waypoint_count == 0 or sample_count == 0 or sample_count < waypoint_count: + return [] + + dp = torch.full_like(cost, float("inf")) + parents = torch.full( + (waypoint_count, sample_count), -1, dtype=torch.long, device=cost.device + ) + dp[0] = cost[0] + for waypoint_index in range(1, waypoint_count): + for sample_index in range(waypoint_index, sample_count): + previous = dp[waypoint_index - 1, :sample_index] + best_cost, best_index = torch.min(previous, dim=0) + dp[waypoint_index, sample_index] = ( + best_cost + cost[waypoint_index, sample_index] + ) + parents[waypoint_index, sample_index] = best_index + + final_index = int(torch.argmin(dp[-1]).item()) + if not torch.isfinite(dp[-1, final_index]): + return [] + indices = [final_index] + for waypoint_index in range(waypoint_count - 1, 0, -1): + final_index = int(parents[waypoint_index, final_index].item()) + indices.append(final_index) + return list(reversed(indices)) + + +def match_ordered_waypoints( + trajectory_poses: torch.Tensor, + waypoints: torch.Tensor, + *, + position_threshold_m: float, + rotation_threshold_rad: float, +) -> dict[str, object]: + """Evaluate ordered arrival and joint position/rotation waypoint errors.""" + trajectory_poses = torch.as_tensor(trajectory_poses) + waypoints = torch.as_tensor(waypoints) + if trajectory_poses.numel() == 0 or waypoints.numel() == 0: + return { + "ordered_waypoints_reached": False, + "completed_waypoint_ratio": 0.0, + "arrival_indices": [], + "matched_indices": [], + "position_errors_m": [], + "rotation_errors_rad": [], + } + + pos_error, rot_error = _pose_error_matrices(waypoints, trajectory_poses) + arrival_indices: list[int] = [] + next_sample = 0 + for waypoint_index in range(waypoints.shape[0]): + valid = torch.nonzero( + (pos_error[waypoint_index, next_sample:] <= position_threshold_m) + & (rot_error[waypoint_index, next_sample:] <= rotation_threshold_rad), + as_tuple=False, + ).flatten() + if valid.numel() == 0: + break + sample_index = next_sample + int(valid[0].item()) + arrival_indices.append(sample_index) + next_sample = sample_index + 1 + + normalized_cost = ( + pos_error / position_threshold_m + rot_error / rotation_threshold_rad + ) + matched_indices = _minimum_cost_monotonic_indices(normalized_cost) + position_errors = [ + float(pos_error[index, sample].item()) + for index, sample in enumerate(matched_indices) + ] + rotation_errors = [ + float(rot_error[index, sample].item()) + for index, sample in enumerate(matched_indices) + ] + completed = len(arrival_indices) + total = int(waypoints.shape[0]) + return { + "ordered_waypoints_reached": completed == total, + "completed_waypoint_ratio": completed / max(total, 1), + "arrival_indices": arrival_indices, + "matched_indices": matched_indices, + "position_errors_m": position_errors, + "rotation_errors_rad": rotation_errors, + } + + +def compute_waypoint_errors( + trajectory_poses: list[torch.Tensor] | torch.Tensor, + waypoints: torch.Tensor, + *, + position_threshold_m: float = 0.05, + rotation_threshold_rad: float = 0.3, +) -> dict[str, float]: + """Return ordered, same-sample waypoint errors for compatibility callers.""" + if isinstance(trajectory_poses, list): + trajectory_tensor = ( + torch.stack(trajectory_poses) + if trajectory_poses + else torch.empty((0, 4, 4)) + ) + else: + trajectory_tensor = trajectory_poses + matched = match_ordered_waypoints( + trajectory_tensor, + waypoints, + position_threshold_m=position_threshold_m, + rotation_threshold_rad=rotation_threshold_rad, + ) + pos_mm = [float(value) * 1000.0 for value in matched["position_errors_m"]] + rot_deg = [ + float(value) * 180.0 / math.pi for value in matched["rotation_errors_rad"] + ] + return { + "mean_waypoint_pos_err_mm": ( + sum(pos_mm) / len(pos_mm) if pos_mm else float("inf") + ), + "max_waypoint_pos_err_mm": max(pos_mm) if pos_mm else float("inf"), + "mean_waypoint_rot_err_deg": ( + sum(rot_deg) / len(rot_deg) if rot_deg else float("inf") + ), + "max_waypoint_rot_err_deg": max(rot_deg) if rot_deg else float("inf"), + } + + +def _resample_joint_path(positions: torch.Tensor, sample_count: int) -> torch.Tensor: + """Resample one joint path at uniform cumulative joint-arc length.""" + positions = torch.as_tensor(positions) + if positions.shape[0] == 0: + return positions + if positions.shape[0] == 1 or sample_count <= 1: + return positions[:1] + segment_length = torch.linalg.norm(positions[1:] - positions[:-1], dim=-1) + cumulative = torch.cat( + [ + torch.zeros(1, device=positions.device, dtype=positions.dtype), + segment_length.cumsum(0), + ] + ) + total = cumulative[-1] + if float(total.item()) <= 1.0e-12: + return positions[:1].expand(sample_count, -1).clone() + targets = torch.linspace( + 0.0, + float(total.item()), + sample_count, + device=positions.device, + dtype=positions.dtype, + ) + right = torch.searchsorted(cumulative, targets, right=True).clamp( + min=1, max=positions.shape[0] - 1 + ) + left = right - 1 + denominator = (cumulative[right] - cumulative[left]).clamp_min(1.0e-12) + ratio = ((targets - cumulative[left]) / denominator).unsqueeze(-1) + return positions[left] + ratio * (positions[right] - positions[left]) + + +def _success_tensor(success: bool | torch.Tensor, batch_size: int) -> torch.Tensor: + """Normalize scalar or tensor planner success to a CPU bool vector.""" + if isinstance(success, torch.Tensor): + values = success.detach().to(device="cpu", dtype=torch.bool).flatten() + if values.numel() == 1 and batch_size > 1: + values = values.expand(batch_size).clone() + if values.numel() != batch_size: + raise ValueError( + f"PlanResult.success has {values.numel()} values for batch_size={batch_size}." + ) + return values + return torch.full((batch_size,), bool(success), dtype=torch.bool) + + +def _joint_limit_metrics( + positions: torch.Tensor, + limits: torch.Tensor, + tolerance_rad: float, +) -> tuple[bool, float]: + """Return whether limits are violated and the maximum normalized excess.""" + lower, upper = limits[:, 0], limits[:, 1] + below = (lower - positions - tolerance_rad).clamp_min(0.0) + above = (positions - upper - tolerance_rad).clamp_min(0.0) + span = (upper - lower).clamp_min(1.0e-6) + normalized = torch.maximum(below, above) / span + maximum = float(normalized.max().item()) if normalized.numel() else 0.0 + return maximum > 0.0, maximum + + +def _path_metrics( + qpos: torch.Tensor, + poses: torch.Tensor, + waypoints: torch.Tensor, +) -> tuple[float, float, float]: + """Return joint length, Cartesian translation length, and path efficiency.""" + joint_length = float(torch.linalg.norm(qpos[1:] - qpos[:-1], dim=-1).sum().item()) + translations = poses[:, :3, 3] + cartesian_length = float( + torch.linalg.norm(translations[1:] - translations[:-1], dim=-1).sum().item() + ) + anchors = torch.cat([poses[:1, :3, 3], waypoints[:, :3, 3]], dim=0) + lower_bound = float( + torch.linalg.norm(anchors[1:] - anchors[:-1], dim=-1).sum().item() + ) + efficiency = ( + min(1.0, lower_bound / cartesian_length) if cartesian_length > 1.0e-12 else 1.0 + ) + return joint_length, cartesian_length, efficiency + + +def make_failure_outcomes( + batch_size: int, + failure_code: str, +) -> tuple[CaseOutcome, ...]: + """Create per-env outcomes for an exception before validation was possible.""" + return tuple( + CaseOutcome( + env_index=index, + planning_success=False, + finite=False, + ordered_waypoints_reached=False, + motion_valid=False, + completed_waypoint_ratio=0.0, + final_translation_err_mm=None, + final_rotation_err_deg=None, + waypoint_translation_err_mm_mean=None, + waypoint_translation_err_mm_p95=None, + waypoint_translation_err_mm_max=None, + waypoint_rotation_err_deg_mean=None, + waypoint_rotation_err_deg_p95=None, + waypoint_rotation_err_deg_max=None, + joint_limit_violation=False, + max_normalized_joint_violation=None, + joint_path_length_rad=None, + cartesian_path_length_m=None, + path_efficiency=None, + failure_code=failure_code, + ) + for index in range(batch_size) + ) + + +def compute_case_outcomes( + result: PlanResult, + case: BenchmarkCase, + robot: "Robot", + control_part: str, + *, + validation_samples: int, + position_threshold_m: float, + rotation_threshold_rad: float, + joint_limit_tolerance_rad: float, +) -> tuple[CaseOutcome, ...]: + """Recompute free-space success and quality from a planner trajectory.""" + planning_success = _success_tensor(result.success, case.batch_size) + if result.positions is None or result.positions.ndim != 3: + return tuple( + CaseOutcome( + **{ + **make_failure_outcomes(1, "planner_reported_failure")[0].__dict__, + "env_index": env_index, + "planning_success": bool(planning_success[env_index].item()), + } + ) + for env_index in range(case.batch_size) + ) + + positions = result.positions.to(robot.device) + if positions.shape[0] != case.batch_size: + raise ValueError( + f"PlanResult.positions batch={positions.shape[0]} does not match " + f"case batch={case.batch_size}." + ) + limits = robot.get_qpos_limits(name=control_part) + finite_paths = [ + bool(torch.isfinite(positions[env_index]).all().item()) + and positions[env_index].shape[0] > 0 + for env_index in range(case.batch_size) + ] + validation_paths = [ + ( + _resample_joint_path(positions[env_index], validation_samples) + if finite_paths[env_index] + else case.start_qpos[env_index : env_index + 1] + .expand(validation_samples, -1) + .clone() + ) + for env_index in range(case.batch_size) + ] + validation_qpos_batch = torch.stack(validation_paths) + validation_pose_batch = robot.compute_batch_fk( + qpos=validation_qpos_batch, + name=control_part, + to_matrix=True, + ) + outcomes: list[CaseOutcome] = [] + for env_index in range(case.batch_size): + native_qpos = positions[env_index] + finite = finite_paths[env_index] + if finite: + validation_qpos = validation_qpos_batch[env_index] + poses = validation_pose_batch[env_index] + else: + validation_qpos = torch.empty( + (0, positions.shape[-1]), device=robot.device, dtype=positions.dtype + ) + poses = torch.empty((0, 4, 4), device=robot.device, dtype=positions.dtype) + + waypoints = case.target_waypoints[env_index] + matching = match_ordered_waypoints( + poses, + waypoints, + position_threshold_m=position_threshold_m, + rotation_threshold_rad=rotation_threshold_rad, + ) + pos_errors_mm = [ + float(value) * 1000.0 for value in matching["position_errors_m"] + ] + rot_errors_deg = [ + float(value) * 180.0 / math.pi for value in matching["rotation_errors_rad"] + ] + joint_violation, normalized_violation = _joint_limit_metrics( + native_qpos, + limits[env_index], + joint_limit_tolerance_rad, + ) + ordered = bool(matching["ordered_waypoints_reached"]) + planner_ok = bool(planning_success[env_index].item()) + # ``PlanResult.success`` is retained as a planner-stage outcome, but it + # is not external ground truth. A trajectory can therefore be motion + # valid even when a backend conservatively reports planning failure. + motion_valid = finite and ordered and not joint_violation + + if poses.shape[0] > 0: + final_pos_m, final_rot_rad = get_pose_err(poses[-1], waypoints[-1]) + joint_length, cartesian_length, efficiency = _path_metrics( + validation_qpos, poses, waypoints + ) + else: + final_pos_m = final_rot_rad = None + joint_length = cartesian_length = efficiency = None + + failure_code = None + if not planner_ok: + failure_code = "planner_reported_failure" + elif not finite: + failure_code = "non_finite_trajectory" + elif not ordered: + failure_code = "waypoint_miss" + elif joint_violation: + failure_code = "joint_limit_violation" + + outcomes.append( + CaseOutcome( + env_index=env_index, + planning_success=planner_ok, + finite=finite, + ordered_waypoints_reached=ordered, + motion_valid=motion_valid, + completed_waypoint_ratio=float(matching["completed_waypoint_ratio"]), + final_translation_err_mm=( + final_pos_m * 1000.0 if final_pos_m is not None else None + ), + final_rotation_err_deg=( + final_rot_rad * 180.0 / math.pi + if final_rot_rad is not None + else None + ), + waypoint_translation_err_mm_mean=( + sum(pos_errors_mm) / len(pos_errors_mm) if pos_errors_mm else None + ), + waypoint_translation_err_mm_p95=( + _percentile(pos_errors_mm, 95.0) if pos_errors_mm else None + ), + waypoint_translation_err_mm_max=( + max(pos_errors_mm) if pos_errors_mm else None + ), + waypoint_rotation_err_deg_mean=( + sum(rot_errors_deg) / len(rot_errors_deg) + if rot_errors_deg + else None + ), + waypoint_rotation_err_deg_p95=( + _percentile(rot_errors_deg, 95.0) if rot_errors_deg else None + ), + waypoint_rotation_err_deg_max=( + max(rot_errors_deg) if rot_errors_deg else None + ), + joint_limit_violation=joint_violation, + max_normalized_joint_violation=normalized_violation, + joint_path_length_rad=joint_length, + cartesian_path_length_m=cartesian_length, + path_efficiency=efficiency, + failure_code=failure_code, + ) + ) + return tuple(outcomes) diff --git a/scripts/benchmark/planners/neural_planner/models.py b/scripts/benchmark/planners/neural_planner/models.py new file mode 100644 index 000000000..aa7c58199 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/models.py @@ -0,0 +1,147 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Typed records shared by the motion-generation benchmark modules.""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, field +from enum import Enum + +import torch + +__all__ = [ + "AlgorithmRole", + "BenchmarkCase", + "CaseOutcome", + "PlannerMetadata", + "TrialPhase", + "TrialRecord", +] + + +class AlgorithmRole(str, Enum): + """Role an algorithm plays in benchmark comparisons.""" + + CANDIDATE = "candidate" + PRIMARY_BASELINE = "primary_baseline" + DIAGNOSTIC_BASELINE = "diagnostic_baseline" + + +class TrialPhase(str, Enum): + """Lifecycle phase represented by a raw trial record.""" + + AVAILABILITY = "availability" + CONSTRUCT = "construct" + PREPARE = "prepare" + COLD = "cold" + WARMUP = "warmup" + MEASURED = "measured" + + +@dataclass(frozen=True) +class PlannerMetadata: + """Stable planner identity and capability metadata.""" + + algorithm_id: str + algorithm_role: AlgorithmRole + adapter: str + config_hash: str + capabilities: frozenset[str] + model_revision: str = "N/A" + inference_dtype: str = "fp32" + supported_robots: tuple[str, ...] = ("franka_panda",) + parameters: dict[str, object] = field(default_factory=dict) + + +@dataclass(frozen=True) +class BenchmarkCase: + """One env-batched free-space planning input frozen before execution.""" + + suite_version: str + track: str + scenario_id: str + case_id: str + seed: int + batch_size: int + num_waypoints: int + path_shape: str + start_state_bins: tuple[str, ...] + start_qpos: torch.Tensor + target_waypoints: torch.Tensor + reference_qpos: torch.Tensor + + +@dataclass(frozen=True) +class CaseOutcome: + """External validation result for one environment row in a batch.""" + + env_index: int + planning_success: bool + finite: bool + ordered_waypoints_reached: bool + motion_valid: bool + completed_waypoint_ratio: float + final_translation_err_mm: float | None + final_rotation_err_deg: float | None + waypoint_translation_err_mm_mean: float | None + waypoint_translation_err_mm_p95: float | None + waypoint_translation_err_mm_max: float | None + waypoint_rotation_err_deg_mean: float | None + waypoint_rotation_err_deg_p95: float | None + waypoint_rotation_err_deg_max: float | None + joint_limit_violation: bool + max_normalized_joint_violation: float | None + joint_path_length_rad: float | None + cartesian_path_length_m: float | None + path_efficiency: float | None + failure_code: str | None = None + + +@dataclass(frozen=True) +class TrialRecord: + """Raw lifecycle or planning record written to ``trials.jsonl``.""" + + suite_version: str + track: str + scenario_id: str + case_id: str + algorithm_id: str + algorithm_role: AlgorithmRole + model_revision: str + planner_config_hash: str + seed: int + repeat: int + batch_size: int + waypoint_count: int + path_shape: str + phase: TrialPhase + status: str = "ok" + failure_code: str | None = None + failure_message: str | None = None + cost_time_ms: float | None = None + cpu_delta_mb: float | None = None + gpu_delta_mb: float | None = None + peak_gpu_mb: float | None = None + metadata: dict[str, object] = field(default_factory=dict) + outcomes: tuple[CaseOutcome, ...] = () + + def to_dict(self) -> dict[str, object]: + """Return a JSON-serializable mapping while retaining numeric values.""" + data = asdict(self) + data["algorithm_role"] = self.algorithm_role.value + data["phase"] = self.phase.value + return data diff --git a/scripts/benchmark/planners/neural_planner/planners/__init__.py b/scripts/benchmark/planners/neural_planner/planners/__init__.py new file mode 100644 index 000000000..65b3edabc --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/planners/__init__.py @@ -0,0 +1,34 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Built-in planner adapters and their registry side effects.""" + +from __future__ import annotations + +from .base import PlannerAdapter, PlannerContext +from .curobo import CuroboAdapter +from .ik_interpolate import IkInterpolateAdapter +from .neural import NeuralAdapterStub +from .toppra import ToppraAdapter + +__all__ = [ + "CuroboAdapter", + "IkInterpolateAdapter", + "NeuralAdapterStub", + "PlannerAdapter", + "PlannerContext", + "ToppraAdapter", +] diff --git a/scripts/benchmark/planners/neural_planner/planners/base.py b/scripts/benchmark/planners/neural_planner/planners/base.py new file mode 100644 index 000000000..cbf9e90d5 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/planners/base.py @@ -0,0 +1,92 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Backend-independent planner adapter contract.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from dataclasses import dataclass +from typing import TYPE_CHECKING + +import torch + +from embodichain.lab.sim.planners.utils import PlanResult + +from ..config import PlannerSpecCfg, stable_hash +from ..models import AlgorithmRole, BenchmarkCase, PlannerMetadata + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + +__all__ = ["PlannerAdapter", "PlannerContext"] + + +@dataclass(frozen=True) +class PlannerContext: + """Runtime objects shared with a planner adapter.""" + + robot: "Robot" + control_part: str + device: torch.device + sample_interval: int + + +class PlannerAdapter(ABC): + """Uniform lifecycle around a motion-planning implementation.""" + + capabilities: frozenset[str] = frozenset() + model_revision: str = "N/A" + separate_prepare: bool = False + """Whether this backend exposes a distinct lazy preparation phase.""" + + def __init__(self, spec: PlannerSpecCfg, context: PlannerContext) -> None: + self.spec = spec + self.context = context + + @property + def metadata(self) -> PlannerMetadata: + """Return stable identity, role, configuration, and capabilities.""" + return PlannerMetadata( + algorithm_id=self.spec.id, + algorithm_role=AlgorithmRole(self.spec.role), + adapter=self.spec.adapter, + config_hash=stable_hash(self.spec.config), + capabilities=self.capabilities, + model_revision=str( + self.spec.config.get("model_revision", self.model_revision) + ), + parameters=dict(self.spec.config), + ) + + def availability(self) -> tuple[bool, str | None]: + """Return whether this adapter can run in the current process.""" + return True, None + + @abstractmethod + def build(self) -> None: + """Construct the underlying planner without preparing lazy backends.""" + + def prepare(self, case: BenchmarkCase) -> dict[str, object] | None: + """Prepare a lazy backend, or return ``None`` when not applicable.""" + return None + + @abstractmethod + def plan(self, case: BenchmarkCase) -> PlanResult: + """Plan one env-batched benchmark case.""" + + def close(self) -> None: + """Release backend resources when the implementation exposes them.""" diff --git a/scripts/benchmark/planners/neural_planner/planners/curobo.py b/scripts/benchmark/planners/neural_planner/planners/curobo.py new file mode 100644 index 000000000..78dffd91c --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/planners/curobo.py @@ -0,0 +1,143 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""cuRobo primary-baseline adapter for an empty collision world.""" + +from __future__ import annotations + +import importlib.util + +import torch + +from embodichain.lab.sim.planners import ( + CuroboAutoGenCfg, + CuroboPlanOptions, + CuroboPlannerCfg, + CuroboWorldCfg, + MotionGenCfg, + MotionGenOptions, + MotionGenerator, + MoveType, + PlanResult, + PlanState, +) + +from ..config import PlannerSpecCfg +from ..models import BenchmarkCase +from ..registry import register_planner_adapter +from .base import PlannerAdapter, PlannerContext + +__all__ = ["CuroboAdapter"] + + +class CuroboAdapter(PlannerAdapter): + """Run cuRobo with a frozen, empty-world operational configuration.""" + + capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) + model_revision = "curobo-v2" + separate_prepare = True + + def __init__(self, spec: PlannerSpecCfg, context: PlannerContext) -> None: + super().__init__(spec, context) + self.motion_generator: MotionGenerator | None = None + + def availability(self) -> tuple[bool, str | None]: + """Require both CUDA and the optional cuRobo runtime.""" + if not torch.cuda.is_available(): + return False, "cuRobo requires CUDA, but CUDA is unavailable." + if importlib.util.find_spec("curobo") is None: + return False, "cuRobo is not installed; install one of the cuRobo extras." + return True, None + + def build(self) -> None: + """Construct MotionGenerator without materializing its lazy backend.""" + values = self.spec.config + world_values = dict(values.get("world", {})) + auto_values = dict(values.get("auto_gen", {})) + if bool(world_values.get("multi_env", False)): + raise ValueError( + "free-space-common requires one shared empty cuRobo world " + "with world.multi_env=false." + ) + world = CuroboWorldCfg( + rigid_objects=None, + obstacle_representation=str( + world_values.get("obstacle_representation", "sphere") + ), + collision_cache=dict( + world_values.get("collision_cache", {"cuboid": 8, "mesh": 2}) + ), + dynamic_obstacle_names=[], + multi_env=False, + ) + planner_cfg = CuroboPlannerCfg( + robot_uid=self.context.robot.uid, + world=world, + auto_gen=CuroboAutoGenCfg(**auto_values), + collision_activation_distance=float( + values.get("collision_activation_distance", 0.01) + ), + max_attempts=int(values.get("max_attempts", 5)), + max_planning_time=values.get("max_planning_time"), + cuda_device=values.get("cuda_device"), + use_cuda_graph=bool(values.get("use_cuda_graph", True)), + cuda_graph_fallback=bool(values.get("cuda_graph_fallback", True)), + interpolation_dt=float(values.get("interpolation_dt", 0.025)), + preserve_plan_samples=bool(values.get("preserve_plan_samples", True)), + warmup_iterations=int(values.get("warmup_iterations", 1)), + ) + self.motion_generator = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg)) + + def prepare(self, case: BenchmarkCase) -> dict[str, object]: + """Materialize and warm the EEF backend without consuming a real case.""" + if self.motion_generator is None: + raise RuntimeError("cuRobo adapter must be built before prepare().") + planner = self.motion_generator.planner + return planner.prepare_backend( + control_part=self.context.control_part, + batch_size=case.batch_size, + move_type=MoveType.EEF_MOVE, + ) + + def plan(self, case: BenchmarkCase) -> PlanResult: + """Plan all ordered EEF waypoints in one MotionGenerator call.""" + if self.motion_generator is None: + raise RuntimeError("cuRobo adapter must be built before plan().") + targets = [ + PlanState.from_xpos(case.target_waypoints[:, index]) + for index in range(case.num_waypoints) + ] + return self.motion_generator.generate( + targets, + MotionGenOptions( + start_qpos=case.start_qpos, + control_part=self.context.control_part, + plan_opts=CuroboPlanOptions( + start_qpos=case.start_qpos, + control_part=self.context.control_part, + ), + ), + ) + + def close(self) -> None: + """Destroy cached cuRobo graph and planner resources.""" + if self.motion_generator is not None: + close_fn = getattr(self.motion_generator.planner, "close", None) + if close_fn is not None: + close_fn() + + +register_planner_adapter("curobo", CuroboAdapter) diff --git a/scripts/benchmark/planners/neural_planner/planners/ik_interpolate.py b/scripts/benchmark/planners/neural_planner/planners/ik_interpolate.py new file mode 100644 index 000000000..fa241b386 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/planners/ik_interpolate.py @@ -0,0 +1,76 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Sequential IK plus joint interpolation diagnostic adapter.""" + +from __future__ import annotations + +import torch + +from embodichain.lab.sim.planners import PlanResult +from embodichain.lab.sim.utility.action_utils import interpolate_with_distance + +from ..models import BenchmarkCase +from ..registry import register_planner_adapter +from .base import PlannerAdapter + +__all__ = ["IkInterpolateAdapter"] + + +class IkInterpolateAdapter(PlannerAdapter): + """Use the robot IK solver followed by fixed-count joint interpolation.""" + + capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) + + def build(self) -> None: + """No planner object is required beyond the benchmark robot.""" + + def plan(self, case: BenchmarkCase) -> PlanResult: + """Solve each waypoint sequentially while retaining per-env failures.""" + robot = self.context.robot + seed = case.start_qpos + alive = torch.ones(case.batch_size, dtype=torch.bool, device=robot.device) + targets = [seed] + for waypoint_index in range(case.num_waypoints): + success, solved = robot.compute_ik( + pose=case.target_waypoints[:, waypoint_index], + name=self.context.control_part, + joint_seed=seed, + ) + success_tensor = torch.as_tensor( + success, dtype=torch.bool, device=robot.device + ).flatten() + if success_tensor.numel() == 1 and case.batch_size > 1: + success_tensor = success_tensor.expand(case.batch_size) + alive &= success_tensor + solved = torch.as_tensor(solved, device=robot.device, dtype=seed.dtype) + seed = torch.where(success_tensor[:, None], solved, seed) + targets.append(seed) + + sparse_path = torch.stack(targets, dim=1) + positions = interpolate_with_distance( + trajectory=sparse_path, + interp_num=self.context.sample_interval, + device=robot.device, + ) + return PlanResult( + success=alive, + positions=positions, + duration=torch.zeros(case.batch_size, device=robot.device), + ) + + +register_planner_adapter("ik_interpolate", IkInterpolateAdapter) diff --git a/scripts/benchmark/planners/neural_planner/planners/neural.py b/scripts/benchmark/planners/neural_planner/planners/neural.py new file mode 100644 index 000000000..20af60396 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/planners/neural.py @@ -0,0 +1,55 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Capability-aware NMG adapter stub reserved for a future checkpoint.""" + +from __future__ import annotations + +from embodichain.lab.sim.planners import PlanResult + +from ..models import BenchmarkCase +from ..registry import register_planner_adapter +from .base import PlannerAdapter + +__all__ = ["NeuralAdapterStub"] + + +class NeuralAdapterStub(PlannerAdapter): + """Expose configurable NMG precision without initializing an unavailable model.""" + + capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) + model_revision = "not-ready" + + def availability(self) -> tuple[bool, str | None]: + """Mark the placeholder unsupported until the checkpoint contract lands.""" + pos_eps = float(self.spec.config.get("pos_eps", 0.05)) + rot_eps = float(self.spec.config.get("rot_eps", 0.3)) + return ( + False, + "NMG adapter is a stub pending the production checkpoint; " + f"configured pos_eps={pos_eps} m, rot_eps={rot_eps} rad.", + ) + + def build(self) -> None: + """Reject accidental construction of the explicit placeholder.""" + raise RuntimeError("The NMG adapter is not implemented yet.") + + def plan(self, case: BenchmarkCase) -> PlanResult: # noqa: ARG002 + """Reject accidental execution of the explicit placeholder.""" + raise RuntimeError("The NMG adapter is not implemented yet.") + + +register_planner_adapter("neural_stub", NeuralAdapterStub) diff --git a/scripts/benchmark/planners/neural_planner/planners/toppra.py b/scripts/benchmark/planners/neural_planner/planners/toppra.py new file mode 100644 index 000000000..cdf6662bc --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/planners/toppra.py @@ -0,0 +1,91 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""IK plus TOPPRA time-parameterization diagnostic adapter.""" + +from __future__ import annotations + +import importlib.util + +from embodichain.lab.sim.planners import ( + MotionGenCfg, + MotionGenOptions, + MotionGenerator, + PlanResult, + PlanState, + ToppraPlannerCfg, + ToppraPlanOptions, +) +from embodichain.lab.sim.planners.utils import TrajectorySampleMethod + +from ..config import PlannerSpecCfg +from ..models import BenchmarkCase +from ..registry import register_planner_adapter +from .base import PlannerAdapter, PlannerContext + +__all__ = ["ToppraAdapter"] + + +class ToppraAdapter(PlannerAdapter): + """Pre-interpolate EEF targets through IK, then run TOPPRA.""" + + capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) + + def __init__(self, spec: PlannerSpecCfg, context: PlannerContext) -> None: + super().__init__(spec, context) + self.motion_generator: MotionGenerator | None = None + + def availability(self) -> tuple[bool, str | None]: + """Report whether the optional TOPPRA package is installed.""" + if importlib.util.find_spec("toppra") is None: + return False, "TOPPRA is not installed." + return True, None + + def build(self) -> None: + """Construct the TOPPRA MotionGenerator.""" + self.motion_generator = MotionGenerator( + MotionGenCfg(planner_cfg=ToppraPlannerCfg(robot_uid=self.context.robot.uid)) + ) + + def plan(self, case: BenchmarkCase) -> PlanResult: + """Plan EEF waypoints through the existing IK-to-TOPPRA pipeline.""" + if self.motion_generator is None: + raise RuntimeError("TOPPRA adapter must be built before plan().") + config = self.spec.config + targets = [ + PlanState.from_xpos(case.target_waypoints[:, index]) + for index in range(case.num_waypoints) + ] + return self.motion_generator.generate( + targets, + MotionGenOptions( + start_qpos=case.start_qpos, + control_part=self.context.control_part, + is_interpolate=True, + is_linear=True, + plan_opts=ToppraPlanOptions( + constraints={ + "velocity": float(config.get("velocity", 0.2)), + "acceleration": float(config.get("acceleration", 0.5)), + }, + sample_method=TrajectorySampleMethod.QUANTITY, + sample_interval=self.context.sample_interval, + ), + ), + ) + + +register_planner_adapter("toppra", ToppraAdapter) diff --git a/scripts/benchmark/planners/neural_planner/registry.py b/scripts/benchmark/planners/neural_planner/registry.py new file mode 100644 index 000000000..ea2e3e425 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/registry.py @@ -0,0 +1,62 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Planner adapter registry used by the generic benchmark runner.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from .config import PlannerSpecCfg + from .planners.base import PlannerAdapter, PlannerContext + +__all__ = [ + "create_planner_adapter", + "planner_adapter_names", + "register_planner_adapter", +] + +_PLANNER_ADAPTERS: dict[str, type["PlannerAdapter"]] = {} + + +def register_planner_adapter(name: str, adapter_cls: type["PlannerAdapter"]) -> None: + """Register one adapter class under a stable configuration name.""" + if not name: + raise ValueError("Planner adapter name must not be empty.") + previous = _PLANNER_ADAPTERS.get(name) + if previous is not None and previous is not adapter_cls: + raise ValueError(f"Planner adapter {name!r} is already registered.") + _PLANNER_ADAPTERS[name] = adapter_cls + + +def planner_adapter_names() -> tuple[str, ...]: + """Return registered adapter names in deterministic order.""" + return tuple(sorted(_PLANNER_ADAPTERS)) + + +def create_planner_adapter( + spec: "PlannerSpecCfg", context: "PlannerContext" +) -> "PlannerAdapter": + """Construct the adapter selected by a planner specification.""" + try: + adapter_cls = _PLANNER_ADAPTERS[spec.adapter] + except KeyError as exc: + raise ValueError( + f"Unknown planner adapter {spec.adapter!r}; " + f"registered adapters: {planner_adapter_names()}." + ) from exc + return adapter_cls(spec=spec, context=context) diff --git a/scripts/benchmark/planners/neural_planner/reporting.py b/scripts/benchmark/planners/neural_planner/reporting.py new file mode 100644 index 000000000..4d9e8f7fe --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/reporting.py @@ -0,0 +1,156 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Render the free-space benchmark as exactly three Markdown tables.""" + +from __future__ import annotations + +import math +from datetime import datetime, timezone +from pathlib import Path + +from .config import SuiteCfg + +__all__ = ["write_markdown_report"] + +TIME_COLUMNS = ( + "track", + "algorithm", + "algorithm_role", + "batch_size", + "waypoint_count", + "num_trials", + "planner_construct_ms", + "backend_prepare_ms", + "cold_plan_ms", + "cost_time_ms", + "warm_plan_ms_p50", + "warm_plan_ms_p95", + "latency_per_env_ms", + "cost_time_per_segment_ms", + "trajectories_per_second", + "cpu_delta_mb", + "gpu_delta_mb", + "peak_gpu_mb", +) + +METRIC_COLUMNS = ( + "track", + "scenario", + "algorithm", + "algorithm_role", + "batch_size", + "waypoint_count", + "path_shape", + "cases", + "coverage_rate", + "success_rate", + "planning_success_rate", + "ordered_waypoint_success_rate", + "motion_valid_rate", + "waypoint_completion_rate", + "final_pos_err_mm", + "final_rot_err_deg", + "waypoint_pos_err_mm_p95", + "waypoint_rot_err_deg_p95", + "joint_violation_rate", + "joint_path_length_rad", + "cartesian_path_length_m", + "path_efficiency", + "top_failure", +) + +LEADERBOARD_COLUMNS = ( + "rank", + "track", + "algorithm", + "algorithm_role", + "model_revision", + "planner_config_hash", + "eligible", + "coverage_rate", + "overall_success_rate", + "planning_success_rate", + "motion_valid_rate", + "task_success_rate", + "latency_p95_ms", + "peak_gpu_mb", +) + + +def _format_value(column: str, value: object) -> str: + """Format one display value without mutating raw aggregate artifacts.""" + if value is None: + return "N/A" + if isinstance(value, bool): + return "true" if value else "false" + if isinstance(value, float): + if not math.isfinite(value): + return "N/A" + if column.endswith("_rate") or column in {"path_efficiency"}: + return f"{value:.2%}" + return f"{value:.6f}" + return str(value) + + +def _format_table(rows: list[dict[str, object]], columns: tuple[str, ...]) -> list[str]: + """Render one table with a stable schema even when rows are empty.""" + lines = [ + "| " + " | ".join(columns) + " |", + "| " + " | ".join(["---"] * len(columns)) + " |", + ] + for row in rows: + lines.append( + "| " + + " | ".join(_format_value(column, row.get(column)) for column in columns) + + " |" + ) + return lines + + +def write_markdown_report( + path: str | Path, + suite: SuiteCfg, + aggregates: dict[str, list[dict[str, object]]], + notes: list[str] | None = None, +) -> Path: + """Write one report containing exactly the required three tables.""" + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + lines = [ + "# Motion Generation Benchmark Report", + "", + f"Generated at: {datetime.now(timezone.utc).isoformat(timespec='seconds')}", + "", + f"- suite: `{suite.name}`", + f"- suite_version: `{suite.suite_version}`", + f"- profile: `{suite.profile}`", + f"- external position threshold: `{suite.protocol.position_threshold_m} m`", + f"- external rotation threshold: `{suite.protocol.rotation_threshold_rad} rad`", + "", + "## Time & Memory", + "", + ] + lines.extend(_format_table(aggregates["time_and_memory"], TIME_COLUMNS)) + lines.extend(["", "## Success & Other Metrics", ""]) + lines.extend(_format_table(aggregates["success_and_metrics"], METRIC_COLUMNS)) + lines.extend(["", "## Leaderboard", ""]) + lines.extend(_format_table(aggregates["leaderboard"], LEADERBOARD_COLUMNS)) + if notes: + lines.extend(["", "## Notes", ""]) + lines.extend(f"- {note}" for note in notes) + output.write_text("\n".join(lines) + "\n", encoding="utf-8") + return output diff --git a/scripts/benchmark/planners/neural_planner/run_benchmark.py b/scripts/benchmark/planners/neural_planner/run_benchmark.py index 45adceacd..544954e3d 100644 --- a/scripts/benchmark/planners/neural_planner/run_benchmark.py +++ b/scripts/benchmark/planners/neural_planner/run_benchmark.py @@ -14,1199 +14,208 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Benchmark NeuralPlanner in isolation on Franka Panda. +"""Run the extensible free-space motion-generation benchmark. -What this measures - Planning latency, memory, rollout steps, and final TCP pose error for - ``NeuralPlanner`` on fixed demo EEF waypoint sets. +cuRobo is the default primary baseline. IK interpolation and TOPPRA are +optional diagnostic baselines. NMG remains an explicitly configurable, +unsupported adapter stub until its production checkpoint contract is ready. -What this does not measure - Atomic-action task success, grasp physics, or obstacle avoidance. - -Default behavior - Only ``neural_planner`` is benchmarked. Use ``--compare-ik`` or - ``--compare-toppra`` to add optional baselines. - -Checkpoints are loaded from the ``dexforce/neural_motion_generator`` HuggingFace -repository unless ``--checkpoint-path`` is provided. - -Output - Markdown report under ``outputs/benchmarks/neural_planner_*.md``. - -Run:: - - embodichain benchmark planners-neural-planner +Run: ``embodichain benchmark planners-neural-planner --suite smoke`` """ from __future__ import annotations import argparse -import math -import os -import sys -import time -from collections import defaultdict -from collections.abc import Callable -from datetime import datetime +from copy import deepcopy from pathlib import Path - -import psutil -import torch - -from embodichain.data import get_data_path -from embodichain.data.assets.planner_assets import download_neural_planner_checkpoint -from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.cfg import RobotCfg -from embodichain.lab.sim.objects import Robot -from embodichain.lab.sim.planners import ( - MotionGenCfg, - MotionGenOptions, - MotionGenerator, - MoveType, - NeuralPlannerCfg, - PlanState, - ToppraPlannerCfg, - ToppraPlanOptions, +from typing import TYPE_CHECKING + +from .compat import ( + IMPL_IK, + IMPL_NEURAL, + IMPL_TOPPRA, + QUALITY_SUMMARY_COLUMNS, + aggregate_legacy_rows, + format_waypoint_grouped_tables, ) -from embodichain.lab.sim.planners.neural_planner import NeuralPlanOptions -from embodichain.lab.sim.planners.utils import PlanResult, TrajectorySampleMethod -from embodichain.lab.sim.utility.action_utils import interpolate_with_distance - -DEFAULT_NUM_WAYPOINTS = [1, 3, 5] -DEFAULT_NUM_TRIALS = 8 -DEFAULT_WARMUP_TRIALS = 1 -DEFAULT_SAMPLE_INTERVAL = 20 -ARM_NAME = "main_arm" -DEFAULT_START_QPOS = [ - 0.0, - -math.pi / 4, - 0.0, - -3 * math.pi / 4, - 0.0, - math.pi / 2, - math.pi / 4, +from .config import PlannerSpecCfg, SuiteCfg, load_suite +from .metrics.trajectory import compute_waypoint_errors, get_pose_err + +if TYPE_CHECKING: + from .runner import BenchmarkRunResult + +__all__ = [ + "IMPL_IK", + "IMPL_NEURAL", + "IMPL_TOPPRA", + "QUALITY_SUMMARY_COLUMNS", + "_aggregate_rows", + "_format_waypoint_grouped_tables", + "add_parser_arguments", + "compute_waypoint_errors", + "get_pose_err", + "run_all_benchmarks", + "run_from_args", ] -IMPL_NEURAL = "neural_planner" -IMPL_IK = "ik_interpolate" -IMPL_TOPPRA = "ik_toppra" +_aggregate_rows = aggregate_legacy_rows +_format_waypoint_grouped_tables = format_waypoint_grouped_tables -def _parse_args() -> argparse.Namespace: - """Parse command line arguments for neural motion generator benchmarks.""" - parser = argparse.ArgumentParser( - description="Benchmark NeuralPlanner planning latency and quality." +def add_parser_arguments(parser: argparse.ArgumentParser) -> None: + """Add free-space benchmark options to an existing argument parser.""" + parser.add_argument( + "--suite", + default="smoke", + help="Suite short name (smoke/coverage) or an explicit YAML path.", ) parser.add_argument( - "--device", - choices=("auto", "cpu", "cuda"), - default="auto", - help="Simulation and planner device. Auto uses CUDA when available.", + "--algorithms", + nargs="+", + default=None, + help="Override enabled suite algorithms by id.", ) parser.add_argument( - "--num-waypoints", + "--extra-baselines", nargs="+", - type=int, - default=DEFAULT_NUM_WAYPOINTS, - help="Number of EEF waypoints to sweep.", + choices=("ik_interpolate", "toppra"), + default=[], + help="Enable optional diagnostic baselines.", ) parser.add_argument( - "--num-trials", - type=int, - default=DEFAULT_NUM_TRIALS, - help="Measured trials per (impl, num_waypoints) configuration.", + "--device", + choices=("auto", "cpu", "cuda"), + default="auto", + help="Simulation device; cuRobo itself always requires CUDA.", + ) + parser.add_argument("--batch-sizes", nargs="+", type=int, default=None) + parser.add_argument("--num-waypoints", nargs="+", type=int, default=None) + parser.add_argument("--seeds", nargs="+", type=int, default=None) + parser.add_argument("--num-trials", type=int, default=None) + parser.add_argument("--warmup-trials", type=int, default=None) + parser.add_argument("--sample-interval", type=int, default=None) + parser.add_argument("--validation-samples", type=int, default=None) + parser.add_argument("--position-threshold-m", type=float, default=None) + parser.add_argument("--rotation-threshold-rad", type=float, default=None) + parser.add_argument( + "--nmg-pos-eps", + type=float, + default=None, + help="NMG internal waypoint position threshold in metres.", ) parser.add_argument( - "--warmup-trials", - type=int, - default=DEFAULT_WARMUP_TRIALS, - help="Warmup trials per configuration; excluded from summary aggregation.", + "--nmg-rot-eps", + type=float, + default=None, + help="NMG internal waypoint rotation threshold in radians.", ) parser.add_argument( - "--sample-interval", - type=int, - default=DEFAULT_SAMPLE_INTERVAL, - help="Resampled trajectory length for ik_interpolate and ik_toppra.", + "--checkpoint-path", + default=None, + help="Reserved NMG checkpoint path; the current NMG adapter remains a stub.", ) parser.add_argument( "--compare-ik", action="store_true", - help="Also benchmark sequential IK plus joint interpolation.", + help="Compatibility alias for --extra-baselines ik_interpolate.", ) parser.add_argument( "--compare-toppra", action="store_true", - help="Also benchmark EEF IK interpolation followed by TOPPRA.", + help="Compatibility alias for --extra-baselines toppra.", ) parser.add_argument( - "--save-trial-details", - action="store_true", - help="Include per-trial rows in the markdown report.", + "--output-root", default="outputs/benchmarks", help="Artifact root directory." ) parser.add_argument( - "--checkpoint-path", - type=str, - default=None, - help="Local neural planner checkpoint path. Skips HuggingFace download.", + "--headless", action="store_true", default=True, help="Run headlessly." ) parser.add_argument( - "--headless", - action="store_true", - default=True, - help="Run simulation headlessly (default: True).", + "--no-headless", action="store_false", dest="headless", help="Open a viewer." ) parser.add_argument( - "--no-headless", - action="store_false", - dest="headless", - help="Open the simulation viewer window.", - ) - return parser.parse_args() - - -def _resolve_device(device_name: str) -> str: - """Resolve requested device name to a simulation device string.""" - if device_name == "cuda" and not torch.cuda.is_available(): - raise RuntimeError("--device cuda was requested, but CUDA is unavailable.") - if device_name == "auto": - return "cuda" if torch.cuda.is_available() else "cpu" - return device_name - - -def _sync_cuda() -> None: - if torch.cuda.is_available(): - torch.cuda.synchronize() - - -def _reset_peak_gpu_memory() -> None: - if torch.cuda.is_available(): - torch.cuda.reset_peak_memory_stats() - - -def _peak_gpu_memory_mb() -> float: - if not torch.cuda.is_available(): - return 0.0 - return torch.cuda.max_memory_allocated() / 1024**2 - - -def _memory_snapshot() -> dict[str, float]: - process = psutil.Process(os.getpid()) - cpu_mb = process.memory_info().rss / 1024**2 - gpu_mb = ( - torch.cuda.memory_allocated() / 1024**2 if torch.cuda.is_available() else 0.0 - ) - return {"cpu_mb": cpu_mb, "gpu_mb": gpu_mb} - - -def _percentile(values: list[float], pct: float) -> float: - if not values: - return 0.0 - ordered = sorted(values) - index = max(0, min(len(ordered) - 1, math.ceil(pct / 100.0 * len(ordered)) - 1)) - return ordered[index] - - -def _format_markdown_table(rows: list[dict[str, object]]) -> list[str]: - if not rows: - return ["No data."] - - headers = list(rows[0].keys()) - lines = [ - "| " + " | ".join(headers) + " |", - "| " + " | ".join(["---"] * len(headers)) + " |", - ] - for row in rows: - lines.append("| " + " | ".join(str(row[h]) for h in headers) + " |") - return lines - - -QUALITY_SUMMARY_COLUMNS = ( - "impl", - "num_trials", - "success_rate", - "final_translation_err_mm_mean", - "final_rotation_err_deg_mean", - "mean_waypoint_pos_err_mm_mean", - "max_waypoint_pos_err_mm_mean", - "mean_waypoint_rot_err_deg_mean", - "max_waypoint_rot_err_deg_mean", -) - -PERFORMANCE_SUMMARY_COLUMNS = ( - "impl", - "num_trials", - "cost_time_ms_mean", - "cost_time_ms_p95", - "rollout_steps_mean", - "cpu_delta_mb_mean", - "gpu_delta_mb_mean", - "peak_gpu_mb_mean", - "peak_gpu_mb_max", -) - -_IMPL_REPORT_ORDER = { - IMPL_NEURAL: 0, - IMPL_IK: 1, - IMPL_TOPPRA: 2, -} - - -def _sort_summary_for_report( - rows: list[dict[str, object]], -) -> list[dict[str, object]]: - """Group summary rows by num_waypoints for side-by-side planner comparison.""" - return sorted( - rows, - key=lambda row: ( - int(row["num_waypoints"]), - _IMPL_REPORT_ORDER.get(str(row["impl"]), 99), - str(row["impl"]), - ), - ) - - -def _group_summary_by_waypoints( - rows: list[dict[str, object]], -) -> list[tuple[int, list[dict[str, object]]]]: - """Return summary rows grouped and sorted by num_waypoints.""" - groups: dict[int, list[dict[str, object]]] = defaultdict(list) - for row in rows: - groups[int(row["num_waypoints"])].append(row) - - grouped: list[tuple[int, list[dict[str, object]]]] = [] - for num_waypoints in sorted(groups): - group_rows = sorted( - groups[num_waypoints], - key=lambda row: ( - _IMPL_REPORT_ORDER.get(str(row["impl"]), 99), - str(row["impl"]), - ), - ) - grouped.append((num_waypoints, group_rows)) - return grouped - - -def _format_waypoint_grouped_tables( - summary_rows: list[dict[str, object]], - columns: tuple[str, ...], -) -> list[str]: - """Render one markdown table per num_waypoints value.""" - grouped = _group_summary_by_waypoints(summary_rows) - if not grouped: - return ["No data."] - - lines: list[str] = [] - for index, (num_waypoints, group_rows) in enumerate(grouped): - if index > 0: - lines.append("") - lines.extend( - [ - f"### num_waypoints = {num_waypoints}", - "", - ] - ) - lines.extend(_format_markdown_table(_project_table_rows(group_rows, columns))) - return lines - - -def _project_table_rows( - rows: list[dict[str, object]], - columns: tuple[str, ...], -) -> list[dict[str, object]]: - return [{column: row[column] for column in columns} for row in rows] - - -def _write_markdown_report( - benchmark_name: str, - trial_rows: list[dict[str, object]], - summary_rows: list[dict[str, object]], - quality_leaderboard_rows: list[dict[str, object]], - performance_leaderboard_rows: list[dict[str, object]], - notes: list[str] | None = None, - *, - include_trial_details: bool = False, -) -> Path: - output_dir = Path("outputs/benchmarks") - output_dir.mkdir(parents=True, exist_ok=True) - - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - report_path = output_dir / f"{benchmark_name}_{timestamp}.md" - - lines: list[str] = [ - f"# {benchmark_name} Benchmark Report", - "", - f"Generated at: {datetime.now().isoformat(timespec='seconds')}", - "", - "## Quality", - "", - ] - lines.extend(_format_waypoint_grouped_tables(summary_rows, QUALITY_SUMMARY_COLUMNS)) - lines.extend(["", "## Performance", ""]) - lines.extend( - _format_waypoint_grouped_tables(summary_rows, PERFORMANCE_SUMMARY_COLUMNS) - ) - lines.extend(["", "## Leaderboard (Quality)", ""]) - lines.extend(_format_markdown_table(quality_leaderboard_rows)) - lines.extend(["", "## Leaderboard (Performance)", ""]) - lines.extend(_format_markdown_table(performance_leaderboard_rows)) - if include_trial_details: - lines.extend(["", "## Trial Details", ""]) - lines.extend(_format_markdown_table(trial_rows)) - - if notes: - lines.extend(["", "## Notes", ""]) - lines.extend([f"- {note}" for note in notes]) - - report_path.write_text("\n".join(lines) + "\n", encoding="utf-8") - return report_path - - -def _franka_tcp() -> list[list[float]]: - c = math.cos(-math.pi / 4) - s = math.sin(-math.pi / 4) - return [ - [c, -s, 0.0, 0.0], - [s, c, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.1034], - [0.0, 0.0, 0.0, 1.0], - ] - - -def _create_franka(sim: SimulationManager) -> Robot: - urdf = get_data_path("Franka/Panda/PandaWithHand.urdf") - if not os.path.isfile(urdf): - raise FileNotFoundError(f"Franka URDF not found: {urdf}") - - cfg_dict = { - "fpath": urdf, - "control_parts": { - ARM_NAME: [ - "Joint1", - "Joint2", - "Joint3", - "Joint4", - "Joint5", - "Joint6", - "Joint7", - ], - }, - "solver_cfg": { - ARM_NAME: { - "class_type": "PytorchSolver", - "end_link_name": "ee_link", - "root_link_name": "base_link", - "tcp": _franka_tcp(), - }, - }, - } - return sim.add_robot(cfg=RobotCfg.from_dict(cfg_dict)) - - -def _make_waypoints(start_pose: torch.Tensor, num_waypoints: int) -> torch.Tensor: - offsets = torch.tensor( - [ - [0.10, 0.00, 0.00], - [0.10, 0.10, 0.00], - [0.00, 0.10, -0.08], - [-0.10, 0.10, -0.08], - [-0.10, 0.00, 0.00], - [0.00, -0.10, 0.00], - [0.10, -0.10, -0.06], - [0.00, 0.00, -0.12], - ], - dtype=start_pose.dtype, - device=start_pose.device, - ) - num_waypoints = max(1, min(int(num_waypoints), offsets.shape[0])) - waypoints = start_pose.unsqueeze(0).repeat(num_waypoints, 1, 1) - waypoints[:, :3, 3] += offsets[:num_waypoints] - return waypoints - - -def _make_target_states(waypoints: torch.Tensor) -> list[PlanState]: - return [ - PlanState.single(move_type=MoveType.EEF_MOVE, xpos=waypoint) - for waypoint in waypoints - ] - - -def get_pose_err( - matrix_a: torch.Tensor, - matrix_b: torch.Tensor, -) -> tuple[float, float]: - """Return translation (m) and rotation (rad) errors between paired 4x4 poses.""" - tensor_a = torch.as_tensor(matrix_a, dtype=torch.float64) - tensor_b = torch.as_tensor(matrix_b, dtype=torch.float64, device=tensor_a.device) - - if tensor_a.ndim == 2: - tensor_a = tensor_a.unsqueeze(0) - if tensor_b.ndim == 2: - tensor_b = tensor_b.unsqueeze(0) - - t_err = torch.linalg.norm(tensor_a[:, :3, 3] - tensor_b[:, :3, 3], dim=-1) - relative_rot = torch.matmul( - tensor_a[:, :3, :3].transpose(-1, -2), - tensor_b[:, :3, :3], - ) - trace = torch.diagonal(relative_rot, dim1=-2, dim2=-1).sum(dim=-1) - cos_angle = torch.clamp((trace - 1.0) / 2.0, min=-1.0, max=1.0) - r_err = torch.arccos(cos_angle) - return float(t_err.item()), float(r_err.item()) - - -def _resolve_checkpoint(checkpoint_path: str | None) -> str | None: - if checkpoint_path: - path = Path(checkpoint_path) - if not path.is_file(): - print(f"Checkpoint not found: {path}") - return None - return str(path) - - try: - return download_neural_planner_checkpoint() - except RuntimeError as exc: - print(str(exc)) - print( - "Neural planner benchmark skipped: checkpoint unavailable.\n" - "Provide --checkpoint-path or configure HF_TOKEN after accepting the " - "model license at https://huggingface.co/dexforce/neural_motion_generator" - ) - return None - - -def _setup_sim_and_robot( - sim_device: str, - headless: bool, -) -> tuple[SimulationManager, Robot, torch.Tensor, torch.Tensor]: - sim = SimulationManager( - SimulationManagerCfg( - headless=headless, - sim_device=sim_device, - num_envs=1, - arena_space=2.0, - ) - ) - robot = _create_franka(sim) - start_qpos = torch.tensor( - DEFAULT_START_QPOS, - dtype=torch.float32, - device=robot.device, - ) - robot.set_qpos( - qpos=start_qpos.unsqueeze(0), - joint_ids=robot.get_joint_ids(ARM_NAME), - ) - sim.update(step=1) - - start_pose = robot.compute_fk( - qpos=start_qpos.unsqueeze(0), - name=ARM_NAME, - to_matrix=True, - )[0] - return sim, robot, start_qpos, start_pose - - -def _neural_plan_options(start_qpos: torch.Tensor) -> MotionGenOptions: - return MotionGenOptions( - control_part=ARM_NAME, - start_qpos=start_qpos, - plan_opts=NeuralPlanOptions( - control_part=ARM_NAME, - start_qpos=start_qpos, - ), - ) - - -def _toppra_motion_options( - start_qpos: torch.Tensor, - sample_interval: int, -) -> MotionGenOptions: - # EEF waypoints are IK-interpolated inside MotionGenerator, then TOPPRA - # time-parameterizes the resulting joint path. This differs from the atomic - # action default arm path (sequential IK + joint interpolation only). - return MotionGenOptions( - control_part=ARM_NAME, - start_qpos=start_qpos, - is_interpolate=True, - is_linear=True, - plan_opts=ToppraPlanOptions( - constraints={"velocity": 0.2, "acceleration": 0.5}, - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=sample_interval, - ), + "--save-trial-details", + action="store_true", + help="Deprecated: numeric trial details are always saved to trials.jsonl.", ) -def _all_success(success: bool | torch.Tensor) -> bool: - if isinstance(success, torch.Tensor): - return bool(torch.all(success).item()) - return bool(success) - - -def plan_ik_interpolate( - robot: Robot, - waypoints: torch.Tensor, - start_qpos: torch.Tensor, - sample_interval: int, -) -> PlanResult: - """Plan via sequential IK followed by joint-space interpolation.""" - qpos_seed = start_qpos.unsqueeze(0) - joint_targets = [start_qpos] - for waypoint in waypoints: - success, qpos = robot.compute_ik( - pose=waypoint.unsqueeze(0), - name=ARM_NAME, - joint_seed=qpos_seed, - ) - if not _all_success(success): - return PlanResult( - success=torch.tensor([False]), - positions=None, - duration=torch.tensor([0.0]), - ) - qpos_seed = qpos - joint_targets.append(qpos.squeeze(0)) - - trajectory = torch.stack(joint_targets, dim=0).unsqueeze(0) - positions = interpolate_with_distance( - trajectory=trajectory, - interp_num=sample_interval, - device=robot.device, - ) - return PlanResult( - success=torch.tensor([True]), - positions=positions, - duration=torch.tensor([0.0]), +def _planner_by_id(suite: SuiteCfg, planner_id: str) -> PlannerSpecCfg: + """Resolve one planner id from the suite with an actionable error.""" + for spec in suite.planners: + if spec.id == planner_id: + return spec + raise ValueError( + f"Suite {suite.name!r} does not declare planner {planner_id!r}; " + f"available ids: {[spec.id for spec in suite.planners]}." ) -def _trajectory_fk_poses(result: PlanResult, robot: Robot) -> list[torch.Tensor]: - """Return TCP poses sampled along the planned trajectory.""" - if result.xpos_list is not None and result.xpos_list.shape[0] > 0: - return [pose for pose in result.xpos_list[0]] - if result.positions is None or result.positions.shape[1] == 0: - return [] - qpos = result.positions[0] - if qpos.dim() == 1: - qpos = qpos.unsqueeze(0) - fk = robot.compute_batch_fk( - qpos=qpos.unsqueeze(0), - name=ARM_NAME, - to_matrix=True, - ).squeeze(0) - return [fk[i] for i in range(fk.shape[0])] - - -def compute_waypoint_errors( - trajectory_poses: list[torch.Tensor], - waypoints: torch.Tensor, -) -> dict[str, float]: - """Compute best-hit pose errors for each target waypoint along a trajectory. - - For every target waypoint, scan all trajectory TCP samples and record the - smallest translation/rotation error. Return the mean and max across waypoints. - """ - empty = { - "mean_waypoint_pos_err_mm": float("inf"), - "max_waypoint_pos_err_mm": float("inf"), - "mean_waypoint_rot_err_deg": float("inf"), - "max_waypoint_rot_err_deg": float("inf"), - } - if not trajectory_poses or waypoints.numel() == 0: - return empty - - waypoint_pos_errors_mm: list[float] = [] - waypoint_rot_errors_deg: list[float] = [] - for waypoint in waypoints: - best_pos_m = float("inf") - best_rot_rad = float("inf") - for pose in trajectory_poses: - pos_m, rot_rad = get_pose_err(pose, waypoint) - best_pos_m = min(best_pos_m, pos_m) - best_rot_rad = min(best_rot_rad, rot_rad) - waypoint_pos_errors_mm.append(best_pos_m * 1000.0) - waypoint_rot_errors_deg.append(best_rot_rad * 180.0 / math.pi) - - return { - "mean_waypoint_pos_err_mm": sum(waypoint_pos_errors_mm) - / len(waypoint_pos_errors_mm), - "max_waypoint_pos_err_mm": max(waypoint_pos_errors_mm), - "mean_waypoint_rot_err_deg": sum(waypoint_rot_errors_deg) - / len(waypoint_rot_errors_deg), - "max_waypoint_rot_err_deg": max(waypoint_rot_errors_deg), - } - - -def _final_eef_pose( - result: PlanResult, - robot: Robot, -) -> torch.Tensor | None: - if result.xpos_list is not None and result.xpos_list.shape[0] > 0: - return result.xpos_list[0][-1] - if result.positions is None or result.positions.shape[1] == 0: - return None - qpos = result.positions[0][-1] - if qpos.dim() == 1: - qpos = qpos.unsqueeze(0) - return robot.compute_fk(qpos=qpos, name=ARM_NAME, to_matrix=True)[0] - - -def _compute_result_metrics( - result: PlanResult, - waypoints: torch.Tensor, - robot: Robot, -) -> dict[str, object]: - success = bool(result.success.all().item()) - rollout_steps = ( - int(result.positions.shape[1]) if result.positions is not None else 0 +def _resolve_planners( + suite: SuiteCfg, + algorithms: list[str] | None, + extra_baselines: list[str], +) -> list[PlannerSpecCfg]: + """Resolve enabled algorithms while retaining suite ordering and roles.""" + selected_ids = ( + list(algorithms) + if algorithms is not None + else [spec.id for spec in suite.planners if spec.enabled] ) - duration_s = float(result.duration[0].item()) - - trajectory_poses = _trajectory_fk_poses(result, robot) - waypoint_errors = compute_waypoint_errors(trajectory_poses, waypoints) - - final_pose = _final_eef_pose(result, robot) - last_waypoint = waypoints[-1] - if final_pose is not None: - t_err_m, r_err_rad = get_pose_err(final_pose, last_waypoint) - translation_err_mm = t_err_m * 1000.0 - rotation_err_deg = r_err_rad * 180.0 / math.pi - else: - translation_err_mm = float("inf") - rotation_err_deg = float("inf") - - return { - "success": success, - "translation_err_mm": translation_err_mm, - "rotation_err_deg": rotation_err_deg, - "mean_waypoint_pos_err_mm": waypoint_errors["mean_waypoint_pos_err_mm"], - "max_waypoint_pos_err_mm": waypoint_errors["max_waypoint_pos_err_mm"], - "mean_waypoint_rot_err_deg": waypoint_errors["mean_waypoint_rot_err_deg"], - "max_waypoint_rot_err_deg": waypoint_errors["max_waypoint_rot_err_deg"], - "rollout_steps": rollout_steps, - "duration_s": duration_s, - } + selected_ids = list(dict.fromkeys(selected_ids)) + for planner_id in extra_baselines: + if planner_id not in selected_ids: + selected_ids.append(planner_id) + if not selected_ids: + raise ValueError("No algorithms were selected for the benchmark.") + return [deepcopy(_planner_by_id(suite, planner_id)) for planner_id in selected_ids] -def _timed_plan( - plan_fn: Callable[[], PlanResult], -) -> tuple[float, dict[str, float], float, PlanResult]: - _reset_peak_gpu_memory() - mem_before = _memory_snapshot() - _sync_cuda() - - start = time.perf_counter() - result = plan_fn() - _sync_cuda() - elapsed = time.perf_counter() - start - - mem_after = _memory_snapshot() - deltas = { - "cpu_mb": mem_after["cpu_mb"] - mem_before["cpu_mb"], - "gpu_mb": mem_after["gpu_mb"] - mem_before["gpu_mb"], - } - return elapsed, deltas, _peak_gpu_memory_mb(), result - - -def _format_finite(value: object) -> str: - numeric = float(value) - return f"{numeric:.6f}" if math.isfinite(numeric) else "inf" - - -def _append_trial_row( - trial_rows: list[dict[str, object]], +def _apply_overrides( + suite: SuiteCfg, *, - impl: str, - num_waypoints: int, - trial_id: int, - warmup: bool, - elapsed_s: float, - mem_deltas: dict[str, float], - peak_gpu_mb: float, - metrics: dict[str, object], -) -> None: - trial_rows.append( - { - "impl": impl, - "num_waypoints": num_waypoints, - "trial_id": trial_id, - "warmup": warmup, - "cost_time_ms": f"{elapsed_s * 1000.0:.6f}", - "cpu_delta_mb": f"{mem_deltas['cpu_mb']:.6f}", - "gpu_delta_mb": f"{mem_deltas['gpu_mb']:.6f}", - "peak_gpu_mb": f"{peak_gpu_mb:.6f}", - "success": metrics["success"], - "final_translation_err_mm": _format_finite(metrics["translation_err_mm"]), - "final_rotation_err_deg": _format_finite(metrics["rotation_err_deg"]), - "mean_waypoint_pos_err_mm": _format_finite( - metrics["mean_waypoint_pos_err_mm"] - ), - "max_waypoint_pos_err_mm": _format_finite( - metrics["max_waypoint_pos_err_mm"] - ), - "mean_waypoint_rot_err_deg": _format_finite( - metrics["mean_waypoint_rot_err_deg"] - ), - "max_waypoint_rot_err_deg": _format_finite( - metrics["max_waypoint_rot_err_deg"] - ), - "rollout_steps": metrics["rollout_steps"], - "duration_s": f"{float(metrics['duration_s']):.6f}", - } - ) - - -def _aggregate_rows( - trial_rows: list[dict[str, object]], -) -> list[dict[str, object]]: - """Aggregate measured trials by (impl, num_waypoints).""" - groups: dict[tuple[str, int], list[dict[str, object]]] = defaultdict(list) - for row in trial_rows: - if bool(row["warmup"]): - continue - key = (str(row["impl"]), int(row["num_waypoints"])) - groups[key].append(row) - - summary_rows: list[dict[str, object]] = [] - for (impl, num_waypoints), rows in sorted(groups.items()): - costs = [float(row["cost_time_ms"]) for row in rows] - successes = [bool(row["success"]) for row in rows] - rollout_steps = [int(row["rollout_steps"]) for row in rows] - t_errs = [ - float(row["final_translation_err_mm"]) - for row in rows - if math.isfinite(float(row["final_translation_err_mm"])) - ] - r_errs = [ - float(row["final_rotation_err_deg"]) - for row in rows - if math.isfinite(float(row["final_rotation_err_deg"])) - ] - mean_wp_pos = [ - float(row["mean_waypoint_pos_err_mm"]) - for row in rows - if math.isfinite(float(row["mean_waypoint_pos_err_mm"])) - ] - max_wp_pos = [ - float(row["max_waypoint_pos_err_mm"]) - for row in rows - if math.isfinite(float(row["max_waypoint_pos_err_mm"])) - ] - mean_wp_rot = [ - float(row["mean_waypoint_rot_err_deg"]) - for row in rows - if math.isfinite(float(row["mean_waypoint_rot_err_deg"])) - ] - max_wp_rot = [ - float(row["max_waypoint_rot_err_deg"]) - for row in rows - if math.isfinite(float(row["max_waypoint_rot_err_deg"])) - ] - cpu_deltas = [float(row["cpu_delta_mb"]) for row in rows] - gpu_deltas = [float(row["gpu_delta_mb"]) for row in rows] - peak_gpus = [float(row["peak_gpu_mb"]) for row in rows] - - summary_rows.append( - { - "impl": impl, - "num_waypoints": num_waypoints, - "num_trials": len(rows), - "success_rate": f"{sum(successes) / max(len(rows), 1):.2%}", - "cost_time_ms_mean": f"{sum(costs) / len(costs):.6f}", - "cost_time_ms_p95": f"{_percentile(costs, 95.0):.6f}", - "rollout_steps_mean": f"{sum(rollout_steps) / len(rollout_steps):.2f}", - "cpu_delta_mb_mean": f"{sum(cpu_deltas) / len(cpu_deltas):.6f}", - "gpu_delta_mb_mean": f"{sum(gpu_deltas) / len(gpu_deltas):.6f}", - "peak_gpu_mb_mean": f"{sum(peak_gpus) / len(peak_gpus):.6f}", - "peak_gpu_mb_max": f"{max(peak_gpus):.6f}", - "final_translation_err_mm_mean": ( - f"{sum(t_errs) / len(t_errs):.6f}" if t_errs else "inf" - ), - "final_rotation_err_deg_mean": ( - f"{sum(r_errs) / len(r_errs):.6f}" if r_errs else "inf" - ), - "mean_waypoint_pos_err_mm_mean": ( - f"{sum(mean_wp_pos) / len(mean_wp_pos):.6f}" - if mean_wp_pos - else "inf" - ), - "max_waypoint_pos_err_mm_mean": ( - f"{sum(max_wp_pos) / len(max_wp_pos):.6f}" if max_wp_pos else "inf" - ), - "mean_waypoint_rot_err_deg_mean": ( - f"{sum(mean_wp_rot) / len(mean_wp_rot):.6f}" - if mean_wp_rot - else "inf" - ), - "max_waypoint_rot_err_deg_mean": ( - f"{sum(max_wp_rot) / len(max_wp_rot):.6f}" if max_wp_rot else "inf" - ), - } - ) - return _sort_summary_for_report(summary_rows) - - -def _group_summary_by_impl( - summary_rows: list[dict[str, object]], -) -> dict[str, list[dict[str, object]]]: - by_impl: dict[str, list[dict[str, object]]] = defaultdict(list) - for row in summary_rows: - by_impl[str(row["impl"])].append(row) - return by_impl - - -def _build_quality_leaderboard_rows( - summary_rows: list[dict[str, object]], -) -> list[dict[str, object]]: - """Rank planners by mean success rate, then lower waypoint error.""" - leaderboard: list[tuple[float, float, str, dict[str, object]]] = [] - for impl, rows in _group_summary_by_impl(summary_rows).items(): - success_rates = [ - float(str(row["success_rate"]).strip("%")) / 100.0 for row in rows - ] - t_errs = [ - float(row["final_translation_err_mm_mean"]) - for row in rows - if math.isfinite(float(row["final_translation_err_mm_mean"])) - ] - r_errs = [ - float(row["final_rotation_err_deg_mean"]) - for row in rows - if math.isfinite(float(row["final_rotation_err_deg_mean"])) - ] - wp_pos = [ - float(row["mean_waypoint_pos_err_mm_mean"]) - for row in rows - if math.isfinite(float(row["mean_waypoint_pos_err_mm_mean"])) - ] - overall_success = sum(success_rates) / len(success_rates) - avg_wp_pos = sum(wp_pos) / len(wp_pos) if wp_pos else math.inf - leaderboard.append( - ( - overall_success, - -avg_wp_pos, - impl, - { - "overall_success_rate": f"{overall_success:.2%}", - "avg_final_translation_err_mm": ( - f"{sum(t_errs) / len(t_errs):.6f}" if t_errs else "inf" - ), - "avg_final_rotation_err_deg": ( - f"{sum(r_errs) / len(r_errs):.6f}" if r_errs else "inf" - ), - "avg_mean_waypoint_pos_err_mm": ( - f"{sum(wp_pos) / len(wp_pos):.6f}" if wp_pos else "inf" - ), - }, - ) - ) - - ranked = sorted(leaderboard, key=lambda item: (item[0], item[1]), reverse=True) - return [ - {"rank": rank, "algorithm": impl, **stats} - for rank, (_, _, impl, stats) in enumerate(ranked, start=1) - ] - - -def _build_performance_leaderboard_rows( - summary_rows: list[dict[str, object]], -) -> list[dict[str, object]]: - """Rank planners by lower mean planning latency.""" - leaderboard: list[tuple[float, str, dict[str, object]]] = [] - for impl, rows in _group_summary_by_impl(summary_rows).items(): - costs = [float(row["cost_time_ms_mean"]) for row in rows] - p95_costs = [float(row["cost_time_ms_p95"]) for row in rows] - cpu_deltas = [float(row["cpu_delta_mb_mean"]) for row in rows] - gpu_deltas = [float(row["gpu_delta_mb_mean"]) for row in rows] - peak_gpus = [float(row["peak_gpu_mb_mean"]) for row in rows] - avg_cost = sum(costs) / len(costs) - leaderboard.append( - ( - -avg_cost, - impl, - { - "avg_cost_time_ms": f"{avg_cost:.6f}", - "p95_cost_time_ms": f"{sum(p95_costs) / len(p95_costs):.6f}", - "avg_cpu_delta_mb": f"{sum(cpu_deltas) / len(cpu_deltas):.6f}", - "avg_gpu_delta_mb": f"{sum(gpu_deltas) / len(gpu_deltas):.6f}", - "avg_peak_gpu_mb": f"{sum(peak_gpus) / len(peak_gpus):.6f}", - }, - ) - ) - - ranked = sorted(leaderboard, key=lambda item: item[0], reverse=True) - return [ - {"rank": rank, "algorithm": impl, **stats} - for rank, (_, impl, stats) in enumerate(ranked, start=1) - ] - - -def _print_run_summary( - impl: str, - num_waypoints: int, - trial_id: int, - warmup: bool, - elapsed_s: float, - mem_deltas: dict[str, float], - peak_gpu_mb: float, - metrics: dict[str, object], -) -> None: - label = "warmup" if warmup else f"trial={trial_id}" - print(f"**** {impl} num_waypoints={num_waypoints} {label}") - print( - f"===Plan time: {elapsed_s * 1000.0:.6f} ms " - f"success={metrics['success']} " - f"steps={metrics['rollout_steps']}" - ) - print( - " " - f"CPU Δ={mem_deltas['cpu_mb']:+.1f} MB " - f"GPU Δ={mem_deltas['gpu_mb']:+.1f} MB " - f"peak GPU={peak_gpu_mb:.1f} MB" - ) - if math.isfinite(float(metrics["translation_err_mm"])): - print( - " " - f"Final waypoint error: {float(metrics['translation_err_mm']):.6f} mm " - f"{float(metrics['rotation_err_deg']):.6f} deg" - ) - if math.isfinite(float(metrics["mean_waypoint_pos_err_mm"])): - print( - " " - f"Mean waypoint error: {float(metrics['mean_waypoint_pos_err_mm']):.6f} mm " - f"{float(metrics['mean_waypoint_rot_err_deg']):.6f} deg " - f"(max {float(metrics['max_waypoint_pos_err_mm']):.6f} mm / " - f"{float(metrics['max_waypoint_rot_err_deg']):.6f} deg)" - ) - - -def _run_impl_trials( - *, - impl: str, - num_waypoints: int, - num_trials: int, - warmup_trials: int, - plan_fn: Callable[[], PlanResult], - robot: Robot, - waypoints: torch.Tensor, - trial_rows: list[dict[str, object]], + batch_sizes: list[int] | None = None, + num_waypoints: list[int] | None = None, + seeds: list[int] | None = None, + num_trials: int | None = None, + warmup_trials: int | None = None, + sample_interval: int | None = None, + validation_samples: int | None = None, + position_threshold_m: float | None = None, + rotation_threshold_rad: float | None = None, + nmg_pos_eps: float | None = None, + nmg_rot_eps: float | None = None, + checkpoint_path: str | None = None, ) -> None: - measured_trial_id = 0 - for trial_idx in range(warmup_trials + num_trials): - warmup = trial_idx < warmup_trials - elapsed, mem_deltas, peak_gpu, result = _timed_plan(plan_fn) - metrics = _compute_result_metrics(result, waypoints, robot) - if not warmup: - _print_run_summary( - impl, - num_waypoints, - measured_trial_id, - warmup, - elapsed, - mem_deltas, - peak_gpu, - metrics, - ) - measured_trial_id += 1 - _append_trial_row( - trial_rows, - impl=impl, - num_waypoints=num_waypoints, - trial_id=trial_idx, - warmup=warmup, - elapsed_s=elapsed, - mem_deltas=mem_deltas, - peak_gpu_mb=peak_gpu, - metrics=metrics, - ) - - -def _init_toppra_motion_generator( - robot: Robot, - start_qpos: torch.Tensor, - sample_interval: int, - notes: list[str], -) -> tuple[MotionGenerator | None, MotionGenOptions | None]: - try: - return ( - MotionGenerator( - cfg=MotionGenCfg( - planner_cfg=ToppraPlannerCfg( - robot_uid=robot.uid, - ) - ) - ), - _toppra_motion_options(start_qpos, sample_interval), - ) - except Exception as exc: - notes.append( - f"Toppra comparison skipped: {exc}. " - "Install with `pip install toppra==0.6.3`." - ) - print(f"Toppra comparison skipped: {exc}") - return None, None - - -def _benchmark_notes( - *, - sim_device: str, - checkpoint_path: str, - num_trials: int, - warmup_trials: int, - sample_interval: int, - compare_ik: bool, - compare_toppra: bool, -) -> list[str]: - impls = [IMPL_NEURAL] - if compare_ik: - impls.append(IMPL_IK) - if compare_toppra: - impls.append(IMPL_TOPPRA) - - checkpoint_name = Path(checkpoint_path).name - return [ - f"Device: {sim_device} | Robot: Franka Panda ({ARM_NAME})", - f"Checkpoint: {checkpoint_name} ({checkpoint_path})", - f"Trials: {warmup_trials} warmup + {num_trials} measured per " - f"(impl, num_waypoints); sample_interval={sample_interval}", - f"Planners: {', '.join(impls)}", - "success_rate follows each planner; pose errors are FK vs target waypoints.", - ] - - -def benchmark_neural_planner( - num_waypoints_list: list[int], - sim_device: str, - headless: bool, - checkpoint_path: str | None, - *, - num_trials: int = DEFAULT_NUM_TRIALS, - warmup_trials: int = DEFAULT_WARMUP_TRIALS, - sample_interval: int = DEFAULT_SAMPLE_INTERVAL, - compare_ik: bool = False, - compare_toppra: bool = False, -) -> ( - tuple[ - list[dict[str, object]], - list[dict[str, object]], - list[dict[str, object]], - list[dict[str, object]], - list[str], - ] - | None -): - resolved_checkpoint = _resolve_checkpoint(checkpoint_path) - if resolved_checkpoint is None: - return None - - if num_trials < 1: - raise ValueError("--num-trials must be >= 1.") - if warmup_trials < 0: - raise ValueError("--warmup-trials must be >= 0.") - if sample_interval < 1: - raise ValueError("--sample-interval must be >= 1.") - - trial_rows: list[dict[str, object]] = [] - notes = _benchmark_notes( - sim_device=sim_device, - checkpoint_path=resolved_checkpoint, - num_trials=num_trials, - warmup_trials=warmup_trials, - sample_interval=sample_interval, - compare_ik=compare_ik, - compare_toppra=compare_toppra, - ) - - print("\n=== NeuralPlanner Benchmark ===") - print(f"Device: {sim_device}") - print(f"Checkpoint: {resolved_checkpoint}") - print( - "num_waypoints values: " - f"{', '.join(str(value) for value in num_waypoints_list)}" - ) - print(f"num_trials={num_trials} warmup_trials={warmup_trials}") - - _, robot, start_qpos, start_pose = _setup_sim_and_robot(sim_device, headless) - - neural_planner = MotionGenerator( - cfg=MotionGenCfg( - planner_cfg=NeuralPlannerCfg( - robot_uid=robot.uid, - checkpoint_path=resolved_checkpoint, - control_part=ARM_NAME, - ) - ) - ) - neural_options = _neural_plan_options(start_qpos) - - toppra_motion_generator: MotionGenerator | None = None - toppra_options: MotionGenOptions | None = None - if compare_toppra: - toppra_motion_generator, toppra_options = _init_toppra_motion_generator( - robot, - start_qpos, - sample_interval, - notes, - ) - - for num_waypoints in num_waypoints_list: - waypoints = _make_waypoints(start_pose, num_waypoints) - target_states = _make_target_states(waypoints) - - _run_impl_trials( - impl=IMPL_NEURAL, - num_waypoints=num_waypoints, - num_trials=num_trials, - warmup_trials=warmup_trials, - plan_fn=lambda ts=target_states, opts=neural_options: neural_planner.generate( # noqa: E501 - target_states=ts, - options=opts, - ), - robot=robot, - waypoints=waypoints, - trial_rows=trial_rows, - ) - - if compare_ik: - _run_impl_trials( - impl=IMPL_IK, - num_waypoints=num_waypoints, - num_trials=num_trials, - warmup_trials=warmup_trials, - plan_fn=lambda wp=waypoints, sq=start_qpos, si=sample_interval: plan_ik_interpolate( # noqa: E501 - robot, - wp, - sq, - si, - ), - robot=robot, - waypoints=waypoints, - trial_rows=trial_rows, - ) - - if toppra_motion_generator is not None and toppra_options is not None: - _run_impl_trials( - impl=IMPL_TOPPRA, - num_waypoints=num_waypoints, - num_trials=num_trials, - warmup_trials=warmup_trials, - plan_fn=lambda ts=target_states, opts=toppra_options: toppra_motion_generator.generate( # noqa: E501 - target_states=ts, - options=opts, - ), - robot=robot, - waypoints=waypoints, - trial_rows=trial_rows, - ) - - summary_rows = _aggregate_rows(trial_rows) - return ( - trial_rows, - summary_rows, - _build_quality_leaderboard_rows(summary_rows), - _build_performance_leaderboard_rows(summary_rows), - notes, - ) + """Apply explicit CLI/programmatic overrides to a loaded suite.""" + if batch_sizes is not None: + suite.free_space.batch_sizes = batch_sizes + if num_waypoints is not None: + suite.free_space.waypoint_counts = num_waypoints + if seeds is not None: + suite.free_space.seeds = seeds + if num_trials is not None: + suite.protocol.measured_trials = num_trials + if warmup_trials is not None: + suite.protocol.warmup_trials = warmup_trials + if sample_interval is not None: + suite.protocol.sample_interval = sample_interval + if validation_samples is not None: + suite.protocol.validation_samples = validation_samples + if position_threshold_m is not None: + suite.protocol.position_threshold_m = position_threshold_m + if rotation_threshold_rad is not None: + suite.protocol.rotation_threshold_rad = rotation_threshold_rad + + nmg = next((spec for spec in suite.planners if spec.id == "nmg"), None) + if nmg is not None: + if nmg_pos_eps is not None: + nmg.config["pos_eps"] = nmg_pos_eps + if nmg_rot_eps is not None: + nmg.config["rot_eps"] = nmg_rot_eps + if checkpoint_path is not None: + nmg.config["checkpoint_path"] = str(Path(checkpoint_path)) + suite.validate_benchmark() def run_all_benchmarks( @@ -1215,70 +224,93 @@ def run_all_benchmarks( headless: bool = True, checkpoint_path: str | None = None, *, - num_trials: int = DEFAULT_NUM_TRIALS, - warmup_trials: int = DEFAULT_WARMUP_TRIALS, - sample_interval: int = DEFAULT_SAMPLE_INTERVAL, + suite_name: str = "smoke", + algorithms: list[str] | None = None, + extra_baselines: list[str] | None = None, + batch_sizes: list[int] | None = None, + seeds: list[int] | None = None, + num_trials: int | None = None, + warmup_trials: int | None = None, + sample_interval: int | None = None, + validation_samples: int | None = None, + position_threshold_m: float | None = None, + rotation_threshold_rad: float | None = None, + nmg_pos_eps: float | None = None, + nmg_rot_eps: float | None = None, compare_ik: bool = False, compare_toppra: bool = False, - include_trial_details: bool = False, -) -> None: - device = _resolve_device(sim_device) - num_waypoints_list = num_waypoints_list or DEFAULT_NUM_WAYPOINTS - - print("=" * 60) - print("NeuralPlanner Performance Benchmarks") - print("=" * 60) - - result = benchmark_neural_planner( - num_waypoints_list=num_waypoints_list, - sim_device=device, - headless=headless, - checkpoint_path=checkpoint_path, + include_trial_details: bool = False, # noqa: ARG001 - compatibility parameter + output_root: str | Path = "outputs/benchmarks", +) -> BenchmarkRunResult: + """Resolve configuration and run all selected free-space benchmarks.""" + from .runner import BenchmarkRunner + + suite = load_suite(suite_name) + _apply_overrides( + suite, + batch_sizes=batch_sizes, + num_waypoints=num_waypoints_list, + seeds=seeds, num_trials=num_trials, warmup_trials=warmup_trials, sample_interval=sample_interval, - compare_ik=compare_ik, - compare_toppra=compare_toppra, + validation_samples=validation_samples, + position_threshold_m=position_threshold_m, + rotation_threshold_rad=rotation_threshold_rad, + nmg_pos_eps=nmg_pos_eps, + nmg_rot_eps=nmg_rot_eps, + checkpoint_path=checkpoint_path, + ) + extras = list(extra_baselines or []) + if compare_ik and "ik_interpolate" not in extras: + extras.append("ik_interpolate") + if compare_toppra and "toppra" not in extras: + extras.append("toppra") + specs = _resolve_planners(suite, algorithms, extras) + return BenchmarkRunner( + suite, + specs, + device=sim_device, + headless=headless, + output_root=output_root, + ).run() + + +def run_from_args(args: argparse.Namespace) -> BenchmarkRunResult: + """Run the benchmark from parsed unified-CLI arguments.""" + return run_all_benchmarks( + num_waypoints_list=args.num_waypoints, + sim_device=args.device, + headless=args.headless, + checkpoint_path=args.checkpoint_path, + suite_name=args.suite, + algorithms=args.algorithms, + extra_baselines=args.extra_baselines, + batch_sizes=args.batch_sizes, + seeds=args.seeds, + num_trials=args.num_trials, + warmup_trials=args.warmup_trials, + sample_interval=args.sample_interval, + validation_samples=args.validation_samples, + position_threshold_m=args.position_threshold_m, + rotation_threshold_rad=args.rotation_threshold_rad, + nmg_pos_eps=args.nmg_pos_eps, + nmg_rot_eps=args.nmg_rot_eps, + compare_ik=args.compare_ik, + compare_toppra=args.compare_toppra, + include_trial_details=args.save_trial_details, + output_root=args.output_root, ) - if result is None: - print("SKIPPED: neural planner benchmark (checkpoint unavailable).") - sys.exit(0) - - ( - trial_rows, - summary_rows, - quality_leaderboard_rows, - performance_leaderboard_rows, - notes, - ) = result - print("\n" + "=" * 60) - print("Benchmarks complete.") - print("=" * 60) - report_path = _write_markdown_report( - benchmark_name="neural_planner", - trial_rows=trial_rows, - summary_rows=summary_rows, - quality_leaderboard_rows=quality_leaderboard_rows, - performance_leaderboard_rows=performance_leaderboard_rows, - notes=notes, - include_trial_details=include_trial_details, +def _parse_args() -> argparse.Namespace: + """Parse standalone module arguments using the unified option schema.""" + parser = argparse.ArgumentParser( + description="Benchmark motion generation on fixed free-space cases." ) - print(f"Markdown report saved: {report_path}") + add_parser_arguments(parser) + return parser.parse_args() if __name__ == "__main__": - cli_args = _parse_args() - run_all_benchmarks( - num_waypoints_list=cli_args.num_waypoints, - sim_device=cli_args.device, - headless=cli_args.headless, - checkpoint_path=cli_args.checkpoint_path, - num_trials=cli_args.num_trials, - warmup_trials=cli_args.warmup_trials, - sample_interval=cli_args.sample_interval, - compare_ik=cli_args.compare_ik, - compare_toppra=cli_args.compare_toppra, - include_trial_details=cli_args.save_trial_details, - ) + run_from_args(_parse_args()) diff --git a/scripts/benchmark/planners/neural_planner/runner.py b/scripts/benchmark/planners/neural_planner/runner.py new file mode 100644 index 000000000..7d3dfef91 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/runner.py @@ -0,0 +1,456 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Generic lifecycle runner for the ``free-space-common`` benchmark.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import TYPE_CHECKING, TypeVar + +import torch + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.planners.utils import PlanResult +from embodichain.lab.sim.robots import FrankaPandaCfg + +from . import planners as _builtin_planners # noqa: F401 - registry side effects +from .aggregation import aggregate_results +from .artifacts import ( + TrialJsonlWriter, + create_run_directory, + environment_metadata, + write_case_manifest, + write_json, + write_resolved_suite, +) +from .config import PlannerSpecCfg, SuiteCfg +from .metrics import compute_case_outcomes, timed_call +from .metrics.trajectory import make_failure_outcomes +from .models import ( + BenchmarkCase, + PlannerMetadata, + TrialPhase, + TrialRecord, +) +from .planners.base import PlannerAdapter, PlannerContext +from .registry import create_planner_adapter +from .reporting import write_markdown_report +from .scenarios import generate_free_space_cases + +if TYPE_CHECKING: + from collections.abc import Callable + + from embodichain.lab.sim.objects import Robot + +__all__ = ["BenchmarkRunResult", "BenchmarkRunner", "resolve_device"] + +_T = TypeVar("_T") +_CONTROL_PART = "arm" +_ROBOT_UID = "benchmark_franka_panda" + + +@dataclass(frozen=True) +class BenchmarkRunResult: + """Paths and aggregate data produced by one completed run.""" + + run_dir: Path + report_path: Path + trials_path: Path + records: tuple[TrialRecord, ...] + aggregates: dict[str, list[dict[str, object]]] + + +def resolve_device(requested: str) -> torch.device: + """Resolve ``auto`` while rejecting an unavailable explicit CUDA request.""" + if requested == "auto": + return torch.device("cuda" if torch.cuda.is_available() else "cpu") + if requested == "cuda" and not torch.cuda.is_available(): + raise RuntimeError("--device cuda was requested, but CUDA is unavailable.") + return torch.device(requested) + + +def _capture(callable_fn: "Callable[[], _T]") -> tuple[_T | None, Exception | None]: + """Return a callable result or its ordinary exception for timed recording.""" + try: + return callable_fn(), None + except Exception as exc: # noqa: BLE001 - failures are benchmark outcomes + return None, exc + + +class BenchmarkRunner: + """Generate fixed cases, execute adapters, aggregate, and report results.""" + + def __init__( + self, + suite: SuiteCfg, + planner_specs: list[PlannerSpecCfg], + *, + device: str = "auto", + headless: bool = True, + output_root: str | Path = "outputs/benchmarks", + ) -> None: + self.suite = suite + self.planner_specs = planner_specs + self.device = resolve_device(device) + self.headless = headless + self.output_root = Path(output_root) + self.records: list[TrialRecord] = [] + self.cases: list[BenchmarkCase] = [] + self.metadata: dict[str, PlannerMetadata] = {} + self.notes: list[str] = [] + + def _create_simulation(self, batch_size: int) -> tuple[SimulationManager, "Robot"]: + """Create one isolated Franka simulator for a fixed batch size.""" + sim = SimulationManager( + SimulationManagerCfg( + headless=self.headless, + sim_device=str(self.device), + num_envs=batch_size, + arena_space=2.0, + ) + ) + robot = sim.add_robot( + cfg=FrankaPandaCfg.from_dict({"uid": _ROBOT_UID, "robot_type": "panda"}) + ) + sim.update(step=1) + return sim, robot + + @staticmethod + def _set_case_start( + sim: SimulationManager, robot: "Robot", case: BenchmarkCase + ) -> None: + """Restore current and target robot state outside the timed region.""" + robot.set_qpos(case.start_qpos, name=_CONTROL_PART, target=False) + robot.set_qpos(case.start_qpos, name=_CONTROL_PART, target=True) + robot.clear_dynamics() + sim.update(step=1) + + def _append(self, writer: TrialJsonlWriter, record: TrialRecord) -> None: + """Retain and immediately persist one raw record.""" + self.records.append(record) + writer.append(record) + + @staticmethod + def _base_record( + metadata: PlannerMetadata, + case: BenchmarkCase, + phase: TrialPhase, + *, + repeat: int = -1, + ) -> dict[str, object]: + """Build fields shared by all lifecycle records.""" + return { + "suite_version": case.suite_version, + "track": case.track, + "scenario_id": case.scenario_id, + "case_id": case.case_id, + "algorithm_id": metadata.algorithm_id, + "algorithm_role": metadata.algorithm_role, + "model_revision": metadata.model_revision, + "planner_config_hash": metadata.config_hash, + "seed": case.seed, + "repeat": repeat, + "batch_size": case.batch_size, + "waypoint_count": case.num_waypoints, + "path_shape": case.path_shape, + "phase": phase, + } + + def _record_unavailable( + self, + writer: TrialJsonlWriter, + metadata: PlannerMetadata, + case: BenchmarkCase, + reason: str, + ) -> None: + """Record an unsupported runtime without converting it into failure.""" + self._append( + writer, + TrialRecord( + **self._base_record(metadata, case, TrialPhase.AVAILABILITY), + status="unsupported", + failure_code="unsupported_capability", + failure_message=reason, + ), + ) + note = f"{metadata.algorithm_id} skipped for B={case.batch_size}: {reason}" + self.notes.append(note) + print(f"SKIPPED: {note}") + + def _record_timed_lifecycle( + self, + writer: TrialJsonlWriter, + metadata: PlannerMetadata, + case: BenchmarkCase, + phase: TrialPhase, + adapter: PlannerAdapter, + callable_fn: "Callable[[], object]", + ) -> tuple[object | None, Exception | None]: + """Measure a construct/prepare operation and persist its outcome.""" + measured = timed_call(lambda: _capture(callable_fn)) + result, error = measured.result + status = "error" if error is not None else "ok" + phase_metadata = result if isinstance(result, dict) else {} + self._append( + writer, + TrialRecord( + **self._base_record(metadata, case, phase), + status=status, + failure_code="planner_exception" if error is not None else None, + failure_message=str(error) if error is not None else None, + cost_time_ms=measured.cost_time_ms, + cpu_delta_mb=measured.cpu_delta_mb, + gpu_delta_mb=measured.gpu_delta_mb, + peak_gpu_mb=measured.peak_gpu_mb, + metadata=phase_metadata, + ), + ) + if error is not None: + self.notes.append( + f"{metadata.algorithm_id} {phase.value} failed for " + f"B={case.batch_size}: {error}" + ) + return result, error + + def _run_plan_call( + self, + writer: TrialJsonlWriter, + sim: SimulationManager, + robot: "Robot", + adapter: PlannerAdapter, + metadata: PlannerMetadata, + case: BenchmarkCase, + phase: TrialPhase, + repeat: int, + ) -> bool: + """Time one plan, validate outside timing, and persist the record.""" + self._set_case_start(sim, robot, case) + measured = timed_call(lambda: _capture(lambda: adapter.plan(case))) + result, error = measured.result + failure_code = None + failure_message = None + status = "ok" + if error is not None: + status = "error" + failure_code = "planner_exception" + failure_message = str(error) + outcomes = make_failure_outcomes(case.batch_size, failure_code) + elif not isinstance(result, PlanResult): + status = "error" + failure_code = "planner_contract_error" + failure_message = f"Expected PlanResult, got {type(result).__name__}." + outcomes = make_failure_outcomes(case.batch_size, failure_code) + elif phase is TrialPhase.WARMUP: + outcomes = () + else: + try: + outcomes = compute_case_outcomes( + result, + case, + robot, + _CONTROL_PART, + validation_samples=self.suite.protocol.validation_samples, + position_threshold_m=self.suite.protocol.position_threshold_m, + rotation_threshold_rad=self.suite.protocol.rotation_threshold_rad, + joint_limit_tolerance_rad=( + self.suite.protocol.joint_limit_tolerance_rad + ), + ) + except Exception as exc: # noqa: BLE001 - metric failure is recorded + status = "error" + failure_code = "metric_evaluation_error" + failure_message = str(exc) + outcomes = make_failure_outcomes(case.batch_size, failure_code) + + self._append( + writer, + TrialRecord( + **self._base_record(metadata, case, phase, repeat=repeat), + status=status, + failure_code=failure_code, + failure_message=failure_message, + cost_time_ms=measured.cost_time_ms, + cpu_delta_mb=measured.cpu_delta_mb, + gpu_delta_mb=measured.gpu_delta_mb, + peak_gpu_mb=measured.peak_gpu_mb, + outcomes=outcomes, + ), + ) + if phase is not TrialPhase.WARMUP: + print( + f" {metadata.algorithm_id:<16} B={case.batch_size:>3d} " + f"W={case.num_waypoints} {case.path_shape:<16} " + f"{phase.value:<8} {measured.cost_time_ms:>10.3f} ms " + f"status={status}" + ) + return error is None and status == "ok" + + def _run_adapter( + self, + writer: TrialJsonlWriter, + sim: SimulationManager, + robot: "Robot", + spec: PlannerSpecCfg, + cases: list[BenchmarkCase], + ) -> None: + """Execute one adapter over every case for a fixed simulator batch.""" + context = PlannerContext( + robot=robot, + control_part=_CONTROL_PART, + device=self.device, + sample_interval=self.suite.protocol.sample_interval, + ) + adapter = create_planner_adapter(spec, context) + metadata = adapter.metadata + self.metadata.setdefault(metadata.algorithm_id, metadata) + first_case = cases[0] + available, reason = adapter.availability() + if not available: + self._record_unavailable( + writer, metadata, first_case, reason or "runtime unavailable" + ) + return + + _, build_error = self._record_timed_lifecycle( + writer, + metadata, + first_case, + TrialPhase.CONSTRUCT, + adapter, + adapter.build, + ) + if build_error is not None: + adapter.close() + return + try: + if adapter.separate_prepare: + _, prepare_error = self._record_timed_lifecycle( + writer, + metadata, + first_case, + TrialPhase.PREPARE, + adapter, + lambda: adapter.prepare(first_case), + ) + if prepare_error is not None: + return + + self._run_plan_call( + writer, + sim, + robot, + adapter, + metadata, + first_case, + TrialPhase.COLD, + repeat=-1, + ) + for case in cases: + for warmup_index in range(self.suite.protocol.warmup_trials): + self._run_plan_call( + writer, + sim, + robot, + adapter, + metadata, + case, + TrialPhase.WARMUP, + repeat=warmup_index, + ) + for repeat in range(self.suite.protocol.measured_trials): + self._run_plan_call( + writer, + sim, + robot, + adapter, + metadata, + case, + TrialPhase.MEASURED, + repeat=repeat, + ) + finally: + adapter.close() + + def run(self) -> BenchmarkRunResult: + """Run the suite and write all required artifacts.""" + run_dir = create_run_directory(self.output_root, self.suite.name) + write_resolved_suite(run_dir / "resolved_suite.yaml", self.suite) + write_json(run_dir / "environment.json", environment_metadata()) + writer = TrialJsonlWriter(run_dir / "trials.jsonl") + + print("=" * 60) + print("Motion Generation Free-Space Benchmark") + print("=" * 60) + print( + f"suite={self.suite.suite_version} device={self.device} " + f"planners={','.join(spec.id for spec in self.planner_specs)}" + ) + + for batch_size in self.suite.free_space.batch_sizes: + sim: SimulationManager | None = None + try: + sim, robot = self._create_simulation(batch_size) + cases = generate_free_space_cases( + self.suite, robot, _CONTROL_PART, batch_size + ) + self.cases.extend(cases) + for spec in self.planner_specs: + self._run_adapter(writer, sim, robot, spec, cases) + finally: + if sim is not None: + # Benchmarks must aggregate and report after simulator + # teardown; the SimulationManager default exits the whole + # process, so opt into deferred in-process cleanup here. + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + write_case_manifest(run_dir / "case_manifest.json", self.cases) + metadata = [ + self.metadata[spec.id] + for spec in self.planner_specs + if spec.id in self.metadata + ] + aggregates = aggregate_results( + self.records, + metadata, + self.cases, + self.suite.protocol.measured_trials, + ) + write_json(run_dir / "aggregates.json", aggregates) + report_path = write_markdown_report( + run_dir / "report.md", + self.suite, + aggregates, + notes=[ + "CPU/GPU memory values are process/PyTorch allocator deltas around timed calls.", + "Continuous error and path metrics are conditioned on externally motion-valid trajectories.", + "Collision, dynamic, execution, and task metrics are N/A in free-space-common v1.", + *self.notes, + ], + ) + print("=" * 60) + print("Benchmarks complete.") + print("=" * 60) + print(f"Markdown report saved: {report_path}") + return BenchmarkRunResult( + run_dir=run_dir, + report_path=report_path, + trials_path=writer.path, + records=tuple(self.records), + aggregates=aggregates, + ) diff --git a/scripts/benchmark/planners/neural_planner/scenarios/__init__.py b/scripts/benchmark/planners/neural_planner/scenarios/__init__.py new file mode 100644 index 000000000..31156ef03 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/scenarios/__init__.py @@ -0,0 +1,23 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Scenario providers for motion-generation benchmarks.""" + +from __future__ import annotations + +from .free_space import generate_free_space_cases + +__all__ = ["generate_free_space_cases"] diff --git a/scripts/benchmark/planners/neural_planner/scenarios/free_space.py b/scripts/benchmark/planners/neural_planner/scenarios/free_space.py new file mode 100644 index 000000000..a13446e5d --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/scenarios/free_space.py @@ -0,0 +1,216 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Deterministic reachable cases for the ``free-space-common`` track.""" + +from __future__ import annotations + +import math +from typing import TYPE_CHECKING + +import torch + +from ..config import SuiteCfg, stable_hash +from ..models import BenchmarkCase + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + +__all__ = ["generate_free_space_cases"] + +_NOMINAL_QPOS = torch.tensor( + [0.0, -math.pi / 4, 0.0, -3.0 * math.pi / 4, 0.0, math.pi / 2, math.pi / 4], + dtype=torch.float32, +) + + +def _clamp_with_margin(qpos: torch.Tensor, limits: torch.Tensor) -> torch.Tensor: + """Clamp qpos inside ten-percent joint-limit margins.""" + lower, upper = limits[:, 0], limits[:, 1] + margin = (upper - lower).clamp_min(1.0e-3) * 0.05 + return torch.maximum(torch.minimum(qpos, upper - margin), lower + margin) + + +def _start_qpos_for_bin( + name: str, + limits: torch.Tensor, + generator: torch.Generator, +) -> torch.Tensor: + """Create one deterministic start posture for a named condition bin.""" + lower, upper = limits[:, 0], limits[:, 1] + midpoint = (lower + upper) * 0.5 + span = upper - lower + nominal = _clamp_with_margin(_NOMINAL_QPOS.to(limits), limits) + + if name == "nominal": + return nominal + if name == "random_reachable": + noise = torch.rand(limits.shape[0], generator=generator) - 0.5 + return _clamp_with_margin(midpoint + noise.to(limits) * span * 0.55, limits) + if name == "near_limit": + signs = torch.where( + torch.arange(limits.shape[0]) % 2 == 0, + torch.ones(limits.shape[0]), + -torch.ones(limits.shape[0]), + ).to(limits) + return _clamp_with_margin(midpoint + signs * span * 0.42, limits) + if name == "near_singularity": + candidate = torch.tensor( + [0.0, 0.0, 0.0, -0.15, 0.0, 0.20, 0.0], dtype=limits.dtype + ).to(limits) + return _clamp_with_margin(candidate, limits) + raise ValueError(f"Unknown free-space start_state_bin {name!r}.") + + +def _joint_delta(path_shape: str, alpha: float, dofs: int) -> torch.Tensor: + """Return a bounded reference joint displacement for one path sample.""" + delta = torch.zeros(dofs, dtype=torch.float32) + if path_shape == "direct": + delta[: min(dofs, 4)] = torch.tensor([0.22, -0.16, 0.12, 0.10])[:dofs] + return delta * alpha + if path_shape == "l_turn": + first = torch.zeros_like(delta) + second = torch.zeros_like(delta) + first[: min(dofs, 3)] = torch.tensor([0.18, -0.12, 0.08])[:dofs] + if dofs > 3: + second[3 : min(dofs, 7)] = torch.tensor([0.12, -0.16, 0.18, -0.12])[ + : max(0, min(dofs, 7) - 3) + ] + if alpha <= 0.5: + return first * (alpha * 2.0) + return first + second * ((alpha - 0.5) * 2.0) + if path_shape == "s_curve": + if dofs > 0: + delta[0] = 0.20 * alpha + if dofs > 1: + delta[1] = -0.16 * math.sin(math.pi * alpha) + if dofs > 3: + delta[3] = 0.12 * math.sin(2.0 * math.pi * alpha) + return delta + if path_shape == "orientation_only": + if dofs > 4: + delta[4] = 0.25 * alpha + if dofs > 6: + delta[6] = -0.30 * alpha + return delta + if path_shape == "combined": + direct = _joint_delta("direct", alpha, dofs) + orientation = _joint_delta("orientation_only", alpha, dofs) + return direct + orientation + raise ValueError(f"Unknown free-space path_shape {path_shape!r}.") + + +def _build_case( + suite: SuiteCfg, + robot: "Robot", + control_part: str, + *, + seed: int, + batch_size: int, + num_waypoints: int, + path_shape: str, + shape_index: int, +) -> BenchmarkCase: + """Build one reachable env-batched case using FK reference targets.""" + limits = robot.get_qpos_limits(name=control_part)[0].detach().cpu() + if limits.shape[0] != _NOMINAL_QPOS.shape[0]: + raise ValueError( + "free-space-common v1 expects a 7-DoF Franka arm, got " + f"{limits.shape[0]} DoF." + ) + + configured_bins = suite.free_space.start_state_bins + start_bins: list[str] = [] + starts: list[torch.Tensor] = [] + for env_index in range(batch_size): + bin_index = (seed + shape_index + env_index) % len(configured_bins) + bin_name = configured_bins[bin_index] + generator = torch.Generator(device="cpu") + generator.manual_seed(seed * 100_003 + shape_index * 997 + env_index) + start_bins.append(bin_name) + starts.append(_start_qpos_for_bin(bin_name, limits, generator)) + + start_qpos_cpu = torch.stack(starts) + references: list[torch.Tensor] = [] + for waypoint_index in range(num_waypoints): + alpha = float(waypoint_index + 1) / float(num_waypoints) + delta = _joint_delta(path_shape, alpha, start_qpos_cpu.shape[-1]) + target = _clamp_with_margin(start_qpos_cpu + delta.unsqueeze(0), limits) + references.append(target) + reference_qpos_cpu = torch.stack(references, dim=1) + + start_qpos = start_qpos_cpu.to(robot.device) + reference_qpos = reference_qpos_cpu.to(robot.device) + waypoint_poses = [] + for waypoint_index in range(num_waypoints): + waypoint_poses.append( + robot.compute_fk( + qpos=reference_qpos[:, waypoint_index], + name=control_part, + to_matrix=True, + ) + ) + target_waypoints = torch.stack(waypoint_poses, dim=1) + + identity = { + "suite_version": suite.suite_version, + "seed": seed, + "batch_size": batch_size, + "num_waypoints": num_waypoints, + "path_shape": path_shape, + "start_state_bins": start_bins, + } + case_id = f"free_space_{stable_hash(identity)[:16]}" + return BenchmarkCase( + suite_version=suite.suite_version, + track="free-space-common", + scenario_id="waypoint_path" if num_waypoints > 1 else "reach", + case_id=case_id, + seed=seed, + batch_size=batch_size, + num_waypoints=num_waypoints, + path_shape=path_shape, + start_state_bins=tuple(start_bins), + start_qpos=start_qpos, + target_waypoints=target_waypoints, + reference_qpos=reference_qpos, + ) + + +def generate_free_space_cases( + suite: SuiteCfg, + robot: "Robot", + control_part: str, + batch_size: int, +) -> list[BenchmarkCase]: + """Generate the fixed case manifest for one simulator batch size.""" + cases: list[BenchmarkCase] = [] + for seed in suite.free_space.seeds: + for num_waypoints in suite.free_space.waypoint_counts: + for shape_index, path_shape in enumerate(suite.free_space.path_shapes): + cases.append( + _build_case( + suite, + robot, + control_part, + seed=seed, + batch_size=batch_size, + num_waypoints=num_waypoints, + path_shape=path_shape, + shape_index=shape_index, + ) + ) + return cases diff --git a/scripts/benchmark/planners/neural_planner/suites/coverage.yaml b/scripts/benchmark/planners/neural_planner/suites/coverage.yaml new file mode 100644 index 000000000..1f4033aa4 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/suites/coverage.yaml @@ -0,0 +1,62 @@ +schema_version: 1 +name: motion_generation +suite_version: free_space_common_coverage_v1 +profile: coverage + +planners: + - id: curobo + adapter: curobo + role: primary_baseline + enabled: true + config: + max_attempts: 5 + max_planning_time: null + interpolation_dt: 0.025 + collision_activation_distance: 0.01 + use_cuda_graph: true + cuda_graph_fallback: true + warmup_iterations: 1 + preserve_plan_samples: true + world: + obstacle_representation: sphere + multi_env: false + auto_gen: + fit_type: voxel + sphere_density: 0.1 + collision_sphere_buffer: 0.0 + - id: ik_interpolate + adapter: ik_interpolate + role: diagnostic_baseline + enabled: false + config: {} + - id: toppra + adapter: toppra + role: diagnostic_baseline + enabled: false + config: + velocity: 0.2 + acceleration: 0.5 + - id: nmg + adapter: neural_stub + role: candidate + enabled: false + config: + model_revision: not-ready + pos_eps: 0.05 + rot_eps: 0.3 + +protocol: + warmup_trials: 3 + measured_trials: 20 + sample_interval: 80 + validation_samples: 256 + position_threshold_m: 0.05 + rotation_threshold_rad: 0.3 + joint_limit_tolerance_rad: 0.00001 + +free_space: + batch_sizes: [1, 8, 64] + waypoint_counts: [1, 3, 5] + path_shapes: [direct, l_turn, s_curve, orientation_only, combined] + start_state_bins: [nominal, random_reachable, near_limit, near_singularity] + seeds: [11, 23, 37, 53, 71] diff --git a/scripts/benchmark/planners/neural_planner/suites/smoke.yaml b/scripts/benchmark/planners/neural_planner/suites/smoke.yaml new file mode 100644 index 000000000..c798333a6 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/suites/smoke.yaml @@ -0,0 +1,62 @@ +schema_version: 1 +name: motion_generation +suite_version: free_space_common_smoke_v1 +profile: smoke + +planners: + - id: curobo + adapter: curobo + role: primary_baseline + enabled: true + config: + max_attempts: 5 + max_planning_time: null + interpolation_dt: 0.025 + collision_activation_distance: 0.01 + use_cuda_graph: true + cuda_graph_fallback: true + warmup_iterations: 1 + preserve_plan_samples: true + world: + obstacle_representation: sphere + multi_env: false + auto_gen: + fit_type: voxel + sphere_density: 0.1 + collision_sphere_buffer: 0.0 + - id: ik_interpolate + adapter: ik_interpolate + role: diagnostic_baseline + enabled: false + config: {} + - id: toppra + adapter: toppra + role: diagnostic_baseline + enabled: false + config: + velocity: 0.2 + acceleration: 0.5 + - id: nmg + adapter: neural_stub + role: candidate + enabled: false + config: + model_revision: not-ready + pos_eps: 0.05 + rot_eps: 0.3 + +protocol: + warmup_trials: 1 + measured_trials: 3 + sample_interval: 40 + validation_samples: 128 + position_threshold_m: 0.05 + rotation_threshold_rad: 0.3 + joint_limit_tolerance_rad: 0.00001 + +free_space: + batch_sizes: [1] + waypoint_counts: [1, 3, 5] + path_shapes: [direct, l_turn, s_curve] + start_state_bins: [nominal] + seeds: [11] diff --git a/tests/benchmark/planners/test_motion_generation_benchmark.py b/tests/benchmark/planners/test_motion_generation_benchmark.py new file mode 100644 index 000000000..c5d694528 --- /dev/null +++ b/tests/benchmark/planners/test_motion_generation_benchmark.py @@ -0,0 +1,317 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Unit tests for the free-space motion-generation benchmark architecture.""" + +from __future__ import annotations + +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.planners.curobo.curobo_planner import CuroboPlanner +from embodichain.lab.sim.planners.utils import MoveType, PlanResult +from scripts.benchmark.planners.neural_planner.aggregation import aggregate_results +from scripts.benchmark.planners.neural_planner.config import load_suite +from scripts.benchmark.planners.neural_planner.metrics.trajectory import ( + compute_case_outcomes, + match_ordered_waypoints, +) +from scripts.benchmark.planners.neural_planner.models import ( + AlgorithmRole, + BenchmarkCase, + CaseOutcome, + PlannerMetadata, + TrialPhase, + TrialRecord, +) +from scripts.benchmark.planners.neural_planner.reporting import ( + write_markdown_report, +) +from scripts.benchmark.planners.neural_planner.run_benchmark import ( + _apply_overrides, +) +from scripts.benchmark.planners.neural_planner.scenarios.free_space import ( + generate_free_space_cases, +) + + +def _translated_pose(x: float) -> torch.Tensor: + pose = torch.eye(4) + pose[0, 3] = x + return pose + + +def test_ordered_waypoints_reject_out_of_order_hits(): + waypoints = torch.stack([_translated_pose(0.1), _translated_pose(0.0)]) + trajectory = torch.stack([_translated_pose(0.0), _translated_pose(0.1)]) + + result = match_ordered_waypoints( + trajectory, + waypoints, + position_threshold_m=1.0e-4, + rotation_threshold_rad=1.0e-4, + ) + + assert result["ordered_waypoints_reached"] is False + assert result["completed_waypoint_ratio"] == pytest.approx(0.5) + + +def test_ordered_waypoint_requires_position_and_rotation_at_same_sample(): + target = torch.eye(4) + target[:3, :3] = torch.tensor([[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) + position_only = torch.eye(4) + rotation_only = target.clone() + rotation_only[0, 3] = 0.2 + + result = match_ordered_waypoints( + torch.stack([position_only, rotation_only]), + target.unsqueeze(0), + position_threshold_m=0.01, + rotation_threshold_rad=0.01, + ) + + assert result["ordered_waypoints_reached"] is False + assert result["arrival_indices"] == [] + + +class _MetricRobot: + device = torch.device("cpu") + + def get_qpos_limits(self, name: str): # noqa: ARG002 + limits = torch.tensor([[-1.0, 1.0]]).repeat(7, 1) + return limits.unsqueeze(0) + + def compute_batch_fk( + self, qpos: torch.Tensor, name: str, to_matrix: bool + ): # noqa: ARG002 + poses = torch.eye(4).repeat(qpos.shape[0], qpos.shape[1], 1, 1) + poses[..., :3, 3] = qpos[..., :3] + return poses + + +def test_motion_valid_is_independent_of_planner_reported_success(): + case = _case() + case.target_waypoints[0, 0, 0, 3] = 0.1 + positions = torch.zeros(1, 2, 7) + positions[0, 1, 0] = 0.1 + + outcomes = compute_case_outcomes( + PlanResult(success=False, positions=positions), + case, + _MetricRobot(), + "arm", + validation_samples=8, + position_threshold_m=1.0e-4, + rotation_threshold_rad=1.0e-4, + joint_limit_tolerance_rad=1.0e-5, + ) + + assert outcomes[0].planning_success is False + assert outcomes[0].motion_valid is True + assert outcomes[0].failure_code == "planner_reported_failure" + + +def test_nmg_precision_and_external_accuracy_are_independently_configurable(): + suite = load_suite("smoke") + _apply_overrides( + suite, + position_threshold_m=0.02, + rotation_threshold_rad=0.10, + nmg_pos_eps=0.03, + nmg_rot_eps=0.20, + ) + nmg = next(spec for spec in suite.planners if spec.id == "nmg") + + assert suite.protocol.position_threshold_m == pytest.approx(0.02) + assert suite.protocol.rotation_threshold_rad == pytest.approx(0.10) + assert nmg.config["pos_eps"] == pytest.approx(0.03) + assert nmg.config["rot_eps"] == pytest.approx(0.20) + + +@pytest.mark.parametrize("override", [{"nmg_pos_eps": 0.0}, {"nmg_rot_eps": -0.1}]) +def test_nmg_precision_rejects_non_positive_values(override): + suite = load_suite("smoke") + + with pytest.raises(ValueError, match="NMG"): + _apply_overrides(suite, **override) + + +class _FakeRobot: + device = torch.device("cpu") + + def get_qpos_limits(self, name: str): # noqa: ARG002 + lower = torch.tensor([-2.8, -1.7, -2.8, -3.0, -2.8, 0.0, -2.8]) + upper = torch.tensor([2.8, 1.7, 2.8, -0.05, 2.8, 3.7, 2.8]) + return torch.stack([lower, upper], dim=-1).unsqueeze(0) + + def compute_fk( + self, qpos: torch.Tensor, name: str, to_matrix: bool + ): # noqa: ARG002 + poses = torch.eye(4).repeat(qpos.shape[0], 1, 1) + poses[:, :3, 3] = qpos[:, :3] + return poses + + +def test_free_space_manifest_is_seed_stable_and_algorithm_independent(): + suite = load_suite("smoke") + robot = _FakeRobot() + + first = generate_free_space_cases(suite, robot, "arm", batch_size=1) + second = generate_free_space_cases(suite, robot, "arm", batch_size=1) + + assert [case.case_id for case in first] == [case.case_id for case in second] + assert torch.equal(first[0].start_qpos, second[0].start_qpos) + assert torch.equal(first[0].target_waypoints, second[0].target_waypoints) + + +def _case() -> BenchmarkCase: + return BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-1", + seed=11, + batch_size=1, + num_waypoints=1, + path_shape="direct", + start_state_bins=("nominal",), + start_qpos=torch.zeros(1, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4), + reference_qpos=torch.zeros(1, 1, 7), + ) + + +def _outcome() -> CaseOutcome: + return CaseOutcome( + env_index=0, + planning_success=True, + finite=True, + ordered_waypoints_reached=True, + motion_valid=True, + completed_waypoint_ratio=1.0, + final_translation_err_mm=1.0, + final_rotation_err_deg=2.0, + waypoint_translation_err_mm_mean=1.0, + waypoint_translation_err_mm_p95=1.0, + waypoint_translation_err_mm_max=1.0, + waypoint_rotation_err_deg_mean=2.0, + waypoint_rotation_err_deg_p95=2.0, + waypoint_rotation_err_deg_max=2.0, + joint_limit_violation=False, + max_normalized_joint_violation=0.0, + joint_path_length_rad=0.2, + cartesian_path_length_m=0.1, + path_efficiency=1.0, + ) + + +def _record(phase: TrialPhase, cost: float) -> TrialRecord: + return TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-1", + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=11, + repeat=0, + batch_size=1, + waypoint_count=1, + path_shape="direct", + phase=phase, + cost_time_ms=cost, + cpu_delta_mb=1.0, + gpu_delta_mb=2.0, + peak_gpu_mb=3.0, + outcomes=(_outcome(),) if phase is TrialPhase.MEASURED else (), + ) + + +def test_aggregation_excludes_warmup_and_keeps_unavailable_algorithm(): + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ), + PlannerMetadata( + algorithm_id="nmg", + algorithm_role=AlgorithmRole.CANDIDATE, + adapter="neural_stub", + config_hash="def", + capabilities=frozenset({"eef_waypoint"}), + ), + ] + aggregates = aggregate_results( + [_record(TrialPhase.WARMUP, 999.0), _record(TrialPhase.MEASURED, 10.0)], + metadata, + [_case()], + measured_trials=1, + ) + + perf = next( + row for row in aggregates["time_and_memory"] if row["algorithm"] == "curobo" + ) + assert perf["cost_time_ms"] == pytest.approx(10.0) + leaderboard = aggregates["leaderboard"] + assert {row["algorithm"] for row in leaderboard} == {"curobo", "nmg"} + nmg = next(row for row in leaderboard if row["algorithm"] == "nmg") + assert nmg["eligible"] is False + assert nmg["coverage_rate"] == pytest.approx(0.0) + + +def test_report_contains_exactly_three_markdown_tables(tmp_path): + suite = load_suite("smoke") + aggregates = { + "time_and_memory": [], + "success_and_metrics": [], + "leaderboard": [], + } + + report = write_markdown_report(tmp_path / "report.md", suite, aggregates) + text = report.read_text(encoding="utf-8") + + assert text.count("\n| ---") == 3 + assert text.count("## Time & Memory") == 1 + assert text.count("## Success & Other Metrics") == 1 + assert text.count("## Leaderboard") == 1 + + +def test_curobo_prepare_backend_exposes_actual_graph_mode(): + planner = object.__new__(CuroboPlanner) + planner.robot = Mock(num_instances=8) + planner.cfg = Mock(world=Mock(multi_env=False)) + backend = Mock( + control_part="arm", + batch_size=8, + planning_mode=MoveType.EEF_MOVE, + use_cuda_graph=False, + ) + planner._get_backend = Mock(return_value=backend) + + result = planner.prepare_backend( + control_part="arm", batch_size=8, move_type=MoveType.EEF_MOVE + ) + + planner._get_backend.assert_called_once_with("arm", 8, MoveType.EEF_MOVE) + assert result["use_cuda_graph"] is False + assert result["batch_size"] == 8 From 97af6086521fbb407431767a8a4623354877a782 Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 02:15:28 +0800 Subject: [PATCH 02/17] Separate planner and external failure codes --- .../planners/neural_planner/aggregation.py | 2 +- .../neural_planner/metrics/trajectory.py | 42 +++++++++++---- .../planners/neural_planner/models.py | 1 + .../test_motion_generation_benchmark.py | 54 ++++++++++++++++++- 4 files changed, 88 insertions(+), 11 deletions(-) diff --git a/scripts/benchmark/planners/neural_planner/aggregation.py b/scripts/benchmark/planners/neural_planner/aggregation.py index eac057b88..28292365d 100644 --- a/scripts/benchmark/planners/neural_planner/aggregation.py +++ b/scripts/benchmark/planners/neural_planner/aggregation.py @@ -59,7 +59,7 @@ def _rate(values: Iterable[bool]) -> float | None: def _top_failure(outcomes: list[CaseOutcome]) -> str | None: - """Return the most frequent non-empty failure code.""" + """Return the most frequent non-empty external failure code.""" failures = Counter( outcome.failure_code for outcome in outcomes if outcome.failure_code ) diff --git a/scripts/benchmark/planners/neural_planner/metrics/trajectory.py b/scripts/benchmark/planners/neural_planner/metrics/trajectory.py index e3fa71a6f..3f898c3d6 100644 --- a/scripts/benchmark/planners/neural_planner/metrics/trajectory.py +++ b/scripts/benchmark/planners/neural_planner/metrics/trajectory.py @@ -301,6 +301,8 @@ def _path_metrics( def make_failure_outcomes( batch_size: int, failure_code: str, + *, + planner_failure_code: str | None = None, ) -> tuple[CaseOutcome, ...]: """Create per-env outcomes for an exception before validation was possible.""" return tuple( @@ -325,6 +327,7 @@ def make_failure_outcomes( cartesian_path_length_m=None, path_efficiency=None, failure_code=failure_code, + planner_failure_code=planner_failure_code, ) for index in range(batch_size) ) @@ -346,11 +349,31 @@ def compute_case_outcomes( if result.positions is None or result.positions.ndim != 3: return tuple( CaseOutcome( - **{ - **make_failure_outcomes(1, "planner_reported_failure")[0].__dict__, - "env_index": env_index, - "planning_success": bool(planning_success[env_index].item()), - } + env_index=env_index, + planning_success=bool(planning_success[env_index].item()), + finite=False, + ordered_waypoints_reached=False, + motion_valid=False, + completed_waypoint_ratio=0.0, + final_translation_err_mm=None, + final_rotation_err_deg=None, + waypoint_translation_err_mm_mean=None, + waypoint_translation_err_mm_p95=None, + waypoint_translation_err_mm_max=None, + waypoint_rotation_err_deg_mean=None, + waypoint_rotation_err_deg_p95=None, + waypoint_rotation_err_deg_max=None, + joint_limit_violation=False, + max_normalized_joint_violation=None, + joint_path_length_rad=None, + cartesian_path_length_m=None, + path_efficiency=None, + failure_code="non_finite_trajectory", + planner_failure_code=( + None + if bool(planning_success[env_index].item()) + else "planner_reported_failure" + ), ) for env_index in range(case.batch_size) ) @@ -430,15 +453,15 @@ def compute_case_outcomes( final_pos_m = final_rot_rad = None joint_length = cartesian_length = efficiency = None - failure_code = None - if not planner_ok: - failure_code = "planner_reported_failure" - elif not finite: + if not finite: failure_code = "non_finite_trajectory" elif not ordered: failure_code = "waypoint_miss" elif joint_violation: failure_code = "joint_limit_violation" + else: + failure_code = None + planner_failure_code = None if planner_ok else "planner_reported_failure" outcomes.append( CaseOutcome( @@ -482,6 +505,7 @@ def compute_case_outcomes( cartesian_path_length_m=cartesian_length, path_efficiency=efficiency, failure_code=failure_code, + planner_failure_code=planner_failure_code, ) ) return tuple(outcomes) diff --git a/scripts/benchmark/planners/neural_planner/models.py b/scripts/benchmark/planners/neural_planner/models.py index aa7c58199..28836ea5b 100644 --- a/scripts/benchmark/planners/neural_planner/models.py +++ b/scripts/benchmark/planners/neural_planner/models.py @@ -109,6 +109,7 @@ class CaseOutcome: cartesian_path_length_m: float | None path_efficiency: float | None failure_code: str | None = None + planner_failure_code: str | None = None @dataclass(frozen=True) diff --git a/tests/benchmark/planners/test_motion_generation_benchmark.py b/tests/benchmark/planners/test_motion_generation_benchmark.py index c5d694528..f8926ff37 100644 --- a/tests/benchmark/planners/test_motion_generation_benchmark.py +++ b/tests/benchmark/planners/test_motion_generation_benchmark.py @@ -123,7 +123,59 @@ def test_motion_valid_is_independent_of_planner_reported_success(): assert outcomes[0].planning_success is False assert outcomes[0].motion_valid is True - assert outcomes[0].failure_code == "planner_reported_failure" + assert outcomes[0].failure_code is None + assert outcomes[0].planner_failure_code == "planner_reported_failure" + + +def test_top_failure_ignores_planner_internal_codes_when_motion_valid(): + case = _case() + case.target_waypoints[0, 0, 0, 3] = 0.1 + positions = torch.zeros(1, 2, 7) + positions[0, 1, 0] = 0.1 + outcomes = compute_case_outcomes( + PlanResult(success=False, positions=positions), + case, + _MetricRobot(), + "arm", + validation_samples=8, + position_threshold_m=1.0e-4, + rotation_threshold_rad=1.0e-4, + joint_limit_tolerance_rad=1.0e-5, + ) + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ) + ] + measured = TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-1", + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=11, + repeat=0, + batch_size=1, + waypoint_count=1, + path_shape="direct", + phase=TrialPhase.MEASURED, + cost_time_ms=10.0, + outcomes=outcomes, + ) + + aggregates = aggregate_results([measured], metadata, [case], measured_trials=1) + row = aggregates["success_and_metrics"][0] + + assert row["motion_valid_rate"] == pytest.approx(1.0) + assert row["planning_success_rate"] == pytest.approx(0.0) + assert row["top_failure"] is None def test_nmg_precision_and_external_accuracy_are_independently_configurable(): From edda1d0a1de82a2606ccc4547eba480c114bb525 Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 02:18:15 +0800 Subject: [PATCH 03/17] Stratify free-space cases by start_state_bin --- .../planners/neural_planner/aggregation.py | 21 ++- .../planners/neural_planner/artifacts.py | 2 +- .../planners/neural_planner/models.py | 3 +- .../planners/neural_planner/reporting.py | 1 + .../planners/neural_planner/runner.py | 1 + .../neural_planner/scenarios/free_space.py | 44 +++--- .../test_motion_generation_benchmark.py | 133 +++++++++++++++++- 7 files changed, 177 insertions(+), 28 deletions(-) diff --git a/scripts/benchmark/planners/neural_planner/aggregation.py b/scripts/benchmark/planners/neural_planner/aggregation.py index 28292365d..4450e5eee 100644 --- a/scripts/benchmark/planners/neural_planner/aggregation.py +++ b/scripts/benchmark/planners/neural_planner/aggregation.py @@ -184,7 +184,7 @@ def _metric_rows( measured_trials: int, ) -> list[dict[str, object]]: """Aggregate external success and quality metrics by scenario condition.""" - outcome_groups: dict[tuple[str, str, int, int, str], list[CaseOutcome]] = ( + outcome_groups: dict[tuple[str, str, int, int, str, str], list[CaseOutcome]] = ( defaultdict(list) ) for record in records: @@ -196,20 +196,29 @@ def _metric_rows( record.batch_size, record.waypoint_count, record.path_shape, + record.start_state_bin, ) outcome_groups[key].extend(record.outcomes) - expected_by_group: Counter[tuple[str, int, int, str]] = Counter() - unique_cases_by_group: Counter[tuple[str, int, int, str]] = Counter() + expected_by_group: Counter[tuple[str, int, int, str, str]] = Counter() + unique_cases_by_group: Counter[tuple[str, int, int, str, str]] = Counter() for case in cases: - key = (case.scenario_id, case.batch_size, case.num_waypoints, case.path_shape) + key = ( + case.scenario_id, + case.batch_size, + case.num_waypoints, + case.path_shape, + case.start_state_bin, + ) expected_by_group[key] += case.batch_size * measured_trials unique_cases_by_group[key] += case.batch_size rows: list[dict[str, object]] = [] for info in metadata: for group_key in sorted(expected_by_group): - scenario_id, batch_size, waypoint_count, path_shape = group_key + scenario_id, batch_size, waypoint_count, path_shape, start_state_bin = ( + group_key + ) outcomes = outcome_groups.get( ( info.algorithm_id, @@ -217,6 +226,7 @@ def _metric_rows( batch_size, waypoint_count, path_shape, + start_state_bin, ), [], ) @@ -231,6 +241,7 @@ def _metric_rows( "batch_size": batch_size, "waypoint_count": waypoint_count, "path_shape": path_shape, + "start_state_bin": start_state_bin, "cases": unique_cases_by_group[group_key], "coverage_rate": min(1.0, len(outcomes) / max(expected, 1)), "success_rate": _rate(outcome.motion_valid for outcome in outcomes), diff --git a/scripts/benchmark/planners/neural_planner/artifacts.py b/scripts/benchmark/planners/neural_planner/artifacts.py index 49ec54437..4544230f6 100644 --- a/scripts/benchmark/planners/neural_planner/artifacts.py +++ b/scripts/benchmark/planners/neural_planner/artifacts.py @@ -123,7 +123,7 @@ def _case_to_dict(case: BenchmarkCase) -> dict[str, Any]: "batch_size": case.batch_size, "num_waypoints": case.num_waypoints, "path_shape": case.path_shape, - "start_state_bins": list(case.start_state_bins), + "start_state_bin": case.start_state_bin, "start_qpos": case.start_qpos.detach().cpu().tolist(), "target_waypoints": case.target_waypoints.detach().cpu().tolist(), "validity_evidence": { diff --git a/scripts/benchmark/planners/neural_planner/models.py b/scripts/benchmark/planners/neural_planner/models.py index 28836ea5b..02868bc78 100644 --- a/scripts/benchmark/planners/neural_planner/models.py +++ b/scripts/benchmark/planners/neural_planner/models.py @@ -79,7 +79,7 @@ class BenchmarkCase: batch_size: int num_waypoints: int path_shape: str - start_state_bins: tuple[str, ...] + start_state_bin: str start_qpos: torch.Tensor target_waypoints: torch.Tensor reference_qpos: torch.Tensor @@ -129,6 +129,7 @@ class TrialRecord: batch_size: int waypoint_count: int path_shape: str + start_state_bin: str phase: TrialPhase status: str = "ok" failure_code: str | None = None diff --git a/scripts/benchmark/planners/neural_planner/reporting.py b/scripts/benchmark/planners/neural_planner/reporting.py index 4d9e8f7fe..1617fe1b2 100644 --- a/scripts/benchmark/planners/neural_planner/reporting.py +++ b/scripts/benchmark/planners/neural_planner/reporting.py @@ -55,6 +55,7 @@ "batch_size", "waypoint_count", "path_shape", + "start_state_bin", "cases", "coverage_rate", "success_rate", diff --git a/scripts/benchmark/planners/neural_planner/runner.py b/scripts/benchmark/planners/neural_planner/runner.py index 7d3dfef91..c60717f36 100644 --- a/scripts/benchmark/planners/neural_planner/runner.py +++ b/scripts/benchmark/planners/neural_planner/runner.py @@ -168,6 +168,7 @@ def _base_record( "batch_size": case.batch_size, "waypoint_count": case.num_waypoints, "path_shape": case.path_shape, + "start_state_bin": case.start_state_bin, "phase": phase, } diff --git a/scripts/benchmark/planners/neural_planner/scenarios/free_space.py b/scripts/benchmark/planners/neural_planner/scenarios/free_space.py index a13446e5d..a8cc5eb15 100644 --- a/scripts/benchmark/planners/neural_planner/scenarios/free_space.py +++ b/scripts/benchmark/planners/neural_planner/scenarios/free_space.py @@ -123,6 +123,8 @@ def _build_case( num_waypoints: int, path_shape: str, shape_index: int, + start_state_bin: str, + bin_index: int, ) -> BenchmarkCase: """Build one reachable env-batched case using FK reference targets.""" limits = robot.get_qpos_limits(name=control_part)[0].detach().cpu() @@ -132,16 +134,13 @@ def _build_case( f"{limits.shape[0]} DoF." ) - configured_bins = suite.free_space.start_state_bins - start_bins: list[str] = [] starts: list[torch.Tensor] = [] for env_index in range(batch_size): - bin_index = (seed + shape_index + env_index) % len(configured_bins) - bin_name = configured_bins[bin_index] generator = torch.Generator(device="cpu") - generator.manual_seed(seed * 100_003 + shape_index * 997 + env_index) - start_bins.append(bin_name) - starts.append(_start_qpos_for_bin(bin_name, limits, generator)) + generator.manual_seed( + seed * 100_003 + shape_index * 997 + bin_index * 131 + env_index + ) + starts.append(_start_qpos_for_bin(start_state_bin, limits, generator)) start_qpos_cpu = torch.stack(starts) references: list[torch.Tensor] = [] @@ -171,7 +170,7 @@ def _build_case( "batch_size": batch_size, "num_waypoints": num_waypoints, "path_shape": path_shape, - "start_state_bins": start_bins, + "start_state_bin": start_state_bin, } case_id = f"free_space_{stable_hash(identity)[:16]}" return BenchmarkCase( @@ -183,7 +182,7 @@ def _build_case( batch_size=batch_size, num_waypoints=num_waypoints, path_shape=path_shape, - start_state_bins=tuple(start_bins), + start_state_bin=start_state_bin, start_qpos=start_qpos, target_waypoints=target_waypoints, reference_qpos=reference_qpos, @@ -201,16 +200,21 @@ def generate_free_space_cases( for seed in suite.free_space.seeds: for num_waypoints in suite.free_space.waypoint_counts: for shape_index, path_shape in enumerate(suite.free_space.path_shapes): - cases.append( - _build_case( - suite, - robot, - control_part, - seed=seed, - batch_size=batch_size, - num_waypoints=num_waypoints, - path_shape=path_shape, - shape_index=shape_index, + for bin_index, start_state_bin in enumerate( + suite.free_space.start_state_bins + ): + cases.append( + _build_case( + suite, + robot, + control_part, + seed=seed, + batch_size=batch_size, + num_waypoints=num_waypoints, + path_shape=path_shape, + shape_index=shape_index, + start_state_bin=start_state_bin, + bin_index=bin_index, + ) ) - ) return cases diff --git a/tests/benchmark/planners/test_motion_generation_benchmark.py b/tests/benchmark/planners/test_motion_generation_benchmark.py index f8926ff37..25695a55a 100644 --- a/tests/benchmark/planners/test_motion_generation_benchmark.py +++ b/tests/benchmark/planners/test_motion_generation_benchmark.py @@ -165,6 +165,7 @@ def test_top_failure_ignores_planner_internal_codes_when_motion_valid(): batch_size=1, waypoint_count=1, path_shape="direct", + start_state_bin="nominal", phase=TrialPhase.MEASURED, cost_time_ms=10.0, outcomes=outcomes, @@ -176,6 +177,7 @@ def test_top_failure_ignores_planner_internal_codes_when_motion_valid(): assert row["motion_valid_rate"] == pytest.approx(1.0) assert row["planning_success_rate"] == pytest.approx(0.0) assert row["top_failure"] is None + assert row["start_state_bin"] == "nominal" def test_nmg_precision_and_external_accuracy_are_independently_configurable(): @@ -231,6 +233,134 @@ def test_free_space_manifest_is_seed_stable_and_algorithm_independent(): assert torch.equal(first[0].target_waypoints, second[0].target_waypoints) +def test_free_space_cases_use_one_start_state_bin_each(): + suite = load_suite("coverage") + suite.free_space.batch_sizes = [2] + suite.free_space.waypoint_counts = [1] + suite.free_space.path_shapes = ["direct"] + suite.free_space.seeds = [11] + suite.free_space.start_state_bins = ["nominal", "near_limit"] + cases = generate_free_space_cases(suite, _FakeRobot(), "arm", batch_size=2) + + assert [case.start_state_bin for case in cases] == ["nominal", "near_limit"] + assert len({case.case_id for case in cases}) == 2 + + +def test_success_metrics_are_stratified_by_start_state_bin(): + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ) + ] + cases = [ + BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="nominal-case", + seed=11, + batch_size=1, + num_waypoints=1, + path_shape="direct", + start_state_bin="nominal", + start_qpos=torch.zeros(1, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4), + reference_qpos=torch.zeros(1, 1, 7), + ), + BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="limit-case", + seed=11, + batch_size=1, + num_waypoints=1, + path_shape="direct", + start_state_bin="near_limit", + start_qpos=torch.zeros(1, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4), + reference_qpos=torch.zeros(1, 1, 7), + ), + ] + records = [ + TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=cases[0].case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=11, + repeat=0, + batch_size=1, + waypoint_count=1, + path_shape="direct", + start_state_bin="nominal", + phase=TrialPhase.MEASURED, + cost_time_ms=10.0, + outcomes=(_outcome(),), + ), + TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=cases[1].case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=11, + repeat=0, + batch_size=1, + waypoint_count=1, + path_shape="direct", + start_state_bin="near_limit", + phase=TrialPhase.MEASURED, + cost_time_ms=12.0, + outcomes=( + CaseOutcome( + env_index=0, + planning_success=False, + finite=True, + ordered_waypoints_reached=False, + motion_valid=False, + completed_waypoint_ratio=0.0, + final_translation_err_mm=None, + final_rotation_err_deg=None, + waypoint_translation_err_mm_mean=None, + waypoint_translation_err_mm_p95=None, + waypoint_translation_err_mm_max=None, + waypoint_rotation_err_deg_mean=None, + waypoint_rotation_err_deg_p95=None, + waypoint_rotation_err_deg_max=None, + joint_limit_violation=False, + max_normalized_joint_violation=0.0, + joint_path_length_rad=None, + cartesian_path_length_m=None, + path_efficiency=None, + failure_code="waypoint_miss", + ), + ), + ), + ] + + rows = aggregate_results(records, metadata, cases, measured_trials=1)[ + "success_and_metrics" + ] + by_bin = {row["start_state_bin"]: row for row in rows} + + assert set(by_bin) == {"nominal", "near_limit"} + assert by_bin["nominal"]["motion_valid_rate"] == pytest.approx(1.0) + assert by_bin["near_limit"]["motion_valid_rate"] == pytest.approx(0.0) + assert by_bin["near_limit"]["top_failure"] == "waypoint_miss" + + def _case() -> BenchmarkCase: return BenchmarkCase( suite_version="test_v1", @@ -241,7 +371,7 @@ def _case() -> BenchmarkCase: batch_size=1, num_waypoints=1, path_shape="direct", - start_state_bins=("nominal",), + start_state_bin="nominal", start_qpos=torch.zeros(1, 7), target_waypoints=torch.eye(4).reshape(1, 1, 4, 4), reference_qpos=torch.zeros(1, 1, 7), @@ -287,6 +417,7 @@ def _record(phase: TrialPhase, cost: float) -> TrialRecord: batch_size=1, waypoint_count=1, path_shape="direct", + start_state_bin="nominal", phase=phase, cost_time_ms=cost, cpu_delta_mb=1.0, From cf07e48e16acffc0c3b49543e371c0d513a76925 Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 02:25:17 +0800 Subject: [PATCH 04/17] Add track-based scenario registry --- .../planners/neural_planner/aggregation.py | 225 +++++++++++------- .../planners/neural_planner/config.py | 136 ++++++++--- .../planners/neural_planner/registry.py | 36 ++- .../planners/neural_planner/runner.py | 46 ++-- .../neural_planner/scenarios/__init__.py | 5 +- .../planners/neural_planner/scenarios/base.py | 51 ++++ .../neural_planner/scenarios/free_space.py | 77 +++--- .../neural_planner/suites/coverage.yaml | 16 +- .../planners/neural_planner/suites/smoke.yaml | 16 +- .../test_motion_generation_benchmark.py | 36 ++- 10 files changed, 446 insertions(+), 198 deletions(-) create mode 100644 scripts/benchmark/planners/neural_planner/scenarios/base.py diff --git a/scripts/benchmark/planners/neural_planner/aggregation.py b/scripts/benchmark/planners/neural_planner/aggregation.py index 4450e5eee..e83e781e0 100644 --- a/scripts/benchmark/planners/neural_planner/aggregation.py +++ b/scripts/benchmark/planners/neural_planner/aggregation.py @@ -66,16 +66,25 @@ def _top_failure(outcomes: list[CaseOutcome]) -> str | None: return failures.most_common(1)[0][0] if failures else None +def _track_ids(records: list[TrialRecord], cases: list[BenchmarkCase]) -> list[str]: + """Return deterministic track ids observed in cases or records.""" + tracks = {case.track for case in cases} + tracks.update(record.track for record in records) + return sorted(tracks) + + def _lifecycle_value( records: list[TrialRecord], + track: str, algorithm_id: str, batch_size: int, phase: TrialPhase, ) -> float | None: - """Return the first lifecycle cost for one algorithm and batch size.""" + """Return the first lifecycle cost for one track, algorithm, and batch size.""" for record in records: if ( - record.algorithm_id == algorithm_id + record.track == track + and record.algorithm_id == algorithm_id and record.batch_size == batch_size and record.phase is phase ): @@ -84,40 +93,49 @@ def _lifecycle_value( def _performance_rows( - records: list[TrialRecord], metadata: list[PlannerMetadata] + records: list[TrialRecord], + metadata: list[PlannerMetadata], + cases: list[BenchmarkCase], ) -> list[dict[str, object]]: - """Aggregate steady-state time and memory by algorithm and input shape.""" - measured_groups: dict[tuple[str, int, int], list[TrialRecord]] = defaultdict(list) + """Aggregate steady-state time and memory by track, algorithm, and input shape.""" + measured_groups: dict[tuple[str, str, int, int], list[TrialRecord]] = defaultdict( + list + ) for record in records: if record.phase is TrialPhase.MEASURED: measured_groups[ - (record.algorithm_id, record.batch_size, record.waypoint_count) + ( + record.track, + record.algorithm_id, + record.batch_size, + record.waypoint_count, + ) ].append(record) metadata_by_id = {item.algorithm_id: item for item in metadata} rows: list[dict[str, object]] = [] for key in sorted(measured_groups): - algorithm_id, batch_size, waypoint_count = key + track, algorithm_id, batch_size, waypoint_count = key group = measured_groups[key] info = metadata_by_id[algorithm_id] costs = [record.cost_time_ms for record in group] mean_cost = _mean(costs) rows.append( { - "track": "free-space-common", + "track": track, "algorithm": algorithm_id, "algorithm_role": info.algorithm_role.value, "batch_size": batch_size, "waypoint_count": waypoint_count, "num_trials": len(group), "planner_construct_ms": _lifecycle_value( - records, algorithm_id, batch_size, TrialPhase.CONSTRUCT + records, track, algorithm_id, batch_size, TrialPhase.CONSTRUCT ), "backend_prepare_ms": _lifecycle_value( - records, algorithm_id, batch_size, TrialPhase.PREPARE + records, track, algorithm_id, batch_size, TrialPhase.PREPARE ), "cold_plan_ms": _lifecycle_value( - records, algorithm_id, batch_size, TrialPhase.COLD + records, track, algorithm_id, batch_size, TrialPhase.COLD ), "cost_time_ms": mean_cost, "warm_plan_ms_p50": _percentile(costs, 50.0), @@ -141,35 +159,37 @@ def _performance_rows( } ) - present_algorithms = {row["algorithm"] for row in rows} - for info in metadata: - if info.algorithm_id in present_algorithms: - continue - rows.append( - { - "track": "free-space-common", - "algorithm": info.algorithm_id, - "algorithm_role": info.algorithm_role.value, - "batch_size": None, - "waypoint_count": None, - "num_trials": 0, - "planner_construct_ms": None, - "backend_prepare_ms": None, - "cold_plan_ms": None, - "cost_time_ms": None, - "warm_plan_ms_p50": None, - "warm_plan_ms_p95": None, - "latency_per_env_ms": None, - "cost_time_per_segment_ms": None, - "trajectories_per_second": None, - "cpu_delta_mb": None, - "gpu_delta_mb": None, - "peak_gpu_mb": None, - } - ) + present = {(row["track"], row["algorithm"]) for row in rows} + for track in _track_ids(records, cases): + for info in metadata: + if (track, info.algorithm_id) in present: + continue + rows.append( + { + "track": track, + "algorithm": info.algorithm_id, + "algorithm_role": info.algorithm_role.value, + "batch_size": None, + "waypoint_count": None, + "num_trials": 0, + "planner_construct_ms": None, + "backend_prepare_ms": None, + "cold_plan_ms": None, + "cost_time_ms": None, + "warm_plan_ms_p50": None, + "warm_plan_ms_p95": None, + "latency_per_env_ms": None, + "cost_time_per_segment_ms": None, + "trajectories_per_second": None, + "cpu_delta_mb": None, + "gpu_delta_mb": None, + "peak_gpu_mb": None, + } + ) return sorted( rows, key=lambda row: ( + str(row["track"]), str(row["algorithm"]), int(row["batch_size"] or 0), int(row["waypoint_count"] or 0), @@ -184,13 +204,14 @@ def _metric_rows( measured_trials: int, ) -> list[dict[str, object]]: """Aggregate external success and quality metrics by scenario condition.""" - outcome_groups: dict[tuple[str, str, int, int, str, str], list[CaseOutcome]] = ( - defaultdict(list) - ) + outcome_groups: dict[ + tuple[str, str, str, int, int, str, str], list[CaseOutcome] + ] = defaultdict(list) for record in records: if record.phase is not TrialPhase.MEASURED: continue key = ( + record.track, record.algorithm_id, record.scenario_id, record.batch_size, @@ -200,10 +221,11 @@ def _metric_rows( ) outcome_groups[key].extend(record.outcomes) - expected_by_group: Counter[tuple[str, int, int, str, str]] = Counter() - unique_cases_by_group: Counter[tuple[str, int, int, str, str]] = Counter() + expected_by_group: Counter[tuple[str, str, int, int, str, str]] = Counter() + unique_cases_by_group: Counter[tuple[str, str, int, int, str, str]] = Counter() for case in cases: key = ( + case.track, case.scenario_id, case.batch_size, case.num_waypoints, @@ -216,11 +238,17 @@ def _metric_rows( rows: list[dict[str, object]] = [] for info in metadata: for group_key in sorted(expected_by_group): - scenario_id, batch_size, waypoint_count, path_shape, start_state_bin = ( - group_key - ) + ( + track, + scenario_id, + batch_size, + waypoint_count, + path_shape, + start_state_bin, + ) = group_key outcomes = outcome_groups.get( ( + track, info.algorithm_id, scenario_id, batch_size, @@ -234,7 +262,7 @@ def _metric_rows( expected = expected_by_group[group_key] rows.append( { - "track": "free-space-common", + "track": track, "scenario": scenario_id, "algorithm": info.algorithm_id, "algorithm_role": info.algorithm_role.value, @@ -295,54 +323,69 @@ def _leaderboard_rows( cases: list[BenchmarkCase], measured_trials: int, ) -> list[dict[str, object]]: - """Build a complete success/coverage/latency ordered leaderboard.""" - expected_outcomes = sum(case.batch_size for case in cases) * measured_trials + """Build a complete success/coverage/latency ordered leaderboard per track.""" entries: list[dict[str, object]] = [] - for info in metadata: - measured = [ - record - for record in records - if record.algorithm_id == info.algorithm_id - and record.phase is TrialPhase.MEASURED - ] - outcomes = [outcome for record in measured for outcome in record.outcomes] - coverage = min(1.0, len(outcomes) / max(expected_outcomes, 1)) - motion_rate = _rate(outcome.motion_valid for outcome in outcomes) or 0.0 - planning_rate = _rate(outcome.planning_success for outcome in outcomes) or 0.0 - latency_p95 = _percentile((record.cost_time_ms for record in measured), 95.0) - peak_gpu = max((record.peak_gpu_mb or 0.0 for record in measured), default=None) - entries.append( - { - "track": "free-space-common", - "algorithm": info.algorithm_id, - "algorithm_role": info.algorithm_role.value, - "model_revision": info.model_revision, - "planner_config_hash": info.config_hash[:12], - "eligible": coverage >= 1.0 - 1.0e-12, - "coverage_rate": coverage, - "overall_success_rate": motion_rate, - "planning_success_rate": planning_rate, - "motion_valid_rate": motion_rate, - "task_success_rate": None, - "latency_p95_ms": latency_p95, - "peak_gpu_mb": peak_gpu, - } + for track in _track_ids(records, cases): + track_cases = [case for case in cases if case.track == track] + expected_outcomes = ( + sum(case.batch_size for case in track_cases) * measured_trials ) + track_entries: list[dict[str, object]] = [] + for info in metadata: + measured = [ + record + for record in records + if record.algorithm_id == info.algorithm_id + and record.track == track + and record.phase is TrialPhase.MEASURED + ] + outcomes = [outcome for record in measured for outcome in record.outcomes] + coverage = min(1.0, len(outcomes) / max(expected_outcomes, 1)) + motion_rate = _rate(outcome.motion_valid for outcome in outcomes) or 0.0 + planning_rate = ( + _rate(outcome.planning_success for outcome in outcomes) or 0.0 + ) + latency_p95 = _percentile( + (record.cost_time_ms for record in measured), 95.0 + ) + peak_gpu = max( + (record.peak_gpu_mb or 0.0 for record in measured), default=None + ) + track_entries.append( + { + "track": track, + "algorithm": info.algorithm_id, + "algorithm_role": info.algorithm_role.value, + "model_revision": info.model_revision, + "planner_config_hash": info.config_hash[:12], + "eligible": coverage >= 1.0 - 1.0e-12, + "coverage_rate": coverage, + "overall_success_rate": motion_rate, + "planning_success_rate": planning_rate, + "motion_valid_rate": motion_rate, + "task_success_rate": None, + "latency_p95_ms": latency_p95, + "peak_gpu_mb": peak_gpu, + } + ) - entries.sort( - key=lambda row: ( - not bool(row["eligible"]), - -float(row["overall_success_rate"]), - -float(row["coverage_rate"]), - ( - float(row["latency_p95_ms"]) - if row["latency_p95_ms"] is not None - else math.inf - ), - str(row["algorithm"]), + track_entries.sort( + key=lambda row: ( + not bool(row["eligible"]), + -float(row["overall_success_rate"]), + -float(row["coverage_rate"]), + ( + float(row["latency_p95_ms"]) + if row["latency_p95_ms"] is not None + else math.inf + ), + str(row["algorithm"]), + ) ) - ) - return [{"rank": rank, **entry} for rank, entry in enumerate(entries, start=1)] + entries.extend( + {"rank": rank, **entry} for rank, entry in enumerate(track_entries, start=1) + ) + return entries def aggregate_results( @@ -353,7 +396,7 @@ def aggregate_results( ) -> dict[str, list[dict[str, object]]]: """Build all three report datasets from raw numeric records.""" return { - "time_and_memory": _performance_rows(records, metadata), + "time_and_memory": _performance_rows(records, metadata, cases), "success_and_metrics": _metric_rows(records, metadata, cases, measured_trials), "leaderboard": _leaderboard_rows(records, metadata, cases, measured_trials), } diff --git a/scripts/benchmark/planners/neural_planner/config.py b/scripts/benchmark/planners/neural_planner/config.py index ff200b16b..592d5b579 100644 --- a/scripts/benchmark/planners/neural_planner/config.py +++ b/scripts/benchmark/planners/neural_planner/config.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Configuration loading and hashing for free-space planner benchmark suites.""" +"""Configuration loading and hashing for motion-generation benchmark suites.""" from __future__ import annotations @@ -36,12 +36,15 @@ "PlannerSpecCfg", "ProtocolCfg", "SuiteCfg", + "TrackCfg", "load_suite", "stable_hash", "suite_to_dict", ] BENCHMARK_ROOT = Path(__file__).resolve().parent +_FREE_SPACE_TRACK_ID = "free-space-common" +_FREE_SPACE_SCENARIO = "free_space" _SUPPORTED_PATH_SHAPES = { "direct", "l_turn", @@ -92,6 +95,16 @@ class FreeSpaceTrackCfg: seeds: list[int] = [11] +@configclass +class TrackCfg: + """One enabled benchmark track and its scenario provider.""" + + id: str = "" + scenario: str = "" + enabled: bool = True + config: dict[str, Any] = {} + + @configclass class SuiteCfg: """Resolved benchmark suite configuration.""" @@ -102,12 +115,14 @@ class SuiteCfg: profile: str = "smoke" planners: list[PlannerSpecCfg] = [] protocol: ProtocolCfg = ProtocolCfg() + tracks: list[TrackCfg] = [] free_space: FreeSpaceTrackCfg = FreeSpaceTrackCfg() @classmethod def from_dict(cls, data: dict[str, Any]) -> "SuiteCfg": """Build and validate a suite from a YAML-compatible mapping.""" planners = [PlannerSpecCfg(**item) for item in data.get("planners", [])] + tracks, free_space = _resolve_tracks_and_free_space(data) suite = cls( schema_version=int(data.get("schema_version", 1)), name=str(data.get("name", "free_space_common")), @@ -115,13 +130,26 @@ def from_dict(cls, data: dict[str, Any]) -> "SuiteCfg": profile=str(data.get("profile", "smoke")), planners=planners, protocol=ProtocolCfg(**data.get("protocol", {})), - free_space=FreeSpaceTrackCfg(**data.get("free_space", {})), + tracks=tracks, + free_space=free_space, ) suite.validate_benchmark() return suite + def enabled_tracks(self) -> list[TrackCfg]: + """Return enabled tracks in suite order.""" + return [track for track in self.tracks if track.enabled] + + def sync_track_configs(self) -> None: + """Copy typed free-space settings into the matching track config.""" + for track in self.tracks: + if track.scenario == _FREE_SPACE_SCENARIO: + track.id = track.id or _FREE_SPACE_TRACK_ID + track.config = asdict(self.free_space) + def validate_benchmark(self) -> None: """Validate values that affect benchmark correctness.""" + self.sync_track_configs() missing_fields = self.validate() if missing_fields: raise ValueError( @@ -143,6 +171,16 @@ def validate_benchmark(self) -> None: "Every planner must define a non-empty id and adapter." ) AlgorithmRole(spec.role) + if not self.tracks: + raise ValueError("The benchmark suite must declare at least one track.") + track_ids = [track.id for track in self.tracks] + if len(track_ids) != len(set(track_ids)): + raise ValueError("Track ids must be unique within a suite.") + if not self.enabled_tracks(): + raise ValueError("At least one track must be enabled.") + for track in self.tracks: + if not track.id or not track.scenario: + raise ValueError("Every track must define a non-empty id and scenario.") if self.protocol.warmup_trials < 0: raise ValueError("warmup_trials must be >= 0.") if self.protocol.measured_trials < 1: @@ -157,37 +195,8 @@ 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.") - if not self.free_space.batch_sizes or any( - value < 1 for value in self.free_space.batch_sizes - ): - raise ValueError("batch_sizes must contain positive integers.") - if not self.free_space.waypoint_counts or any( - value < 1 for value in self.free_space.waypoint_counts - ): - raise ValueError("waypoint_counts must contain positive integers.") - if not self.free_space.seeds: - raise ValueError("seeds must not be empty.") - if not self.free_space.path_shapes: - raise ValueError("path_shapes must not be empty.") - unknown_shapes = set(self.free_space.path_shapes) - _SUPPORTED_PATH_SHAPES - if unknown_shapes: - raise ValueError(f"Unsupported path_shapes: {sorted(unknown_shapes)}.") - if not self.free_space.start_state_bins: - raise ValueError("start_state_bins must not be empty.") - unknown_bins = ( - set(self.free_space.start_state_bins) - _SUPPORTED_START_STATE_BINS - ) - if unknown_bins: - raise ValueError(f"Unsupported start_state_bins: {sorted(unknown_bins)}.") - for name, values in ( - ("batch_sizes", self.free_space.batch_sizes), - ("waypoint_counts", self.free_space.waypoint_counts), - ("path_shapes", self.free_space.path_shapes), - ("start_state_bins", self.free_space.start_state_bins), - ("seeds", self.free_space.seeds), - ): - if len(values) != len(set(values)): - raise ValueError(f"{name} must not contain duplicate values.") + if any(track.scenario == _FREE_SPACE_SCENARIO for track in self.tracks): + _validate_free_space(self.free_space) nmg = next((spec for spec in self.planners if spec.id == "nmg"), None) if nmg is not None: if float(nmg.config.get("pos_eps", 0.05)) <= 0.0: @@ -196,6 +205,62 @@ def validate_benchmark(self) -> None: raise ValueError("NMG rot_eps must be > 0.") +def _resolve_tracks_and_free_space( + data: dict[str, Any], +) -> tuple[list[TrackCfg], FreeSpaceTrackCfg]: + """Accept either ``tracks`` or legacy top-level ``free_space``.""" + tracks_data = data.get("tracks") + free_space_data = dict(data.get("free_space", {}) or {}) + if tracks_data is None: + tracks = [ + TrackCfg( + id=_FREE_SPACE_TRACK_ID, + scenario=_FREE_SPACE_SCENARIO, + enabled=True, + config=dict(free_space_data), + ) + ] + else: + if not isinstance(tracks_data, list): + raise TypeError("tracks must be a list of track mappings.") + tracks = [TrackCfg(**item) for item in tracks_data] + for track in tracks: + if track.scenario == _FREE_SPACE_SCENARIO and track.config: + free_space_data = {**free_space_data, **dict(track.config)} + return tracks, FreeSpaceTrackCfg(**free_space_data) + + +def _validate_free_space(free_space: FreeSpaceTrackCfg) -> None: + """Validate free-space case-matrix fields.""" + if not free_space.batch_sizes or any(value < 1 for value in free_space.batch_sizes): + raise ValueError("batch_sizes must contain positive integers.") + if not free_space.waypoint_counts or any( + value < 1 for value in free_space.waypoint_counts + ): + raise ValueError("waypoint_counts must contain positive integers.") + if not free_space.seeds: + raise ValueError("seeds must not be empty.") + if not free_space.path_shapes: + raise ValueError("path_shapes must not be empty.") + unknown_shapes = set(free_space.path_shapes) - _SUPPORTED_PATH_SHAPES + if unknown_shapes: + raise ValueError(f"Unsupported path_shapes: {sorted(unknown_shapes)}.") + if not free_space.start_state_bins: + raise ValueError("start_state_bins must not be empty.") + unknown_bins = set(free_space.start_state_bins) - _SUPPORTED_START_STATE_BINS + if unknown_bins: + raise ValueError(f"Unsupported start_state_bins: {sorted(unknown_bins)}.") + for name, values in ( + ("batch_sizes", free_space.batch_sizes), + ("waypoint_counts", free_space.waypoint_counts), + ("path_shapes", free_space.path_shapes), + ("start_state_bins", free_space.start_state_bins), + ("seeds", free_space.seeds), + ): + if len(values) != len(set(values)): + raise ValueError(f"{name} must not contain duplicate values.") + + def load_suite(name_or_path: str = "smoke") -> SuiteCfg: """Load a suite by short name or explicit YAML path.""" requested = Path(name_or_path) @@ -217,7 +282,10 @@ def load_suite(name_or_path: str = "smoke") -> SuiteCfg: def suite_to_dict(suite: SuiteCfg) -> dict[str, Any]: """Convert a resolved suite to plain YAML/JSON-compatible values.""" - return asdict(suite) + suite.sync_track_configs() + data = asdict(suite) + data.pop("free_space", None) + return data def stable_hash(value: object) -> str: diff --git a/scripts/benchmark/planners/neural_planner/registry.py b/scripts/benchmark/planners/neural_planner/registry.py index ea2e3e425..e43aa6fb0 100644 --- a/scripts/benchmark/planners/neural_planner/registry.py +++ b/scripts/benchmark/planners/neural_planner/registry.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Planner adapter registry used by the generic benchmark runner.""" +"""Planner and scenario registries used by the generic benchmark runner.""" from __future__ import annotations @@ -23,14 +23,19 @@ if TYPE_CHECKING: from .config import PlannerSpecCfg from .planners.base import PlannerAdapter, PlannerContext + from .scenarios.base import ScenarioProvider __all__ = [ "create_planner_adapter", + "create_scenario_provider", "planner_adapter_names", "register_planner_adapter", + "register_scenario_provider", + "scenario_provider_names", ] _PLANNER_ADAPTERS: dict[str, type["PlannerAdapter"]] = {} +_SCENARIO_PROVIDERS: dict[str, type["ScenarioProvider"]] = {} def register_planner_adapter(name: str, adapter_cls: type["PlannerAdapter"]) -> None: @@ -60,3 +65,32 @@ def create_planner_adapter( f"registered adapters: {planner_adapter_names()}." ) from exc return adapter_cls(spec=spec, context=context) + + +def register_scenario_provider( + name: str, provider_cls: type["ScenarioProvider"] +) -> None: + """Register one scenario provider class under a stable configuration name.""" + if not name: + raise ValueError("Scenario provider name must not be empty.") + previous = _SCENARIO_PROVIDERS.get(name) + if previous is not None and previous is not provider_cls: + raise ValueError(f"Scenario provider {name!r} is already registered.") + _SCENARIO_PROVIDERS[name] = provider_cls + + +def scenario_provider_names() -> tuple[str, ...]: + """Return registered scenario provider names in deterministic order.""" + return tuple(sorted(_SCENARIO_PROVIDERS)) + + +def create_scenario_provider(name: str) -> "ScenarioProvider": + """Construct the scenario provider selected by a track specification.""" + try: + provider_cls = _SCENARIO_PROVIDERS[name] + except KeyError as exc: + raise ValueError( + f"Unknown scenario provider {name!r}; " + f"registered providers: {scenario_provider_names()}." + ) from exc + return provider_cls() diff --git a/scripts/benchmark/planners/neural_planner/runner.py b/scripts/benchmark/planners/neural_planner/runner.py index c60717f36..e44474211 100644 --- a/scripts/benchmark/planners/neural_planner/runner.py +++ b/scripts/benchmark/planners/neural_planner/runner.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Generic lifecycle runner for the ``free-space-common`` benchmark.""" +"""Generic lifecycle runner for motion-generation benchmark tracks.""" from __future__ import annotations @@ -29,6 +29,7 @@ from embodichain.lab.sim.robots import FrankaPandaCfg from . import planners as _builtin_planners # noqa: F401 - registry side effects +from . import scenarios as _builtin_scenarios # noqa: F401 - registry side effects from .aggregation import aggregate_results from .artifacts import ( TrialJsonlWriter, @@ -48,9 +49,8 @@ TrialRecord, ) from .planners.base import PlannerAdapter, PlannerContext -from .registry import create_planner_adapter +from .registry import create_planner_adapter, create_scenario_provider from .reporting import write_markdown_report -from .scenarios import generate_free_space_cases if TYPE_CHECKING: from collections.abc import Callable @@ -395,30 +395,34 @@ def run(self) -> BenchmarkRunResult: writer = TrialJsonlWriter(run_dir / "trials.jsonl") print("=" * 60) - print("Motion Generation Free-Space Benchmark") + print("Motion Generation Benchmark") print("=" * 60) + enabled_tracks = self.suite.enabled_tracks() print( f"suite={self.suite.suite_version} device={self.device} " + f"tracks={','.join(track.id for track in enabled_tracks)} " f"planners={','.join(spec.id for spec in self.planner_specs)}" ) - for batch_size in self.suite.free_space.batch_sizes: - sim: SimulationManager | None = None - try: - sim, robot = self._create_simulation(batch_size) - cases = generate_free_space_cases( - self.suite, robot, _CONTROL_PART, batch_size - ) - self.cases.extend(cases) - for spec in self.planner_specs: - self._run_adapter(writer, sim, robot, spec, cases) - finally: - if sim is not None: - # Benchmarks must aggregate and report after simulator - # teardown; the SimulationManager default exits the whole - # process, so opt into deferred in-process cleanup here. - sim.destroy(exit_process=False) - SimulationManager.flush_cleanup_queue() + for track in enabled_tracks: + provider = create_scenario_provider(track.scenario) + for batch_size in provider.batch_sizes(self.suite, track): + sim: SimulationManager | None = None + try: + sim, robot = self._create_simulation(batch_size) + cases = provider.generate_cases( + self.suite, track, robot, _CONTROL_PART, batch_size + ) + self.cases.extend(cases) + for spec in self.planner_specs: + self._run_adapter(writer, sim, robot, spec, cases) + finally: + if sim is not None: + # Benchmarks must aggregate and report after simulator + # teardown; the SimulationManager default exits the whole + # process, so opt into deferred in-process cleanup here. + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() write_case_manifest(run_dir / "case_manifest.json", self.cases) metadata = [ diff --git a/scripts/benchmark/planners/neural_planner/scenarios/__init__.py b/scripts/benchmark/planners/neural_planner/scenarios/__init__.py index 31156ef03..e959d74df 100644 --- a/scripts/benchmark/planners/neural_planner/scenarios/__init__.py +++ b/scripts/benchmark/planners/neural_planner/scenarios/__init__.py @@ -18,6 +18,7 @@ from __future__ import annotations -from .free_space import generate_free_space_cases +from .base import ScenarioProvider +from .free_space import FreeSpaceScenario -__all__ = ["generate_free_space_cases"] +__all__ = ["FreeSpaceScenario", "ScenarioProvider"] diff --git a/scripts/benchmark/planners/neural_planner/scenarios/base.py b/scripts/benchmark/planners/neural_planner/scenarios/base.py new file mode 100644 index 000000000..11435cb71 --- /dev/null +++ b/scripts/benchmark/planners/neural_planner/scenarios/base.py @@ -0,0 +1,51 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Scenario provider contract for motion-generation tracks.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + + from ..config import SuiteCfg, TrackCfg + from ..models import BenchmarkCase + +__all__ = ["ScenarioProvider"] + + +class ScenarioProvider(ABC): + """Generate fixed cases for one registered scenario kind.""" + + required_capabilities: frozenset[str] = frozenset() + + @abstractmethod + def batch_sizes(self, suite: "SuiteCfg", track: "TrackCfg") -> list[int]: + """Return simulator batch sizes required by this track.""" + + @abstractmethod + def generate_cases( + self, + suite: "SuiteCfg", + track: "TrackCfg", + robot: "Robot", + control_part: str, + batch_size: int, + ) -> list["BenchmarkCase"]: + """Build the frozen case manifest for one batch size.""" diff --git a/scripts/benchmark/planners/neural_planner/scenarios/free_space.py b/scripts/benchmark/planners/neural_planner/scenarios/free_space.py index a8cc5eb15..d8d853a04 100644 --- a/scripts/benchmark/planners/neural_planner/scenarios/free_space.py +++ b/scripts/benchmark/planners/neural_planner/scenarios/free_space.py @@ -23,13 +23,15 @@ import torch -from ..config import SuiteCfg, stable_hash +from ..config import SuiteCfg, TrackCfg, stable_hash from ..models import BenchmarkCase +from ..registry import register_scenario_provider +from .base import ScenarioProvider if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot -__all__ = ["generate_free_space_cases"] +__all__ = ["FreeSpaceScenario"] _NOMINAL_QPOS = torch.tensor( [0.0, -math.pi / 4, 0.0, -3.0 * math.pi / 4, 0.0, math.pi / 2, math.pi / 4], @@ -125,6 +127,7 @@ def _build_case( shape_index: int, start_state_bin: str, bin_index: int, + track_id: str, ) -> BenchmarkCase: """Build one reachable env-batched case using FK reference targets.""" limits = robot.get_qpos_limits(name=control_part)[0].detach().cpu() @@ -166,6 +169,7 @@ def _build_case( identity = { "suite_version": suite.suite_version, + "track": track_id, "seed": seed, "batch_size": batch_size, "num_waypoints": num_waypoints, @@ -175,7 +179,7 @@ def _build_case( case_id = f"free_space_{stable_hash(identity)[:16]}" return BenchmarkCase( suite_version=suite.suite_version, - track="free-space-common", + track=track_id, scenario_id="waypoint_path" if num_waypoints > 1 else "reach", case_id=case_id, seed=seed, @@ -189,32 +193,43 @@ def _build_case( ) -def generate_free_space_cases( - suite: SuiteCfg, - robot: "Robot", - control_part: str, - batch_size: int, -) -> list[BenchmarkCase]: - """Generate the fixed case manifest for one simulator batch size.""" - cases: list[BenchmarkCase] = [] - for seed in suite.free_space.seeds: - for num_waypoints in suite.free_space.waypoint_counts: - for shape_index, path_shape in enumerate(suite.free_space.path_shapes): - for bin_index, start_state_bin in enumerate( - suite.free_space.start_state_bins - ): - cases.append( - _build_case( - suite, - robot, - control_part, - seed=seed, - batch_size=batch_size, - num_waypoints=num_waypoints, - path_shape=path_shape, - shape_index=shape_index, - start_state_bin=start_state_bin, - bin_index=bin_index, +class FreeSpaceScenario(ScenarioProvider): + required_capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) + + def batch_sizes(self, suite: SuiteCfg, track: TrackCfg) -> list[int]: # noqa: ARG002 + return list(suite.free_space.batch_sizes) + + def generate_cases( + self, + suite: SuiteCfg, + track: TrackCfg, + robot: "Robot", + control_part: str, + batch_size: int, + ) -> list[BenchmarkCase]: + cases: list[BenchmarkCase] = [] + for seed in suite.free_space.seeds: + for num_waypoints in suite.free_space.waypoint_counts: + for shape_index, path_shape in enumerate(suite.free_space.path_shapes): + for bin_index, start_state_bin in enumerate( + suite.free_space.start_state_bins + ): + cases.append( + _build_case( + suite, + robot, + control_part, + seed=seed, + batch_size=batch_size, + num_waypoints=num_waypoints, + path_shape=path_shape, + shape_index=shape_index, + start_state_bin=start_state_bin, + bin_index=bin_index, + track_id=track.id, + ) ) - ) - return cases + return cases + + +register_scenario_provider("free_space", FreeSpaceScenario) diff --git a/scripts/benchmark/planners/neural_planner/suites/coverage.yaml b/scripts/benchmark/planners/neural_planner/suites/coverage.yaml index 1f4033aa4..fffc8ab82 100644 --- a/scripts/benchmark/planners/neural_planner/suites/coverage.yaml +++ b/scripts/benchmark/planners/neural_planner/suites/coverage.yaml @@ -54,9 +54,13 @@ protocol: rotation_threshold_rad: 0.3 joint_limit_tolerance_rad: 0.00001 -free_space: - batch_sizes: [1, 8, 64] - waypoint_counts: [1, 3, 5] - path_shapes: [direct, l_turn, s_curve, orientation_only, combined] - start_state_bins: [nominal, random_reachable, near_limit, near_singularity] - seeds: [11, 23, 37, 53, 71] +tracks: + - id: free-space-common + scenario: free_space + enabled: true + config: + batch_sizes: [1, 8, 64] + waypoint_counts: [1, 3, 5] + path_shapes: [direct, l_turn, s_curve, orientation_only, combined] + start_state_bins: [nominal, random_reachable, near_limit, near_singularity] + seeds: [11, 23, 37, 53, 71] diff --git a/scripts/benchmark/planners/neural_planner/suites/smoke.yaml b/scripts/benchmark/planners/neural_planner/suites/smoke.yaml index c798333a6..fc5a7087b 100644 --- a/scripts/benchmark/planners/neural_planner/suites/smoke.yaml +++ b/scripts/benchmark/planners/neural_planner/suites/smoke.yaml @@ -54,9 +54,13 @@ protocol: rotation_threshold_rad: 0.3 joint_limit_tolerance_rad: 0.00001 -free_space: - batch_sizes: [1] - waypoint_counts: [1, 3, 5] - path_shapes: [direct, l_turn, s_curve] - start_state_bins: [nominal] - seeds: [11] +tracks: + - id: free-space-common + scenario: free_space + enabled: true + config: + batch_sizes: [1] + waypoint_counts: [1, 3, 5] + path_shapes: [direct, l_turn, s_curve] + start_state_bins: [nominal] + seeds: [11] diff --git a/tests/benchmark/planners/test_motion_generation_benchmark.py b/tests/benchmark/planners/test_motion_generation_benchmark.py index 25695a55a..64ea24ef3 100644 --- a/tests/benchmark/planners/test_motion_generation_benchmark.py +++ b/tests/benchmark/planners/test_motion_generation_benchmark.py @@ -31,6 +31,9 @@ compute_case_outcomes, match_ordered_waypoints, ) +from scripts.benchmark.planners.neural_planner import ( + scenarios as _scenarios, +) # noqa: F401 from scripts.benchmark.planners.neural_planner.models import ( AlgorithmRole, BenchmarkCase, @@ -39,15 +42,16 @@ TrialPhase, TrialRecord, ) +from scripts.benchmark.planners.neural_planner.registry import ( + create_scenario_provider, + scenario_provider_names, +) from scripts.benchmark.planners.neural_planner.reporting import ( write_markdown_report, ) from scripts.benchmark.planners.neural_planner.run_benchmark import ( _apply_overrides, ) -from scripts.benchmark.planners.neural_planner.scenarios.free_space import ( - generate_free_space_cases, -) def _translated_pose(x: float) -> torch.Tensor: @@ -221,16 +225,33 @@ def compute_fk( return poses +def test_suite_loads_tracks_and_keeps_mutable_free_space_config(): + suite = load_suite("smoke") + + assert [track.id for track in suite.enabled_tracks()] == ["free-space-common"] + assert suite.enabled_tracks()[0].scenario == "free_space" + assert "free_space" in scenario_provider_names() + provider = create_scenario_provider("free_space") + assert provider.batch_sizes(suite, suite.enabled_tracks()[0]) == [1] + + suite.free_space.batch_sizes = [1, 8] + suite.validate_benchmark() + assert suite.enabled_tracks()[0].config["batch_sizes"] == [1, 8] + + def test_free_space_manifest_is_seed_stable_and_algorithm_independent(): suite = load_suite("smoke") robot = _FakeRobot() + provider = create_scenario_provider("free_space") + track = suite.enabled_tracks()[0] - first = generate_free_space_cases(suite, robot, "arm", batch_size=1) - second = generate_free_space_cases(suite, robot, "arm", batch_size=1) + first = provider.generate_cases(suite, track, robot, "arm", batch_size=1) + second = provider.generate_cases(suite, track, robot, "arm", batch_size=1) assert [case.case_id for case in first] == [case.case_id for case in second] assert torch.equal(first[0].start_qpos, second[0].start_qpos) assert torch.equal(first[0].target_waypoints, second[0].target_waypoints) + assert {case.track for case in first} == {"free-space-common"} def test_free_space_cases_use_one_start_state_bin_each(): @@ -240,7 +261,10 @@ def test_free_space_cases_use_one_start_state_bin_each(): suite.free_space.path_shapes = ["direct"] suite.free_space.seeds = [11] suite.free_space.start_state_bins = ["nominal", "near_limit"] - cases = generate_free_space_cases(suite, _FakeRobot(), "arm", batch_size=2) + track = suite.enabled_tracks()[0] + cases = create_scenario_provider("free_space").generate_cases( + suite, track, _FakeRobot(), "arm", batch_size=2 + ) assert [case.start_state_bin for case in cases] == ["nominal", "near_limit"] assert len({case.case_id for case in cases}) == 2 From 9ed48378fdfe4be3889a2709991191386a11971c Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 02:47:14 +0800 Subject: [PATCH 05/17] Restructure files --- docs/source/guides/cli.md | 4 ++-- scripts/benchmark/__main__.py | 24 +++++++++---------- .../__init__.py | 2 +- .../run_benchmark.py} | 2 +- .../BENCHMARK_DESIGN.md | 6 ++--- .../benchmark/motion_generation/__init__.py | 21 ++++++++++++++++ .../aggregation.py | 0 .../artifacts.py | 0 .../compat.py | 0 .../config.py | 0 .../metrics/__init__.py | 0 .../metrics/performance.py | 0 .../metrics/trajectory.py | 0 .../models.py | 0 .../planners/__init__.py | 0 .../planners/base.py | 0 .../planners/curobo.py | 0 .../planners/ik_interpolate.py | 0 .../planners/neural.py | 0 .../planners/toppra.py | 0 .../registry.py | 0 .../reporting.py | 0 .../run_benchmark.py | 2 +- .../runner.py | 0 .../scenarios/__init__.py | 0 .../scenarios/base.py | 0 .../scenarios/free_space.py | 0 .../suites/coverage.yaml | 0 .../suites/smoke.yaml | 0 tests/benchmark/motion_generation/__init__.py | 21 ++++++++++++++++ .../test_motion_generation_benchmark.py | 16 ++++++------- .../planners/test_neural_planner_benchmark.py | 2 +- 32 files changed, 70 insertions(+), 30 deletions(-) rename scripts/benchmark/{planners/neural_planner => curobo_extraction}/__init__.py (93%) rename scripts/benchmark/{planners/benchmark_curobo_extraction.py => curobo_extraction/run_benchmark.py} (99%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/BENCHMARK_DESIGN.md (99%) create mode 100644 scripts/benchmark/motion_generation/__init__.py rename scripts/benchmark/{planners/neural_planner => motion_generation}/aggregation.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/artifacts.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/compat.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/config.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/metrics/__init__.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/metrics/performance.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/metrics/trajectory.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/models.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/planners/__init__.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/planners/base.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/planners/curobo.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/planners/ik_interpolate.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/planners/neural.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/planners/toppra.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/registry.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/reporting.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/run_benchmark.py (99%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/runner.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/scenarios/__init__.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/scenarios/base.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/scenarios/free_space.py (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/suites/coverage.yaml (100%) rename scripts/benchmark/{planners/neural_planner => motion_generation}/suites/smoke.yaml (100%) create mode 100644 tests/benchmark/motion_generation/__init__.py rename tests/benchmark/{planners => motion_generation}/test_motion_generation_benchmark.py (96%) diff --git a/docs/source/guides/cli.md b/docs/source/guides/cli.md index 3a0c7e17e..2aeefb11a 100644 --- a/docs/source/guides/cli.md +++ b/docs/source/guides/cli.md @@ -394,9 +394,9 @@ Run the packaged benchmark suites through the same CLI: # RL train/evaluate/report workflow embodichain benchmark rl --tasks push_cube --algorithms ppo -# Kinematic solver and neural planner benchmarks +# Kinematic solver and motion-generation benchmarks embodichain benchmark robotics-kinematic-solver --solvers all -embodichain benchmark planners-neural-planner --num-waypoints 1 3 5 +embodichain benchmark motion-generation --suite smoke # Atomic actions, grasp generation, and workspace analysis embodichain benchmark atomic-action --smoke diff --git a/scripts/benchmark/__main__.py b/scripts/benchmark/__main__.py index e25ec2463..226843aa4 100644 --- a/scripts/benchmark/__main__.py +++ b/scripts/benchmark/__main__.py @@ -21,7 +21,7 @@ embodichain benchmark rl --tasks push_cube --algorithms ppo --suite default embodichain benchmark rl --rebuild-report-only embodichain benchmark robotics-kinematic-solver -s pytorch - embodichain benchmark planners-neural-planner --num-waypoints 1 3 5 + embodichain benchmark motion-generation --suite smoke embodichain benchmark atomic-action --smoke embodichain benchmark grasp-pose-generator --device cuda embodichain benchmark workspace-analyzer @@ -50,11 +50,9 @@ def _run_rl_cli(_: argparse.Namespace) -> None: rl_main() -def _run_neural_planner_cli(args: argparse.Namespace) -> None: +def _run_motion_generation_cli(args: argparse.Namespace) -> None: """Run the free-space motion-generation benchmark.""" - from scripts.benchmark.planners.neural_planner.run_benchmark import ( - run_from_args, - ) + from scripts.benchmark.motion_generation.run_benchmark import run_from_args run_from_args(args) @@ -113,17 +111,17 @@ def main(argv: Sequence[str] | None = None) -> None: ) robotics_ks_parser.set_defaults(func=_run_robotics_kinematic_solver_cli) - # -- planners-neural-planner -------------------------------------------- - neural_planner_parser = subparsers.add_parser( - "planners-neural-planner", - help="Benchmark free-space motion generation with cuRobo as baseline.", - ) - from scripts.benchmark.planners.neural_planner.run_benchmark import ( + # -- motion-generation --------------------------------------------------- + from scripts.benchmark.motion_generation.run_benchmark import ( add_parser_arguments, ) - add_parser_arguments(neural_planner_parser) - neural_planner_parser.set_defaults(func=_run_neural_planner_cli) + motion_generation_parser = subparsers.add_parser( + "motion-generation", + help="Benchmark free-space motion generation with cuRobo as baseline.", + ) + add_parser_arguments(motion_generation_parser) + motion_generation_parser.set_defaults(func=_run_motion_generation_cli) # -- atomic-action ------------------------------------------------------- atomic_action_parser = subparsers.add_parser( diff --git a/scripts/benchmark/planners/neural_planner/__init__.py b/scripts/benchmark/curobo_extraction/__init__.py similarity index 93% rename from scripts/benchmark/planners/neural_planner/__init__.py rename to scripts/benchmark/curobo_extraction/__init__.py index 70862ba73..02562579c 100644 --- a/scripts/benchmark/planners/neural_planner/__init__.py +++ b/scripts/benchmark/curobo_extraction/__init__.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Extensible free-space motion-generation benchmark.""" +"""cuRobo post-processing hot-path microbenchmark.""" from __future__ import annotations diff --git a/scripts/benchmark/planners/benchmark_curobo_extraction.py b/scripts/benchmark/curobo_extraction/run_benchmark.py similarity index 99% rename from scripts/benchmark/planners/benchmark_curobo_extraction.py rename to scripts/benchmark/curobo_extraction/run_benchmark.py index fa6f8c3a8..f8d07603b 100644 --- a/scripts/benchmark/planners/benchmark_curobo_extraction.py +++ b/scripts/benchmark/curobo_extraction/run_benchmark.py @@ -29,7 +29,7 @@ old ``_extract_segment`` does B per-row H2D copies, replaced by one bulk H2D. Part (2) only shows on CUDA, so the benchmark runs on both ``cuda`` and ``cpu``. -Run: python -m scripts.benchmark.planners.benchmark_curobo_extraction +Run: python -m scripts.benchmark.curobo_extraction.run_benchmark """ from __future__ import annotations diff --git a/scripts/benchmark/planners/neural_planner/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md similarity index 99% rename from scripts/benchmark/planners/neural_planner/BENCHMARK_DESIGN.md rename to scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md index 6646c9b86..2fb6a863e 100644 --- a/scripts/benchmark/planners/neural_planner/BENCHMARK_DESIGN.md +++ b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md @@ -68,7 +68,7 @@ reported as `unsupported`, not silently converted into success or failure. ### 2.2 Gaps in the current NeuralPlanner benchmark -`scripts/benchmark/planners/neural_planner/run_benchmark.py` already handles: +`scripts/benchmark/motion_generation/run_benchmark.py` already handles: - warmup trials separately from measured trials; - CUDA synchronization; @@ -153,7 +153,7 @@ Suite YAML + fixed Case Manifest Keep the existing CLI entry point: ```bash -embodichain benchmark planners-neural-planner +embodichain benchmark motion-generation ``` Reuse established patterns from the current benchmark system: @@ -170,7 +170,7 @@ Reuse established patterns from the current benchmark system: Refactor the current monolithic script incrementally into: ```text -scripts/benchmark/planners/neural_planner/ +scripts/benchmark/motion_generation/ ├── run_benchmark.py # thin CLI and compatibility entry point ├── config.py # suite, planner, and scenario configuration ├── registry.py # planner/scenario/metric registries diff --git a/scripts/benchmark/motion_generation/__init__.py b/scripts/benchmark/motion_generation/__init__.py new file mode 100644 index 000000000..7e2d362fd --- /dev/null +++ b/scripts/benchmark/motion_generation/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Extensible motion-generation benchmark suite.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/scripts/benchmark/planners/neural_planner/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/aggregation.py rename to scripts/benchmark/motion_generation/aggregation.py diff --git a/scripts/benchmark/planners/neural_planner/artifacts.py b/scripts/benchmark/motion_generation/artifacts.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/artifacts.py rename to scripts/benchmark/motion_generation/artifacts.py diff --git a/scripts/benchmark/planners/neural_planner/compat.py b/scripts/benchmark/motion_generation/compat.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/compat.py rename to scripts/benchmark/motion_generation/compat.py diff --git a/scripts/benchmark/planners/neural_planner/config.py b/scripts/benchmark/motion_generation/config.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/config.py rename to scripts/benchmark/motion_generation/config.py diff --git a/scripts/benchmark/planners/neural_planner/metrics/__init__.py b/scripts/benchmark/motion_generation/metrics/__init__.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/metrics/__init__.py rename to scripts/benchmark/motion_generation/metrics/__init__.py diff --git a/scripts/benchmark/planners/neural_planner/metrics/performance.py b/scripts/benchmark/motion_generation/metrics/performance.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/metrics/performance.py rename to scripts/benchmark/motion_generation/metrics/performance.py diff --git a/scripts/benchmark/planners/neural_planner/metrics/trajectory.py b/scripts/benchmark/motion_generation/metrics/trajectory.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/metrics/trajectory.py rename to scripts/benchmark/motion_generation/metrics/trajectory.py diff --git a/scripts/benchmark/planners/neural_planner/models.py b/scripts/benchmark/motion_generation/models.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/models.py rename to scripts/benchmark/motion_generation/models.py diff --git a/scripts/benchmark/planners/neural_planner/planners/__init__.py b/scripts/benchmark/motion_generation/planners/__init__.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/planners/__init__.py rename to scripts/benchmark/motion_generation/planners/__init__.py diff --git a/scripts/benchmark/planners/neural_planner/planners/base.py b/scripts/benchmark/motion_generation/planners/base.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/planners/base.py rename to scripts/benchmark/motion_generation/planners/base.py diff --git a/scripts/benchmark/planners/neural_planner/planners/curobo.py b/scripts/benchmark/motion_generation/planners/curobo.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/planners/curobo.py rename to scripts/benchmark/motion_generation/planners/curobo.py diff --git a/scripts/benchmark/planners/neural_planner/planners/ik_interpolate.py b/scripts/benchmark/motion_generation/planners/ik_interpolate.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/planners/ik_interpolate.py rename to scripts/benchmark/motion_generation/planners/ik_interpolate.py diff --git a/scripts/benchmark/planners/neural_planner/planners/neural.py b/scripts/benchmark/motion_generation/planners/neural.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/planners/neural.py rename to scripts/benchmark/motion_generation/planners/neural.py diff --git a/scripts/benchmark/planners/neural_planner/planners/toppra.py b/scripts/benchmark/motion_generation/planners/toppra.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/planners/toppra.py rename to scripts/benchmark/motion_generation/planners/toppra.py diff --git a/scripts/benchmark/planners/neural_planner/registry.py b/scripts/benchmark/motion_generation/registry.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/registry.py rename to scripts/benchmark/motion_generation/registry.py diff --git a/scripts/benchmark/planners/neural_planner/reporting.py b/scripts/benchmark/motion_generation/reporting.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/reporting.py rename to scripts/benchmark/motion_generation/reporting.py diff --git a/scripts/benchmark/planners/neural_planner/run_benchmark.py b/scripts/benchmark/motion_generation/run_benchmark.py similarity index 99% rename from scripts/benchmark/planners/neural_planner/run_benchmark.py rename to scripts/benchmark/motion_generation/run_benchmark.py index 544954e3d..bf23616a8 100644 --- a/scripts/benchmark/planners/neural_planner/run_benchmark.py +++ b/scripts/benchmark/motion_generation/run_benchmark.py @@ -20,7 +20,7 @@ optional diagnostic baselines. NMG remains an explicitly configurable, unsupported adapter stub until its production checkpoint contract is ready. -Run: ``embodichain benchmark planners-neural-planner --suite smoke`` +Run: ``embodichain benchmark motion-generation --suite smoke`` """ from __future__ import annotations diff --git a/scripts/benchmark/planners/neural_planner/runner.py b/scripts/benchmark/motion_generation/runner.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/runner.py rename to scripts/benchmark/motion_generation/runner.py diff --git a/scripts/benchmark/planners/neural_planner/scenarios/__init__.py b/scripts/benchmark/motion_generation/scenarios/__init__.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/scenarios/__init__.py rename to scripts/benchmark/motion_generation/scenarios/__init__.py diff --git a/scripts/benchmark/planners/neural_planner/scenarios/base.py b/scripts/benchmark/motion_generation/scenarios/base.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/scenarios/base.py rename to scripts/benchmark/motion_generation/scenarios/base.py diff --git a/scripts/benchmark/planners/neural_planner/scenarios/free_space.py b/scripts/benchmark/motion_generation/scenarios/free_space.py similarity index 100% rename from scripts/benchmark/planners/neural_planner/scenarios/free_space.py rename to scripts/benchmark/motion_generation/scenarios/free_space.py diff --git a/scripts/benchmark/planners/neural_planner/suites/coverage.yaml b/scripts/benchmark/motion_generation/suites/coverage.yaml similarity index 100% rename from scripts/benchmark/planners/neural_planner/suites/coverage.yaml rename to scripts/benchmark/motion_generation/suites/coverage.yaml diff --git a/scripts/benchmark/planners/neural_planner/suites/smoke.yaml b/scripts/benchmark/motion_generation/suites/smoke.yaml similarity index 100% rename from scripts/benchmark/planners/neural_planner/suites/smoke.yaml rename to scripts/benchmark/motion_generation/suites/smoke.yaml diff --git a/tests/benchmark/motion_generation/__init__.py b/tests/benchmark/motion_generation/__init__.py new file mode 100644 index 000000000..2a4a608be --- /dev/null +++ b/tests/benchmark/motion_generation/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for the motion-generation benchmark suite.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/tests/benchmark/planners/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py similarity index 96% rename from tests/benchmark/planners/test_motion_generation_benchmark.py rename to tests/benchmark/motion_generation/test_motion_generation_benchmark.py index 64ea24ef3..dec891d5f 100644 --- a/tests/benchmark/planners/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -25,16 +25,16 @@ from embodichain.lab.sim.planners.curobo.curobo_planner import CuroboPlanner from embodichain.lab.sim.planners.utils import MoveType, PlanResult -from scripts.benchmark.planners.neural_planner.aggregation import aggregate_results -from scripts.benchmark.planners.neural_planner.config import load_suite -from scripts.benchmark.planners.neural_planner.metrics.trajectory import ( +from scripts.benchmark.motion_generation.aggregation import aggregate_results +from scripts.benchmark.motion_generation.config import load_suite +from scripts.benchmark.motion_generation.metrics.trajectory import ( compute_case_outcomes, match_ordered_waypoints, ) -from scripts.benchmark.planners.neural_planner import ( +from scripts.benchmark.motion_generation import ( scenarios as _scenarios, ) # noqa: F401 -from scripts.benchmark.planners.neural_planner.models import ( +from scripts.benchmark.motion_generation.models import ( AlgorithmRole, BenchmarkCase, CaseOutcome, @@ -42,14 +42,14 @@ TrialPhase, TrialRecord, ) -from scripts.benchmark.planners.neural_planner.registry import ( +from scripts.benchmark.motion_generation.registry import ( create_scenario_provider, scenario_provider_names, ) -from scripts.benchmark.planners.neural_planner.reporting import ( +from scripts.benchmark.motion_generation.reporting import ( write_markdown_report, ) -from scripts.benchmark.planners.neural_planner.run_benchmark import ( +from scripts.benchmark.motion_generation.run_benchmark import ( _apply_overrides, ) diff --git a/tests/benchmark/planners/test_neural_planner_benchmark.py b/tests/benchmark/planners/test_neural_planner_benchmark.py index 2abdeb19f..70f74b08c 100644 --- a/tests/benchmark/planners/test_neural_planner_benchmark.py +++ b/tests/benchmark/planners/test_neural_planner_benchmark.py @@ -21,7 +21,7 @@ import pytest import torch -from scripts.benchmark.planners.neural_planner.run_benchmark import ( +from scripts.benchmark.motion_generation.run_benchmark import ( IMPL_IK, IMPL_NEURAL, IMPL_TOPPRA, From 9a14b2caeb884a93b3bcc354da04862f933826ce Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 02:49:44 +0800 Subject: [PATCH 06/17] Remove design docs --- .../motion_generation/BENCHMARK_DESIGN.md | 995 ------------------ scripts/benchmark/motion_generation/README.md | 33 + 2 files changed, 33 insertions(+), 995 deletions(-) delete mode 100644 scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md create mode 100644 scripts/benchmark/motion_generation/README.md diff --git a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md deleted file mode 100644 index 2fb6a863e..000000000 --- a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md +++ /dev/null @@ -1,995 +0,0 @@ -# Neural Motion Generator Benchmark Design - -## Proposal - -Build an extensible motion-generation benchmark for EmbodiChain that treats -Neural Motion Generator (NMG) checkpoints as candidates and cuRobo as the -primary baseline. The benchmark should evaluate three distinct questions: - -1. How fast and resource-efficient is trajectory generation? -2. Is the generated trajectory accurate, safe, smooth, and executable? -3. Does the trajectory complete an Atomic Action or a multi-action task under - physics simulation? - -The default comparison should be NMG versus cuRobo. IK plus interpolation and -TOPPRA should remain optional diagnostic baselines rather than define the main -leaderboard. - -## Motivation - -The existing NeuralPlanner benchmark provides useful latency, memory, rollout, -and endpoint-error measurements, but it only exercises fixed Franka waypoint -sets in planner-only mode. It does not measure collision safety, dynamic -feasibility, execution tracking, or physical task completion. It also does not -currently include cuRobo even though both `BasePlanner` and `MotionGenerator` -support the cuRobo backend. - -NMG will continue to evolve toward obstacle conditioning, multimodal motion, -physics-aware refinement, closed-loop recovery, and cross-embodiment -adaptation. A versioned suite, fixed case manifests, capability-aware tracks, -and stage-specific outcomes are needed so future results remain comparable and -failures remain diagnosable. - -## 1. Objectives and scope - -The benchmark should support: - -- regression testing across NMG checkpoint and model revisions; -- paired comparison between NMG and cuRobo for success, trajectory quality, - collision safety, latency, throughput, and memory; -- optional IK plus interpolation and TOPPRA diagnostics; -- incremental tracks for obstacle tokens, multimodal generation, Analytic - Policy Gradient (APG) refinement, closed-loop recovery, and new robots; -- actionable failure attribution for training-data and model iteration. - -The benchmark must not collapse every measurement into one opaque composite -score. In particular, `PlanResult.success` must not be treated as equivalent to -physical task success. - -## 2. Current implementation and design implications - -### 2.1 Current NeuralPlanner capability boundary - -`embodichain/lab/sim/planners/neural_planner.py` currently: - -- directly supports only `MoveType.EEF_MOVE`; -- uses a 7-DoF waypoint Transformer checkpoint, currently centered on Franka; -- derives the maximum number of waypoints from checkpoint `waypoint_max`; -- 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; -- has no explicit obstacle, collision, multimodal-sampling, or APG-refinement - input/output interface yet. - -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. - -### 2.2 Gaps in the current NeuralPlanner benchmark - -`scripts/benchmark/motion_generation/run_benchmark.py` already handles: - -- warmup trials separately from measured trials; -- CUDA synchronization; -- CPU RSS, GPU allocation delta, and peak GPU memory; -- planning latency, final TCP error, and waypoint best-hit error; -- optional IK-interpolation and TOPPRA baselines. - -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; -- path length, path efficiency, and smoothness; -- physics execution and controller tracking; -- Atomic Action task completion; -- batch-scaling measurements; -- resolved checkpoint, case-manifest, seed, software, and hardware metadata. - -The planner and `MotionGenerator` already register `CuroboPlanner`, but the -current NMG benchmark does not include it. The new default matrix should be -`NMG vs cuRobo`; IK interpolation and TOPPRA should not be the primary -reference. - -The current `compute_waypoint_errors()` independently searches the whole -trajectory for the best sample for each waypoint. This can reward out-of-order -motion and can select different samples for the best position and orientation. -The new benchmark must use ordered waypoint matching. - -The current report also creates multiple Quality/Performance tables grouped by -waypoint count plus two leaderboards. EmbodiChain benchmark convention requires -one Markdown report with exactly three tables: - -1. `Time & Memory` -2. `Success & Other Metrics` -3. `Leaderboard` - -### 2.3 Atomic Action integration - -`ActionCfg.motion_source` defaults to `"ik_interp"`. Existing Atomic Action -benchmarks construct a TOPPRA `MotionGenerator`, but cases that do not -explicitly change `motion_source` still use local IK plus interpolation. - -NMG and cuRobo Atomic Action evaluation must explicitly set: - -```python -cfg.motion_source = "motion_gen" -``` - -The NMG checkpoint must remain confined to `NeuralPlannerCfg`. Atomic Action -scenarios, grasp sampling, objects, controllers, and task-success rules must -not contain NMG-specific branches. The planner factory should be the only -backend-specific injection point. - -## 3. Architecture - -```text -Suite YAML + fixed Case Manifest - | - v - Scenario Providers - / | \ - planner-only trajectory atomic-task - | | | - +------ Planner Factory/Adapter ------+ - | - v - Raw Trial Records - | - +-----------+-----------+ - | | - Metric Evaluators Failure Classifier - | | - +-----------+-----------+ - | - v - Aggregates + Leaderboard - | - v - one Markdown report with exactly 3 tables -``` - -Keep the existing CLI entry point: - -```bash -embodichain benchmark motion-generation -``` - -Reuse established patterns from the current benchmark system: - -- dispatch through `scripts/benchmark/__main__.py`; -- Atomic Action `smoke/coverage/full` profiles, case sweeps, physics replay, - and physical-success rules; -- RL benchmark suite YAML, config/runner/reporting separation, resolved - protocol, per-run artifacts, and compatibility-aware resume; -- the current NeuralPlanner warmup, CUDA synchronization, memory measurement, - and optional-baseline flow; -- one Markdown report, exactly three tables, and a complete leaderboard. - -Refactor the current monolithic script incrementally into: - -```text -scripts/benchmark/motion_generation/ -├── run_benchmark.py # thin CLI and compatibility entry point -├── config.py # suite, planner, and scenario configuration -├── registry.py # planner/scenario/metric registries -├── runner.py # case matrix, warmup, trials, and resume -├── artifacts.py # manifests, JSONL, and environment metadata -├── aggregation.py # grouping, confidence intervals, leaderboard -├── reporting.py # exactly three Markdown tables -├── planners/ -│ ├── base.py -│ ├── neural.py -│ ├── curobo.py -│ ├── ik_interpolate.py -│ └── toppra.py -├── scenarios/ -│ ├── reach.py -│ ├── waypoint_path.py -│ ├── obstacle.py -│ ├── perturbation.py -│ └── atomic_action.py -├── metrics/ -│ ├── performance.py -│ ├── kinematic.py -│ ├── dynamic.py -│ ├── collision.py -│ ├── execution.py -│ └── task.py -└── suites/ - ├── smoke.yaml - ├── coverage.yaml - └── full.yaml -``` - -### 3.1 Extension interfaces - -Runner logic should not branch on planner names. Use protocols such as: - -```python -class PlannerAdapter(Protocol): - @property - def metadata(self) -> PlannerMetadata: ... - - def build(self, context: BenchmarkContext) -> MotionGenerator: ... - - def prepare(self, case: BenchmarkCase) -> PreparationMetrics: ... - - def warmup(self, case: BenchmarkCase) -> None: ... - - def plan(self, case: BenchmarkCase) -> PlanResult: ... - - -class ScenarioProvider(Protocol): - @property - def required_capabilities(self) -> frozenset[str]: ... - - def generate_cases( - self, - manifest: SuiteManifest, - seed: int, - ) -> Iterable[BenchmarkCase]: ... - - -class MetricEvaluator(Protocol): - @property - def required_artifacts(self) -> frozenset[str]: ... - - def evaluate(self, trial: TrialArtifacts) -> dict[str, float | bool]: ... -``` - -`PlannerMetadata` should include at least: - -- `algorithm_id`, for example `nmg_transformer`, `curobo`, - `ik_interpolate`, or `toppra`; -- `algorithm_role`: `candidate`, `primary_baseline`, or - `diagnostic_baseline`; -- model revision, checkpoint path, and SHA256 when applicable; -- capabilities such as `eef_waypoint`, `joint_waypoint`, `obstacle`, - `sampling`, `refinement`, and `closed_loop`; -- supported robots, maximum waypoint count, and input/output schema version; -- planner parameters, model parameter count, and inference dtype. - -Adding an NMG architecture, refiner, or baseline should require only a new -adapter/registry entry and suite configuration, not runner, aggregation, or -reporting changes. - -### 3.2 Trial data model - -Each trial should have a stable key: - -```text -(suite_version, track, scenario_id, case_id, algorithm_id, - model_revision, seed, repeat, batch_size) -``` - -`TrialRecord` should separate: - -- **identity**: the key above, robot, device, git commit, and config/checkpoint - hashes; -- **case**: start qpos, target waypoints, obstacle/object state, and - perturbations; -- **outcomes**: planning, motion, execution, and task success plus failure - stage; -- **metrics**: performance, memory, trajectory, and task values. - -Raw JSONL/JSON artifacts should retain numeric types. Percentage formatting -belongs only in Markdown rendering, not before aggregation. - -### 3.3 Primary NMG-versus-cuRobo protocol - -The default suite should require only: - -- `nmg:` as the candidate; -- `curobo:` as the primary baseline. - -IK interpolation and TOPPRA should be enabled only through -`--extra-baselines` or suite configuration. If enabled, they must still appear -in reports and the leaderboard with role `diagnostic_baseline`. - -Use three paired tracks: - -1. **Free-space common input**: cuRobo uses an empty collision world. Both - planners receive identical start qpos and ordered EEF waypoints. This is the - primary quality and performance leaderboard. -2. **Collision-aware deployment**: both planners execute in the same - simulation scene; cuRobo receives the correct collision world while current - NMG does not receive obstacle tokens. This track measures deployment - behavior and the current capability gap, not model quality under equal - information. -3. **Atomic task**: both planners run through the same `AtomicActionEngine`, - objects, grasps, controller, and physical success criteria. - -cuRobo supports both `EEF_MOVE` and `JOINT_MOVE`, while current NMG supports -only `EEF_MOVE`. The primary leaderboard must use their common `EEF_MOVE` -capability. Joint-space cases belong in cuRobo-only or diagnostic tracks. - -#### Freeze the cuRobo configuration - -Every run must record and hash: - -- `max_attempts` and `max_planning_time`; -- `interpolation_dt` and `collision_activation_distance`; -- `use_cuda_graph`, actual fallback state, and `warmup_iterations`; -- robot sphere-fit settings and collision-sphere buffer; -- obstacle representation, collision cache, and `multi_env`; -- static/dynamic obstacle names and world-content hash; -- `preserve_plan_samples`. - -World representation and sphere fitting are part of the baseline definition -and must not change silently between checkpoint comparisons. - -The primary leaderboard should use a frozen operational configuration, for -example checkpoint-default NMG `max_steps` and fixed cuRobo `max_attempts`. -Also add a latency-budget sweep: - -- sweep NMG `max_steps`; -- sweep cuRobo `max_attempts`, with a common `planning_budget_ms`; -- retain success-latency Pareto data. - -`CuroboPlannerCfg.max_planning_time` currently validates the budget after the -plan; it is not a preemptive real-time deadline. The outer benchmark must -record actual wall latency and `budget_compliance_rate` whether or not a -planner supports interruption. - -#### Lifecycle and timing - -cuRobo lazily creates and caches a backend for each -`(control_part, batch_size, multi_env, move_type)`. First use may include -robot/world YAML generation, sphere fitting, collision-cache allocation, CUDA -graph capture, and warmup. NMG has checkpoint loading, actor construction, and -device transfer. - -Report the following separately for both: - -```text -planner_construct_ms -backend_prepare_ms -cold_plan_ms -warm_plan_ms -``` - -The planning-latency leaderboard must use only `warm_plan_ms`. -`backend_prepare_ms` represents one-time deployment cost; `cold_plan_ms` -represents the first real case. Every batch size and goal type needs its own -prepare/warmup phase. - -cuRobo always plans on CUDA. The primary comparison should therefore use the -same CUDA device and fp32 interface. NMG CPU results may be reported as a -separate characterization, not ranked against cuRobo CUDA latency. - -#### Multi-waypoint and sample policy - -cuRobo plans multiple waypoint segments sequentially. NMG consumes the full -waypoint sequence in one model invocation. The primary performance metric is -the total cost of one `MotionGenerator.generate()` call for the same high-level -input. Also report `num_segments` and `cost_time_per_segment_ms`, but do not -replace total-latency ranking with per-segment latency. - -- Planner-native quality: set cuRobo `preserve_plan_samples=True` to retain - native collision-checked samples and `dt`. -- Common path metrics: resample derived copies from both planners by the same - arc-length procedure. -- Atomic Action/common execution: use the same action `sample_interval` and let - `TrajectoryBuilder` perform common resampling, but do not call the resampled - result a native-timing trajectory. - -#### Collision worlds - -- Use `CuroboWorldCfg.multi_env=False` when every batch row has the same - robot-relative obstacle layout. -- Use `multi_env=True` when obstacle poses differ relative to each robot. -- Supply per-environment dynamic poses through `dynamic_obstacle_names` and - `CuroboPlanOptions.dynamic_obstacle_poses`. -- Dynamic obstacles must use `cuboid` or `mesh`, not the `sphere` - representation that cannot be updated by the original object name. -- Revalidate collision success with an independent simulator/common - validator. cuRobo `success=True` is not benchmark ground truth. - -## 4. Layered evaluation - -### 4.1 L0: generation performance - -L0 isolates planner computation and does not execute the trajectory. - -Sweep: - -- batch size: `1, 8, 64`, with larger batches in the full profile; -- waypoint count: `1, 3, min(5, model_max)`, plus supported maxima; -- start state: nominal, random reachable, near joint limit, near singularity; -- path shape: direct, L-turn, S-curve, orientation-only, and combined - translation/orientation; -- target-distance and orientation-delta bins; -- primary device/dtype: same CUDA device and fp32 interface for NMG and cuRobo; -- separate NMG CPU/fp16/bf16 characterization; -- cold start and warm steady state. - -Timing boundaries: - -- measure `planner_construct_ms` and `backend_prepare_ms` separately; -- measure `cold_plan_ms` for the first real input; -- measure `warm_plan_ms` after fixed warmup; -- exclude setup, case generation, reporting, FK metrics, and validation; -- call `torch.cuda.synchronize()` before and after CUDA timing. - -Primary metrics: - -- latency p50/p95/p99; -- `latency_per_env_ms`; -- `cost_time_per_segment_ms` for explaining multi-waypoint scaling; -- trajectories per second; -- rollout steps and policy steps per second; -- CPU RSS delta, GPU allocation delta, and peak GPU memory; -- real-time factor only when trajectory duration has clear semantics. - -For Atomic Action tracks, separate `action_planning_ms`, -`physics_execution_ms`, and `task_end_to_end_ms`. - -### 4.2 L1: trajectory quality and executability - -Distinguish three evaluation views: - -1. **path-only**: resample by arc length and compare geometry; -2. **native-timing**: use each planner's own `dt/duration`; -3. **common-execution**: use the same controller, control dt, and simulator. - -Do not directly compare NMG's fixed nominal `dt=0.01` against IK interpolation -with no meaningful duration. Report unavailable native timing as `N/A`. -Dynamic fairness should come from common execution or common -time-parameterization. - -Use `preserve_plan_samples=True` for cuRobo native-timing evaluation and the -original NMG `PlanResult`. Recompute endpoint, constraint, collision, and -smoothness metrics from output trajectories rather than trusting either -planner's internal success flag. - -#### Goal and waypoint metrics - -- final translation error in mm; -- final rotation geodesic error in degrees; -- ordered waypoint success rate; -- waypoint translation/rotation mean, p95, and maximum error; -- completed waypoint ratio; -- time or step to final target. - -Define ordered arrival as: - -```text -t_i = the first sample satisfying t_i > t_(i-1), and - position_error(t_i) <= pos_threshold, and - rotation_error(t_i) <= rot_threshold -``` - -The waypoint sequence succeeds only if every valid waypoint has a matching -`t_i`. Continuous error statistics may use monotonic dynamic programming to -jointly match waypoints and trajectory samples. Position and orientation must -not select unrelated best samples. - -#### Kinematic and dynamic metrics - -- finite-value rate; -- joint-position-limit violation rate and maximum normalized violation; -- joint velocity, acceleration, and jerk violation rates; -- maximum/mean joint velocity, acceleration, and jerk; -- joint path length; -- Cartesian translation and rotation path length; -- path efficiency relative to a geometric lower bound or same-case reference; -- path curvature and path-only smoothness; -- time-indexed integrated squared acceleration and jerk; -- endpoint settling error and hold stability. - -Define `motion_valid` independently: - -```text -motion_valid = - finite - and ordered_waypoints_reached - and joint_limits_satisfied - and dynamic_limits_satisfied_when_applicable - and collision_free_when_applicable -``` - -#### Collision and physics-execution metrics - -- environment collision rate; -- self-collision rate; -- minimum clearance; -- undesired-contact count and maximum contact impulse; -- controller joint-tracking RMSE and maximum error; -- executed TCP tracking RMSE; -- execution timeout rate; -- final pose error after simulation execution; -- final pose drift after a fixed stable-hold period. - -Enable collision metrics only when the scenario supplies a trustworthy -collision world. In `free-space-common`, cuRobo receives an empty world. In -`collision-deployment`, cuRobo receives the full world while current NMG is an -`obstacle_unaware` candidate. The report must expose this information -asymmetry. - -#### Reference-based metrics - -ADE/FDE, expert joint distance, and cost ratio are diagnostic, not primary -success criteria. A single reference path can unfairly penalize valid alternate -IK branches or left/right obstacle-avoidance modes. - -cuRobo may serve as a strong reference for path cost, duration, and clearance, -but it is not the only ground truth. NMG should pass whenever it satisfies the -same external constraints and task criteria, even with a different valid path. - -Future generative NMG tracks should add: - -- top-k feasibility/success; -- best-of-k cost; -- valid mode count and trajectory diversity; -- total sampling cost per successful sample. - -### 4.3 L2: Atomic Actions and task completion - -L2 uses `AtomicActionEngine` to generate a trajectory and then executes or -replays it in physics simulation. Object, contact, and robot state determine -task success. - -Backend fairness: - -- NMG and primary baseline cuRobo use `motion_source="motion_gen"`; -- optional TOPPRA diagnostics use `motion_source="motion_gen"`; -- optional IK-interpolation diagnostics use `motion_source="ik_interp"`; -- grasp generator, object preset, start state, target, controller, sample - interval, seed, and physics parameters are identical; -- do not execute a fabricated trajectory after planning failure; -- restore robot, object, and simulator state before every case. - -Contact tasks need explicit collision-world ownership: - -- the manipulated Pick/Place target must not be treated as a generic - non-contact obstacle during required contact phases; -- tables, environmental obstacles, and non-target objects should enter the - cuRobo world; -- the current EmbodiChain cuRobo adapter does not expose dynamic held-object - attachment, so `MoveHeldObject` must record - `held_object_geometry_in_planner=false` and validate object collisions in - simulation; -- write visible constraints into `constraint_information` for every result. - -Suggested coverage: - -| Action or sequence | Primary task-success criteria | -|---|---| -| MoveEndEffector | Planning succeeds, executed TCP reaches and holds target, no disallowed collision | -| PickUp | Approach/lift plan succeeds, `held_object` is created, minimum object lift is reached, no drop | -| MoveHeldObject | Object reaches target pose, grasp remains stable, object drift/tilt stays within threshold | -| Place | Place pose reached, release succeeds, final object pose is correct and stable | -| Press | Press depth and valid contact/force reached, retract succeeds, no abnormal object motion | -| Pick-Move-Place | Every stage succeeds in sequence; final object pose and release state are correct | - -Record: - -- `planning_success`; -- `motion_valid`; -- `execution_success`; -- `task_success`; -- per-Atomic-Action stage success; -- task completion time; -- replan/retry count; -- task-specific pose, lift, slip, release, and contact metrics. - -Sequence success must come from one sequential episode. Do not approximate it -by multiplying independently measured action success rates. - -## 5. Scenario tracks and capability gates - -| Track | Current NMG | cuRobo | Purpose | -|---|---:|---:|---| -| `free-space-common` | Supported | Supported | Empty-world, identical EEF-waypoint primary comparison | -| `workspace-generalization` | Supported | Supported | Workspace, orientation, joint-limit, and singularity bins | -| `collision-deployment` | Executable without obstacle input | Supported | Deployment success and current capability gap | -| `atomic-task` | Partially supported | Supported | Atomic Action and action-chain physical completion | -| `obstacle-aware-common-input` | Not yet supported | Supported | Future equal-information scene-constraint comparison | -| `multimodal` | Not yet supported | Single-output reference | Top-k coverage, diversity, and sampling cost | -| `physics-refinement` | Not yet supported | Reference | NMG initialization plus APG/trajectory optimization | -| `closed-loop-recovery` | Not yet supported | Not yet supported | Observation, target, and disturbance recovery | -| `cross-embodiment` | Not yet supported | Multi-robot configuration | Robot, DoF, control-rate, and dynamics adaptation | - -Before case generation, check `required_capabilities`: - -- `supported`: run normally; -- `unsupported`: record the reason and exclude it from that track's - denominator; -- `error`: the adapter declared support but failed, so count it as failure. - -Always report `coverage_rate` to prevent selective execution from improving -rank. Formal track eligibility requires 100% coverage of mandatory cases. -Retain ineligible algorithms in the leaderboard with `eligible=false`. - -## 6. Suites and scenario matrix - -### 6.1 Profiles - -- `smoke`: one robot, a few deterministic cases, `B=1`, one seed; for PRs. -- `coverage`: workspace/path bins, `B=1/8/64`, at least five seeds, and - representative Atomic Action object/position cases; for nightly runs. -- `full`: dense workspace/OOD cases, boundary states, all objects/positions, - obstacles, perturbations, and at least 20 seeds; for model releases. - -### 6.2 Fixed case manifests - -Generate randomized scenarios into a fixed manifest before giving them to any -planner. Save: - -- robot and start qpos; -- ordered target waypoints; -- object/obstacle poses and physical properties; -- target distance, orientation delta, and workspace bin; -- perturbation schedule; -- validity evidence, such as independent reachability validation; -- case schema version. - -Unreachable targets may form a separate robustness track but must not enter the -normal success denominator. Case inclusion must not depend on the success of -the cuRobo run being evaluated, which would create baseline selection bias. -Use generation rules, an independent validator, or a frozen offline oracle. - -### 6.3 Example suite - -```yaml -schema_version: 1 -suite: nmg_coverage_v1 -profile: coverage -seeds: [11, 23, 37, 53, 71] - -planners: - - id: nmg_transformer - adapter: neural - role: candidate - checkpoint: ${NMG_CHECKPOINT} - - id: curobo - adapter: curobo - role: primary_baseline - config: - max_attempts: 5 - interpolation_dt: 0.025 - use_cuda_graph: true - warmup_iterations: 1 - preserve_plan_samples: true - world: - obstacle_representation: mesh - multi_env: false - -tracks: - free-space-common: - required_capabilities: [eef_waypoint] - scenarios: [nominal_reach, random_reach, waypoint_path, boundary_reach] - batch_sizes: [1, 8, 64] - waypoint_counts: [1, 3, 5] - world: empty - collision-deployment: - required_capabilities: [eef_waypoint] - scenarios: [static_obstacle, randomized_obstacle] - comparison_mode: asymmetric_information - atomic-task: - required_capabilities: [eef_waypoint] - scenarios: - - move_end_effector - - pick_up - - move_held_object - - place - - press - - pick_move_place - -scenario_overrides: - randomized_obstacle: - planners: - curobo: - config: - world: - multi_env: true - -track_overrides: - atomic-task: - planners: - curobo: - config: - preserve_plan_samples: false - -protocol: - warmup_trials: 3 - measured_trials: 20 - confidence_level: 0.95 - common_control_dt: 0.01 -``` - -Environment variables are only configuration inputs. Save resolved values, -NMG checkpoint hash, cuRobo config hash, and world-content hash in run -metadata. - -`free-space-common`, `collision-deployment`, and `atomic-task` should create -isolated planner instances. Rebuild cuRobo planner/backends when static world -geometry or representation changes. Reuse a backend only for fixed geometry -whose registered dynamic-obstacle poses are updated. - -## 7. Success semantics and failure taxonomy - -Each case defines `primary_success`: - -- L0 planner-only: `planning_success`; -- L1 trajectory: `motion_valid`; -- L2 execution: `execution_success`; -- L2 task: `task_success`. - -Planner-internal `pos_eps/rot_eps` only determine when that planner stops. -Cross-algorithm `ordered_waypoints_reached`, `motion_valid`, and -`primary_success` must use suite-owned, versioned external thresholds that are -identical for every algorithm. - -Retain every stage: - -```text -input_valid - -> planning_success - -> ordered_waypoints_reached - -> motion_valid - -> execution_success - -> task_success -``` - -Use a stable failure taxonomy: - -- `invalid_case` -- `unsupported_capability` -- `checkpoint_load_failure` -- `planner_exception` -- `planner_reported_failure` -- `timeout` -- `non_finite_trajectory` -- `waypoint_miss` -- `joint_limit_violation` -- `dynamic_limit_violation` -- `self_collision` -- `environment_collision` -- `controller_tracking_failure` -- `object_not_grasped` -- `object_dropped` -- `release_failure` -- `task_goal_miss` - -Store failure stage and reason in raw artifacts. Notes may summarize common -failures, but must not add a fourth Markdown table. - -## 8. Fairness and statistical protocol - -### 8.1 Baseline fairness - -- Use the same case, start qpos, waypoint order, and external tolerance. -- Default to NMG candidate versus cuRobo primary baseline. -- Give cuRobo an empty world in `free-space-common`. -- Record the scene/constraint information visible to each planner in - `collision-deployment`. -- Time only planner generation; exclude FK/collision metrics. -- Do not force identical sample counts during native generation. -- Apply common path resampling only to derived copies. -- Use the same controller and control dt for execution metrics. -- Report native time parameterization and common execution separately. -- Retain baseline failures; never remove failed cases from aggregation. -- Report success-conditioned continuous metrics and all-case failure-aware - statistics to avoid survivor bias. -- Prefer isolated subprocesses for NMG and cuRobo. cuRobo CUDA graphs, - backends, and world caches persist and can contaminate memory or cold-start - results based on execution order. - -### 8.2 Reproducibility and confidence intervals - -- Fix Python, NumPy, Torch, and simulator seeds. -- Warm up every `(planner, scenario, batch_size, waypoint_count)` separately. -- Report latency p50/p95/p99. -- Report Wilson or bootstrap 95% confidence intervals for success. -- Report mean, median, p95, and bootstrap 95% confidence intervals for - continuous metrics. -- Record OS, CPU, GPU, driver, CUDA, Torch, DexSim, and EmbodiChain git commit. -- In addition to PyTorch allocator memory, record process-level GPU memory when - available to capture cuRobo/CUDA-graph allocations. -- Save the resolved suite and case manifest. - -### 8.3 Protocol versioning - -- Version suite, case manifest, metric schema, and report schema independently. -- Increment suite version when thresholds, scenario distribution, or primary - success changes. -- Combine leaderboard results only across identical suite/protocol versions. -- Require matching hardware class, device, dtype, and batch protocol for - latency leaderboards. -- Historical checkpoints may be rerun on a new suite, but old results must not - be silently relabeled as the new protocol. - -## 9. Artifacts and report contract - -Suggested output: - -```text -outputs/benchmarks/nmg// -├── resolved_suite.yaml -├── environment.json -├── case_manifest.json -├── trials.jsonl -├── aggregates.json -├── report.md -└── videos/ # optional representative success/failure cases -``` - -Each run produces one `report.md` with exactly three tables. - -### 9.1 Time & Memory - -Recommended columns: - -```text -track, scenario, comparison_mode, algorithm, algorithm_role, model_revision, -planner_config_hash, batch_size, waypoint_count, num_segments, num_trials, -planner_construct_ms, backend_prepare_ms, cost_time_ms, cold_plan_ms, -warm_plan_ms_p50, warm_plan_ms_p95, trajectories_per_second, -planning_budget_ms, budget_compliance_rate, -cpu_delta_mb, gpu_delta_mb, peak_gpu_mb -``` - -`cost_time_ms` is the mean primary steady-state timed operation for the row. -The scenario protocol or a `timing_scope` field must define that operation. - -### 9.2 Success & Other Metrics - -Aggregate by `(track, scenario, algorithm, condition_bin)`: - -```text -track, scenario, comparison_mode, algorithm, algorithm_role, -constraint_information, cases, coverage_rate, success_rate, -planning_success_rate, motion_valid_rate, execution_success_rate, -task_success_rate, final_pos_err_mm, final_rot_err_deg, -waypoint_completion_rate, joint_violation_rate, dynamic_violation_rate, -collision_rate, min_clearance_m, path_efficiency, jerk_cost, -task_metric, top_failure -``` - -Use `N/A`, not zero, for inapplicable values. - -### 9.3 Leaderboard - -Use one table with a `track` column: - -```text -track, rank, algorithm, algorithm_role, model_revision, planner_config_hash, -eligible, coverage_rate, overall_success_rate, motion_valid_rate, -task_success_rate, latency_p95_ms, peak_gpu_mb -``` - -`overall_success_rate` is the macro average of `primary_success` over all -mandatory cases in the track. Sort within each track by: - -1. `eligible=True`; -2. `overall_success_rate` descending; -3. `coverage_rate` descending; -4. `latency_p95_ms` ascending. - -Include every evaluated algorithm in the current scope, not only the top -entries. Trajectory imitation error must not override task success in ranking. - -## 10. NMG-specific diagnostics - -These should not block v1, but the schema should reserve them. - -### 10.1 Solution leakage - -- Use Cartesian-only conditioning for the primary leaderboard. -- Treat Cartesian plus joint target as an ablation only. -- Test the same Cartesian target from different start qpos and legal IK - branches. -- Report sensitivity to joint-interpolation shortcuts. -- Record actual checkpoint input fields in metadata. - -### 10.2 Physics refinement and APG - -Create paired cases: - -- `nmg_raw` -- `nmg_plus_refinement` - -Report: - -- hard-feasibility gain; -- task-success gain; -- trajectory-cost reduction; -- added latency and iterations; -- refinement-divergence rate. - -### 10.3 Closed-loop recovery - -When observation feedback is available, add: - -- state-observation noise; -- target-pose motion during execution; -- external force or joint-tracking disturbances; -- object slip; -- obstacle motion. - -Report recovery success, time to recover, replan count, maximum post-disturbance -error, and final task success. The current FK rollout must not be reported as -closed-loop recovery. - -## 11. Implementation phases - -### Phase 0: protocol and cuRobo baseline - -- Split config, aggregation, and reporting from the current script. -- Preserve the current CLI and checkpoint download behavior. -- Add a cuRobo adapter and make it the default primary baseline. -- Default the CLI/suite to `nmg curobo`; require explicit diagnostic - IK/TOPPRA baselines. -- Separate cuRobo backend prepare, cold plan, and warm plan timing. -- Produce exactly three report tables. -- Save resolved config, environment metadata, and trial JSONL. -- Add unit tests for report schema, full leaderboard coverage, and ordered - waypoint metrics. - -Minimum tests: - -- ordered waypoint matching rejects out-of-order and split - position/orientation hits; -- warmup trials do not enter aggregation; -- `unsupported`, `error`, and ordinary failure remain distinct; -- failed trials are not silently removed from continuous aggregation; -- the leaderboard includes every in-scope algorithm and applies - success/coverage/latency ordering; -- generated reports contain exactly three Markdown tables; -- planner-only smoke test with a fake NMG checkpoint; -- graceful skip when the optional cuRobo runtime is unavailable; -- cuRobo empty-world, shared-world, and multi-env dynamic-world configuration; -- Atomic Action integration with `motion_source="motion_gen"`. - -### Phase 1: core motion - -- Add fixed manifests, randomized workspace/path cases, and batch scaling. -- Implement paired NMG-versus-cuRobo `free-space-common`. -- Add frozen operational configuration and latency-budget/Pareto sweeps. -- Add path-only, native-timing, joint, and dynamic metrics. -- Add common resampling and failure classification. -- Keep cuRobo as the primary baseline; add IK/TOPPRA only through adapters when - requested. - -### Phase 2: physics and Atomic Actions - -- Add cuRobo collision worlds and `collision-deployment`. -- Parameterize planner construction in Atomic Action benchmarks. -- Explicitly separate `ik_interp` and `motion_gen`. -- Reuse current object/position/approach profiles and physical-success rules. -- Add MoveEndEffector, PickUp, MoveHeldObject, Place, Press, and - Pick-Move-Place. -- Add controller tracking, collision/contact, and stable-hold metrics. - -### Phase 3: future NMG capabilities - -- Add equal-information obstacle-aware, multimodal, APG-refinement, and - closed-loop-recovery tracks. -- Add cross-embodiment and unseen-robot tracks. -- Add release profiles and a long-lived checkpoint leaderboard. - -## 12. Acceptance criteria - -- The current Franka NMG checkpoint and cuRobo run the same default `smoke` and - `free-space-common` manifest. -- cuRobo is the default primary baseline; IK/TOPPRA do not affect the default - main leaderboard. -- cuRobo backend preparation and steady-state planning latency are separate. -- Free-space, collision-deployment, and Atomic Action tracks use correct, - traceable cuRobo world configurations. -- NMG enters supported Atomic Action cases through - `motion_source="motion_gen"`. -- Planning, motion, execution, and task success remain distinct. -- New planners and metrics register without runner changes. -- Every baseline replays the same fixed manifest. -- Native timing and common execution are not mixed. -- Every run has raw trial artifacts, reproducibility metadata, and one - Markdown report. -- The report has exactly three tables and a leaderboard covering every - evaluated algorithm. -- Unsupported capabilities, real failures, and runtime errors remain distinct. -- PR smoke runs finish within a practical budget; coverage/full run in - nightly/release workflows. diff --git a/scripts/benchmark/motion_generation/README.md b/scripts/benchmark/motion_generation/README.md new file mode 100644 index 000000000..bddfd4831 --- /dev/null +++ b/scripts/benchmark/motion_generation/README.md @@ -0,0 +1,33 @@ +# Motion Generation Benchmark + +Free-space motion-generation suite with cuRobo as the default primary baseline. + +## Run + +```bash +embodichain benchmark motion-generation --suite smoke +embodichain benchmark motion-generation --suite coverage +embodichain benchmark motion-generation --extra-baselines ik_interpolate toppra +``` + +Artifacts land under `outputs/benchmarks/motion_generation//` +(`resolved_suite.yaml`, `case_manifest.json`, `trials.jsonl`, `aggregates.json`, +`report.md` with exactly three tables). + +## Implemented + +- Extensible planner/scenario registries and track-based suite YAML +- `free-space-common` track with fixed manifests and start-state bins +- Default matrix: cuRobo (`primary_baseline`); IK / TOPPRA optional diagnostics +- NMG adapter stub (`candidate`, disabled until a checkpoint is ready) +- Lifecycle timing: construct / prepare / cold / warm +- Ordered waypoint matching and external `motion_valid` (separate from + `PlanResult.success`) +- One Markdown report: Time & Memory, Success & Other Metrics, Leaderboard + +## Not implemented yet + +- Real NMG checkpoint adapter +- `collision-deployment` and `atomic-task` tracks +- Physics execution / task-success metrics +- Latency-budget Pareto sweeps, confidence intervals, subprocess isolation From 9e792ea348b3510c307c7ba08851d244cc2feea0 Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 02:53:08 +0800 Subject: [PATCH 07/17] Recover design docs --- .../motion_generation/BENCHMARK_DESIGN.md | 995 ++++++++++++++++++ scripts/benchmark/motion_generation/README.md | 2 + 2 files changed, 997 insertions(+) create mode 100644 scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md diff --git a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md new file mode 100644 index 000000000..2fb6a863e --- /dev/null +++ b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md @@ -0,0 +1,995 @@ +# Neural Motion Generator Benchmark Design + +## Proposal + +Build an extensible motion-generation benchmark for EmbodiChain that treats +Neural Motion Generator (NMG) checkpoints as candidates and cuRobo as the +primary baseline. The benchmark should evaluate three distinct questions: + +1. How fast and resource-efficient is trajectory generation? +2. Is the generated trajectory accurate, safe, smooth, and executable? +3. Does the trajectory complete an Atomic Action or a multi-action task under + physics simulation? + +The default comparison should be NMG versus cuRobo. IK plus interpolation and +TOPPRA should remain optional diagnostic baselines rather than define the main +leaderboard. + +## Motivation + +The existing NeuralPlanner benchmark provides useful latency, memory, rollout, +and endpoint-error measurements, but it only exercises fixed Franka waypoint +sets in planner-only mode. It does not measure collision safety, dynamic +feasibility, execution tracking, or physical task completion. It also does not +currently include cuRobo even though both `BasePlanner` and `MotionGenerator` +support the cuRobo backend. + +NMG will continue to evolve toward obstacle conditioning, multimodal motion, +physics-aware refinement, closed-loop recovery, and cross-embodiment +adaptation. A versioned suite, fixed case manifests, capability-aware tracks, +and stage-specific outcomes are needed so future results remain comparable and +failures remain diagnosable. + +## 1. Objectives and scope + +The benchmark should support: + +- regression testing across NMG checkpoint and model revisions; +- paired comparison between NMG and cuRobo for success, trajectory quality, + collision safety, latency, throughput, and memory; +- optional IK plus interpolation and TOPPRA diagnostics; +- incremental tracks for obstacle tokens, multimodal generation, Analytic + Policy Gradient (APG) refinement, closed-loop recovery, and new robots; +- actionable failure attribution for training-data and model iteration. + +The benchmark must not collapse every measurement into one opaque composite +score. In particular, `PlanResult.success` must not be treated as equivalent to +physical task success. + +## 2. Current implementation and design implications + +### 2.1 Current NeuralPlanner capability boundary + +`embodichain/lab/sim/planners/neural_planner.py` currently: + +- directly supports only `MoveType.EEF_MOVE`; +- uses a 7-DoF waypoint Transformer checkpoint, currently centered on Franka; +- derives the maximum number of waypoints from checkpoint `waypoint_max`; +- 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; +- has no explicit obstacle, collision, multimodal-sampling, or APG-refinement + input/output interface yet. + +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. + +### 2.2 Gaps in the current NeuralPlanner benchmark + +`scripts/benchmark/motion_generation/run_benchmark.py` already handles: + +- warmup trials separately from measured trials; +- CUDA synchronization; +- CPU RSS, GPU allocation delta, and peak GPU memory; +- planning latency, final TCP error, and waypoint best-hit error; +- optional IK-interpolation and TOPPRA baselines. + +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; +- path length, path efficiency, and smoothness; +- physics execution and controller tracking; +- Atomic Action task completion; +- batch-scaling measurements; +- resolved checkpoint, case-manifest, seed, software, and hardware metadata. + +The planner and `MotionGenerator` already register `CuroboPlanner`, but the +current NMG benchmark does not include it. The new default matrix should be +`NMG vs cuRobo`; IK interpolation and TOPPRA should not be the primary +reference. + +The current `compute_waypoint_errors()` independently searches the whole +trajectory for the best sample for each waypoint. This can reward out-of-order +motion and can select different samples for the best position and orientation. +The new benchmark must use ordered waypoint matching. + +The current report also creates multiple Quality/Performance tables grouped by +waypoint count plus two leaderboards. EmbodiChain benchmark convention requires +one Markdown report with exactly three tables: + +1. `Time & Memory` +2. `Success & Other Metrics` +3. `Leaderboard` + +### 2.3 Atomic Action integration + +`ActionCfg.motion_source` defaults to `"ik_interp"`. Existing Atomic Action +benchmarks construct a TOPPRA `MotionGenerator`, but cases that do not +explicitly change `motion_source` still use local IK plus interpolation. + +NMG and cuRobo Atomic Action evaluation must explicitly set: + +```python +cfg.motion_source = "motion_gen" +``` + +The NMG checkpoint must remain confined to `NeuralPlannerCfg`. Atomic Action +scenarios, grasp sampling, objects, controllers, and task-success rules must +not contain NMG-specific branches. The planner factory should be the only +backend-specific injection point. + +## 3. Architecture + +```text +Suite YAML + fixed Case Manifest + | + v + Scenario Providers + / | \ + planner-only trajectory atomic-task + | | | + +------ Planner Factory/Adapter ------+ + | + v + Raw Trial Records + | + +-----------+-----------+ + | | + Metric Evaluators Failure Classifier + | | + +-----------+-----------+ + | + v + Aggregates + Leaderboard + | + v + one Markdown report with exactly 3 tables +``` + +Keep the existing CLI entry point: + +```bash +embodichain benchmark motion-generation +``` + +Reuse established patterns from the current benchmark system: + +- dispatch through `scripts/benchmark/__main__.py`; +- Atomic Action `smoke/coverage/full` profiles, case sweeps, physics replay, + and physical-success rules; +- RL benchmark suite YAML, config/runner/reporting separation, resolved + protocol, per-run artifacts, and compatibility-aware resume; +- the current NeuralPlanner warmup, CUDA synchronization, memory measurement, + and optional-baseline flow; +- one Markdown report, exactly three tables, and a complete leaderboard. + +Refactor the current monolithic script incrementally into: + +```text +scripts/benchmark/motion_generation/ +├── run_benchmark.py # thin CLI and compatibility entry point +├── config.py # suite, planner, and scenario configuration +├── registry.py # planner/scenario/metric registries +├── runner.py # case matrix, warmup, trials, and resume +├── artifacts.py # manifests, JSONL, and environment metadata +├── aggregation.py # grouping, confidence intervals, leaderboard +├── reporting.py # exactly three Markdown tables +├── planners/ +│ ├── base.py +│ ├── neural.py +│ ├── curobo.py +│ ├── ik_interpolate.py +│ └── toppra.py +├── scenarios/ +│ ├── reach.py +│ ├── waypoint_path.py +│ ├── obstacle.py +│ ├── perturbation.py +│ └── atomic_action.py +├── metrics/ +│ ├── performance.py +│ ├── kinematic.py +│ ├── dynamic.py +│ ├── collision.py +│ ├── execution.py +│ └── task.py +└── suites/ + ├── smoke.yaml + ├── coverage.yaml + └── full.yaml +``` + +### 3.1 Extension interfaces + +Runner logic should not branch on planner names. Use protocols such as: + +```python +class PlannerAdapter(Protocol): + @property + def metadata(self) -> PlannerMetadata: ... + + def build(self, context: BenchmarkContext) -> MotionGenerator: ... + + def prepare(self, case: BenchmarkCase) -> PreparationMetrics: ... + + def warmup(self, case: BenchmarkCase) -> None: ... + + def plan(self, case: BenchmarkCase) -> PlanResult: ... + + +class ScenarioProvider(Protocol): + @property + def required_capabilities(self) -> frozenset[str]: ... + + def generate_cases( + self, + manifest: SuiteManifest, + seed: int, + ) -> Iterable[BenchmarkCase]: ... + + +class MetricEvaluator(Protocol): + @property + def required_artifacts(self) -> frozenset[str]: ... + + def evaluate(self, trial: TrialArtifacts) -> dict[str, float | bool]: ... +``` + +`PlannerMetadata` should include at least: + +- `algorithm_id`, for example `nmg_transformer`, `curobo`, + `ik_interpolate`, or `toppra`; +- `algorithm_role`: `candidate`, `primary_baseline`, or + `diagnostic_baseline`; +- model revision, checkpoint path, and SHA256 when applicable; +- capabilities such as `eef_waypoint`, `joint_waypoint`, `obstacle`, + `sampling`, `refinement`, and `closed_loop`; +- supported robots, maximum waypoint count, and input/output schema version; +- planner parameters, model parameter count, and inference dtype. + +Adding an NMG architecture, refiner, or baseline should require only a new +adapter/registry entry and suite configuration, not runner, aggregation, or +reporting changes. + +### 3.2 Trial data model + +Each trial should have a stable key: + +```text +(suite_version, track, scenario_id, case_id, algorithm_id, + model_revision, seed, repeat, batch_size) +``` + +`TrialRecord` should separate: + +- **identity**: the key above, robot, device, git commit, and config/checkpoint + hashes; +- **case**: start qpos, target waypoints, obstacle/object state, and + perturbations; +- **outcomes**: planning, motion, execution, and task success plus failure + stage; +- **metrics**: performance, memory, trajectory, and task values. + +Raw JSONL/JSON artifacts should retain numeric types. Percentage formatting +belongs only in Markdown rendering, not before aggregation. + +### 3.3 Primary NMG-versus-cuRobo protocol + +The default suite should require only: + +- `nmg:` as the candidate; +- `curobo:` as the primary baseline. + +IK interpolation and TOPPRA should be enabled only through +`--extra-baselines` or suite configuration. If enabled, they must still appear +in reports and the leaderboard with role `diagnostic_baseline`. + +Use three paired tracks: + +1. **Free-space common input**: cuRobo uses an empty collision world. Both + planners receive identical start qpos and ordered EEF waypoints. This is the + primary quality and performance leaderboard. +2. **Collision-aware deployment**: both planners execute in the same + simulation scene; cuRobo receives the correct collision world while current + NMG does not receive obstacle tokens. This track measures deployment + behavior and the current capability gap, not model quality under equal + information. +3. **Atomic task**: both planners run through the same `AtomicActionEngine`, + objects, grasps, controller, and physical success criteria. + +cuRobo supports both `EEF_MOVE` and `JOINT_MOVE`, while current NMG supports +only `EEF_MOVE`. The primary leaderboard must use their common `EEF_MOVE` +capability. Joint-space cases belong in cuRobo-only or diagnostic tracks. + +#### Freeze the cuRobo configuration + +Every run must record and hash: + +- `max_attempts` and `max_planning_time`; +- `interpolation_dt` and `collision_activation_distance`; +- `use_cuda_graph`, actual fallback state, and `warmup_iterations`; +- robot sphere-fit settings and collision-sphere buffer; +- obstacle representation, collision cache, and `multi_env`; +- static/dynamic obstacle names and world-content hash; +- `preserve_plan_samples`. + +World representation and sphere fitting are part of the baseline definition +and must not change silently between checkpoint comparisons. + +The primary leaderboard should use a frozen operational configuration, for +example checkpoint-default NMG `max_steps` and fixed cuRobo `max_attempts`. +Also add a latency-budget sweep: + +- sweep NMG `max_steps`; +- sweep cuRobo `max_attempts`, with a common `planning_budget_ms`; +- retain success-latency Pareto data. + +`CuroboPlannerCfg.max_planning_time` currently validates the budget after the +plan; it is not a preemptive real-time deadline. The outer benchmark must +record actual wall latency and `budget_compliance_rate` whether or not a +planner supports interruption. + +#### Lifecycle and timing + +cuRobo lazily creates and caches a backend for each +`(control_part, batch_size, multi_env, move_type)`. First use may include +robot/world YAML generation, sphere fitting, collision-cache allocation, CUDA +graph capture, and warmup. NMG has checkpoint loading, actor construction, and +device transfer. + +Report the following separately for both: + +```text +planner_construct_ms +backend_prepare_ms +cold_plan_ms +warm_plan_ms +``` + +The planning-latency leaderboard must use only `warm_plan_ms`. +`backend_prepare_ms` represents one-time deployment cost; `cold_plan_ms` +represents the first real case. Every batch size and goal type needs its own +prepare/warmup phase. + +cuRobo always plans on CUDA. The primary comparison should therefore use the +same CUDA device and fp32 interface. NMG CPU results may be reported as a +separate characterization, not ranked against cuRobo CUDA latency. + +#### Multi-waypoint and sample policy + +cuRobo plans multiple waypoint segments sequentially. NMG consumes the full +waypoint sequence in one model invocation. The primary performance metric is +the total cost of one `MotionGenerator.generate()` call for the same high-level +input. Also report `num_segments` and `cost_time_per_segment_ms`, but do not +replace total-latency ranking with per-segment latency. + +- Planner-native quality: set cuRobo `preserve_plan_samples=True` to retain + native collision-checked samples and `dt`. +- Common path metrics: resample derived copies from both planners by the same + arc-length procedure. +- Atomic Action/common execution: use the same action `sample_interval` and let + `TrajectoryBuilder` perform common resampling, but do not call the resampled + result a native-timing trajectory. + +#### Collision worlds + +- Use `CuroboWorldCfg.multi_env=False` when every batch row has the same + robot-relative obstacle layout. +- Use `multi_env=True` when obstacle poses differ relative to each robot. +- Supply per-environment dynamic poses through `dynamic_obstacle_names` and + `CuroboPlanOptions.dynamic_obstacle_poses`. +- Dynamic obstacles must use `cuboid` or `mesh`, not the `sphere` + representation that cannot be updated by the original object name. +- Revalidate collision success with an independent simulator/common + validator. cuRobo `success=True` is not benchmark ground truth. + +## 4. Layered evaluation + +### 4.1 L0: generation performance + +L0 isolates planner computation and does not execute the trajectory. + +Sweep: + +- batch size: `1, 8, 64`, with larger batches in the full profile; +- waypoint count: `1, 3, min(5, model_max)`, plus supported maxima; +- start state: nominal, random reachable, near joint limit, near singularity; +- path shape: direct, L-turn, S-curve, orientation-only, and combined + translation/orientation; +- target-distance and orientation-delta bins; +- primary device/dtype: same CUDA device and fp32 interface for NMG and cuRobo; +- separate NMG CPU/fp16/bf16 characterization; +- cold start and warm steady state. + +Timing boundaries: + +- measure `planner_construct_ms` and `backend_prepare_ms` separately; +- measure `cold_plan_ms` for the first real input; +- measure `warm_plan_ms` after fixed warmup; +- exclude setup, case generation, reporting, FK metrics, and validation; +- call `torch.cuda.synchronize()` before and after CUDA timing. + +Primary metrics: + +- latency p50/p95/p99; +- `latency_per_env_ms`; +- `cost_time_per_segment_ms` for explaining multi-waypoint scaling; +- trajectories per second; +- rollout steps and policy steps per second; +- CPU RSS delta, GPU allocation delta, and peak GPU memory; +- real-time factor only when trajectory duration has clear semantics. + +For Atomic Action tracks, separate `action_planning_ms`, +`physics_execution_ms`, and `task_end_to_end_ms`. + +### 4.2 L1: trajectory quality and executability + +Distinguish three evaluation views: + +1. **path-only**: resample by arc length and compare geometry; +2. **native-timing**: use each planner's own `dt/duration`; +3. **common-execution**: use the same controller, control dt, and simulator. + +Do not directly compare NMG's fixed nominal `dt=0.01` against IK interpolation +with no meaningful duration. Report unavailable native timing as `N/A`. +Dynamic fairness should come from common execution or common +time-parameterization. + +Use `preserve_plan_samples=True` for cuRobo native-timing evaluation and the +original NMG `PlanResult`. Recompute endpoint, constraint, collision, and +smoothness metrics from output trajectories rather than trusting either +planner's internal success flag. + +#### Goal and waypoint metrics + +- final translation error in mm; +- final rotation geodesic error in degrees; +- ordered waypoint success rate; +- waypoint translation/rotation mean, p95, and maximum error; +- completed waypoint ratio; +- time or step to final target. + +Define ordered arrival as: + +```text +t_i = the first sample satisfying t_i > t_(i-1), and + position_error(t_i) <= pos_threshold, and + rotation_error(t_i) <= rot_threshold +``` + +The waypoint sequence succeeds only if every valid waypoint has a matching +`t_i`. Continuous error statistics may use monotonic dynamic programming to +jointly match waypoints and trajectory samples. Position and orientation must +not select unrelated best samples. + +#### Kinematic and dynamic metrics + +- finite-value rate; +- joint-position-limit violation rate and maximum normalized violation; +- joint velocity, acceleration, and jerk violation rates; +- maximum/mean joint velocity, acceleration, and jerk; +- joint path length; +- Cartesian translation and rotation path length; +- path efficiency relative to a geometric lower bound or same-case reference; +- path curvature and path-only smoothness; +- time-indexed integrated squared acceleration and jerk; +- endpoint settling error and hold stability. + +Define `motion_valid` independently: + +```text +motion_valid = + finite + and ordered_waypoints_reached + and joint_limits_satisfied + and dynamic_limits_satisfied_when_applicable + and collision_free_when_applicable +``` + +#### Collision and physics-execution metrics + +- environment collision rate; +- self-collision rate; +- minimum clearance; +- undesired-contact count and maximum contact impulse; +- controller joint-tracking RMSE and maximum error; +- executed TCP tracking RMSE; +- execution timeout rate; +- final pose error after simulation execution; +- final pose drift after a fixed stable-hold period. + +Enable collision metrics only when the scenario supplies a trustworthy +collision world. In `free-space-common`, cuRobo receives an empty world. In +`collision-deployment`, cuRobo receives the full world while current NMG is an +`obstacle_unaware` candidate. The report must expose this information +asymmetry. + +#### Reference-based metrics + +ADE/FDE, expert joint distance, and cost ratio are diagnostic, not primary +success criteria. A single reference path can unfairly penalize valid alternate +IK branches or left/right obstacle-avoidance modes. + +cuRobo may serve as a strong reference for path cost, duration, and clearance, +but it is not the only ground truth. NMG should pass whenever it satisfies the +same external constraints and task criteria, even with a different valid path. + +Future generative NMG tracks should add: + +- top-k feasibility/success; +- best-of-k cost; +- valid mode count and trajectory diversity; +- total sampling cost per successful sample. + +### 4.3 L2: Atomic Actions and task completion + +L2 uses `AtomicActionEngine` to generate a trajectory and then executes or +replays it in physics simulation. Object, contact, and robot state determine +task success. + +Backend fairness: + +- NMG and primary baseline cuRobo use `motion_source="motion_gen"`; +- optional TOPPRA diagnostics use `motion_source="motion_gen"`; +- optional IK-interpolation diagnostics use `motion_source="ik_interp"`; +- grasp generator, object preset, start state, target, controller, sample + interval, seed, and physics parameters are identical; +- do not execute a fabricated trajectory after planning failure; +- restore robot, object, and simulator state before every case. + +Contact tasks need explicit collision-world ownership: + +- the manipulated Pick/Place target must not be treated as a generic + non-contact obstacle during required contact phases; +- tables, environmental obstacles, and non-target objects should enter the + cuRobo world; +- the current EmbodiChain cuRobo adapter does not expose dynamic held-object + attachment, so `MoveHeldObject` must record + `held_object_geometry_in_planner=false` and validate object collisions in + simulation; +- write visible constraints into `constraint_information` for every result. + +Suggested coverage: + +| Action or sequence | Primary task-success criteria | +|---|---| +| MoveEndEffector | Planning succeeds, executed TCP reaches and holds target, no disallowed collision | +| PickUp | Approach/lift plan succeeds, `held_object` is created, minimum object lift is reached, no drop | +| MoveHeldObject | Object reaches target pose, grasp remains stable, object drift/tilt stays within threshold | +| Place | Place pose reached, release succeeds, final object pose is correct and stable | +| Press | Press depth and valid contact/force reached, retract succeeds, no abnormal object motion | +| Pick-Move-Place | Every stage succeeds in sequence; final object pose and release state are correct | + +Record: + +- `planning_success`; +- `motion_valid`; +- `execution_success`; +- `task_success`; +- per-Atomic-Action stage success; +- task completion time; +- replan/retry count; +- task-specific pose, lift, slip, release, and contact metrics. + +Sequence success must come from one sequential episode. Do not approximate it +by multiplying independently measured action success rates. + +## 5. Scenario tracks and capability gates + +| Track | Current NMG | cuRobo | Purpose | +|---|---:|---:|---| +| `free-space-common` | Supported | Supported | Empty-world, identical EEF-waypoint primary comparison | +| `workspace-generalization` | Supported | Supported | Workspace, orientation, joint-limit, and singularity bins | +| `collision-deployment` | Executable without obstacle input | Supported | Deployment success and current capability gap | +| `atomic-task` | Partially supported | Supported | Atomic Action and action-chain physical completion | +| `obstacle-aware-common-input` | Not yet supported | Supported | Future equal-information scene-constraint comparison | +| `multimodal` | Not yet supported | Single-output reference | Top-k coverage, diversity, and sampling cost | +| `physics-refinement` | Not yet supported | Reference | NMG initialization plus APG/trajectory optimization | +| `closed-loop-recovery` | Not yet supported | Not yet supported | Observation, target, and disturbance recovery | +| `cross-embodiment` | Not yet supported | Multi-robot configuration | Robot, DoF, control-rate, and dynamics adaptation | + +Before case generation, check `required_capabilities`: + +- `supported`: run normally; +- `unsupported`: record the reason and exclude it from that track's + denominator; +- `error`: the adapter declared support but failed, so count it as failure. + +Always report `coverage_rate` to prevent selective execution from improving +rank. Formal track eligibility requires 100% coverage of mandatory cases. +Retain ineligible algorithms in the leaderboard with `eligible=false`. + +## 6. Suites and scenario matrix + +### 6.1 Profiles + +- `smoke`: one robot, a few deterministic cases, `B=1`, one seed; for PRs. +- `coverage`: workspace/path bins, `B=1/8/64`, at least five seeds, and + representative Atomic Action object/position cases; for nightly runs. +- `full`: dense workspace/OOD cases, boundary states, all objects/positions, + obstacles, perturbations, and at least 20 seeds; for model releases. + +### 6.2 Fixed case manifests + +Generate randomized scenarios into a fixed manifest before giving them to any +planner. Save: + +- robot and start qpos; +- ordered target waypoints; +- object/obstacle poses and physical properties; +- target distance, orientation delta, and workspace bin; +- perturbation schedule; +- validity evidence, such as independent reachability validation; +- case schema version. + +Unreachable targets may form a separate robustness track but must not enter the +normal success denominator. Case inclusion must not depend on the success of +the cuRobo run being evaluated, which would create baseline selection bias. +Use generation rules, an independent validator, or a frozen offline oracle. + +### 6.3 Example suite + +```yaml +schema_version: 1 +suite: nmg_coverage_v1 +profile: coverage +seeds: [11, 23, 37, 53, 71] + +planners: + - id: nmg_transformer + adapter: neural + role: candidate + checkpoint: ${NMG_CHECKPOINT} + - id: curobo + adapter: curobo + role: primary_baseline + config: + max_attempts: 5 + interpolation_dt: 0.025 + use_cuda_graph: true + warmup_iterations: 1 + preserve_plan_samples: true + world: + obstacle_representation: mesh + multi_env: false + +tracks: + free-space-common: + required_capabilities: [eef_waypoint] + scenarios: [nominal_reach, random_reach, waypoint_path, boundary_reach] + batch_sizes: [1, 8, 64] + waypoint_counts: [1, 3, 5] + world: empty + collision-deployment: + required_capabilities: [eef_waypoint] + scenarios: [static_obstacle, randomized_obstacle] + comparison_mode: asymmetric_information + atomic-task: + required_capabilities: [eef_waypoint] + scenarios: + - move_end_effector + - pick_up + - move_held_object + - place + - press + - pick_move_place + +scenario_overrides: + randomized_obstacle: + planners: + curobo: + config: + world: + multi_env: true + +track_overrides: + atomic-task: + planners: + curobo: + config: + preserve_plan_samples: false + +protocol: + warmup_trials: 3 + measured_trials: 20 + confidence_level: 0.95 + common_control_dt: 0.01 +``` + +Environment variables are only configuration inputs. Save resolved values, +NMG checkpoint hash, cuRobo config hash, and world-content hash in run +metadata. + +`free-space-common`, `collision-deployment`, and `atomic-task` should create +isolated planner instances. Rebuild cuRobo planner/backends when static world +geometry or representation changes. Reuse a backend only for fixed geometry +whose registered dynamic-obstacle poses are updated. + +## 7. Success semantics and failure taxonomy + +Each case defines `primary_success`: + +- L0 planner-only: `planning_success`; +- L1 trajectory: `motion_valid`; +- L2 execution: `execution_success`; +- L2 task: `task_success`. + +Planner-internal `pos_eps/rot_eps` only determine when that planner stops. +Cross-algorithm `ordered_waypoints_reached`, `motion_valid`, and +`primary_success` must use suite-owned, versioned external thresholds that are +identical for every algorithm. + +Retain every stage: + +```text +input_valid + -> planning_success + -> ordered_waypoints_reached + -> motion_valid + -> execution_success + -> task_success +``` + +Use a stable failure taxonomy: + +- `invalid_case` +- `unsupported_capability` +- `checkpoint_load_failure` +- `planner_exception` +- `planner_reported_failure` +- `timeout` +- `non_finite_trajectory` +- `waypoint_miss` +- `joint_limit_violation` +- `dynamic_limit_violation` +- `self_collision` +- `environment_collision` +- `controller_tracking_failure` +- `object_not_grasped` +- `object_dropped` +- `release_failure` +- `task_goal_miss` + +Store failure stage and reason in raw artifacts. Notes may summarize common +failures, but must not add a fourth Markdown table. + +## 8. Fairness and statistical protocol + +### 8.1 Baseline fairness + +- Use the same case, start qpos, waypoint order, and external tolerance. +- Default to NMG candidate versus cuRobo primary baseline. +- Give cuRobo an empty world in `free-space-common`. +- Record the scene/constraint information visible to each planner in + `collision-deployment`. +- Time only planner generation; exclude FK/collision metrics. +- Do not force identical sample counts during native generation. +- Apply common path resampling only to derived copies. +- Use the same controller and control dt for execution metrics. +- Report native time parameterization and common execution separately. +- Retain baseline failures; never remove failed cases from aggregation. +- Report success-conditioned continuous metrics and all-case failure-aware + statistics to avoid survivor bias. +- Prefer isolated subprocesses for NMG and cuRobo. cuRobo CUDA graphs, + backends, and world caches persist and can contaminate memory or cold-start + results based on execution order. + +### 8.2 Reproducibility and confidence intervals + +- Fix Python, NumPy, Torch, and simulator seeds. +- Warm up every `(planner, scenario, batch_size, waypoint_count)` separately. +- Report latency p50/p95/p99. +- Report Wilson or bootstrap 95% confidence intervals for success. +- Report mean, median, p95, and bootstrap 95% confidence intervals for + continuous metrics. +- Record OS, CPU, GPU, driver, CUDA, Torch, DexSim, and EmbodiChain git commit. +- In addition to PyTorch allocator memory, record process-level GPU memory when + available to capture cuRobo/CUDA-graph allocations. +- Save the resolved suite and case manifest. + +### 8.3 Protocol versioning + +- Version suite, case manifest, metric schema, and report schema independently. +- Increment suite version when thresholds, scenario distribution, or primary + success changes. +- Combine leaderboard results only across identical suite/protocol versions. +- Require matching hardware class, device, dtype, and batch protocol for + latency leaderboards. +- Historical checkpoints may be rerun on a new suite, but old results must not + be silently relabeled as the new protocol. + +## 9. Artifacts and report contract + +Suggested output: + +```text +outputs/benchmarks/nmg// +├── resolved_suite.yaml +├── environment.json +├── case_manifest.json +├── trials.jsonl +├── aggregates.json +├── report.md +└── videos/ # optional representative success/failure cases +``` + +Each run produces one `report.md` with exactly three tables. + +### 9.1 Time & Memory + +Recommended columns: + +```text +track, scenario, comparison_mode, algorithm, algorithm_role, model_revision, +planner_config_hash, batch_size, waypoint_count, num_segments, num_trials, +planner_construct_ms, backend_prepare_ms, cost_time_ms, cold_plan_ms, +warm_plan_ms_p50, warm_plan_ms_p95, trajectories_per_second, +planning_budget_ms, budget_compliance_rate, +cpu_delta_mb, gpu_delta_mb, peak_gpu_mb +``` + +`cost_time_ms` is the mean primary steady-state timed operation for the row. +The scenario protocol or a `timing_scope` field must define that operation. + +### 9.2 Success & Other Metrics + +Aggregate by `(track, scenario, algorithm, condition_bin)`: + +```text +track, scenario, comparison_mode, algorithm, algorithm_role, +constraint_information, cases, coverage_rate, success_rate, +planning_success_rate, motion_valid_rate, execution_success_rate, +task_success_rate, final_pos_err_mm, final_rot_err_deg, +waypoint_completion_rate, joint_violation_rate, dynamic_violation_rate, +collision_rate, min_clearance_m, path_efficiency, jerk_cost, +task_metric, top_failure +``` + +Use `N/A`, not zero, for inapplicable values. + +### 9.3 Leaderboard + +Use one table with a `track` column: + +```text +track, rank, algorithm, algorithm_role, model_revision, planner_config_hash, +eligible, coverage_rate, overall_success_rate, motion_valid_rate, +task_success_rate, latency_p95_ms, peak_gpu_mb +``` + +`overall_success_rate` is the macro average of `primary_success` over all +mandatory cases in the track. Sort within each track by: + +1. `eligible=True`; +2. `overall_success_rate` descending; +3. `coverage_rate` descending; +4. `latency_p95_ms` ascending. + +Include every evaluated algorithm in the current scope, not only the top +entries. Trajectory imitation error must not override task success in ranking. + +## 10. NMG-specific diagnostics + +These should not block v1, but the schema should reserve them. + +### 10.1 Solution leakage + +- Use Cartesian-only conditioning for the primary leaderboard. +- Treat Cartesian plus joint target as an ablation only. +- Test the same Cartesian target from different start qpos and legal IK + branches. +- Report sensitivity to joint-interpolation shortcuts. +- Record actual checkpoint input fields in metadata. + +### 10.2 Physics refinement and APG + +Create paired cases: + +- `nmg_raw` +- `nmg_plus_refinement` + +Report: + +- hard-feasibility gain; +- task-success gain; +- trajectory-cost reduction; +- added latency and iterations; +- refinement-divergence rate. + +### 10.3 Closed-loop recovery + +When observation feedback is available, add: + +- state-observation noise; +- target-pose motion during execution; +- external force or joint-tracking disturbances; +- object slip; +- obstacle motion. + +Report recovery success, time to recover, replan count, maximum post-disturbance +error, and final task success. The current FK rollout must not be reported as +closed-loop recovery. + +## 11. Implementation phases + +### Phase 0: protocol and cuRobo baseline + +- Split config, aggregation, and reporting from the current script. +- Preserve the current CLI and checkpoint download behavior. +- Add a cuRobo adapter and make it the default primary baseline. +- Default the CLI/suite to `nmg curobo`; require explicit diagnostic + IK/TOPPRA baselines. +- Separate cuRobo backend prepare, cold plan, and warm plan timing. +- Produce exactly three report tables. +- Save resolved config, environment metadata, and trial JSONL. +- Add unit tests for report schema, full leaderboard coverage, and ordered + waypoint metrics. + +Minimum tests: + +- ordered waypoint matching rejects out-of-order and split + position/orientation hits; +- warmup trials do not enter aggregation; +- `unsupported`, `error`, and ordinary failure remain distinct; +- failed trials are not silently removed from continuous aggregation; +- the leaderboard includes every in-scope algorithm and applies + success/coverage/latency ordering; +- generated reports contain exactly three Markdown tables; +- planner-only smoke test with a fake NMG checkpoint; +- graceful skip when the optional cuRobo runtime is unavailable; +- cuRobo empty-world, shared-world, and multi-env dynamic-world configuration; +- Atomic Action integration with `motion_source="motion_gen"`. + +### Phase 1: core motion + +- Add fixed manifests, randomized workspace/path cases, and batch scaling. +- Implement paired NMG-versus-cuRobo `free-space-common`. +- Add frozen operational configuration and latency-budget/Pareto sweeps. +- Add path-only, native-timing, joint, and dynamic metrics. +- Add common resampling and failure classification. +- Keep cuRobo as the primary baseline; add IK/TOPPRA only through adapters when + requested. + +### Phase 2: physics and Atomic Actions + +- Add cuRobo collision worlds and `collision-deployment`. +- Parameterize planner construction in Atomic Action benchmarks. +- Explicitly separate `ik_interp` and `motion_gen`. +- Reuse current object/position/approach profiles and physical-success rules. +- Add MoveEndEffector, PickUp, MoveHeldObject, Place, Press, and + Pick-Move-Place. +- Add controller tracking, collision/contact, and stable-hold metrics. + +### Phase 3: future NMG capabilities + +- Add equal-information obstacle-aware, multimodal, APG-refinement, and + closed-loop-recovery tracks. +- Add cross-embodiment and unseen-robot tracks. +- Add release profiles and a long-lived checkpoint leaderboard. + +## 12. Acceptance criteria + +- The current Franka NMG checkpoint and cuRobo run the same default `smoke` and + `free-space-common` manifest. +- cuRobo is the default primary baseline; IK/TOPPRA do not affect the default + main leaderboard. +- cuRobo backend preparation and steady-state planning latency are separate. +- Free-space, collision-deployment, and Atomic Action tracks use correct, + traceable cuRobo world configurations. +- NMG enters supported Atomic Action cases through + `motion_source="motion_gen"`. +- Planning, motion, execution, and task success remain distinct. +- New planners and metrics register without runner changes. +- Every baseline replays the same fixed manifest. +- Native timing and common execution are not mixed. +- Every run has raw trial artifacts, reproducibility metadata, and one + Markdown report. +- The report has exactly three tables and a leaderboard covering every + evaluated algorithm. +- Unsupported capabilities, real failures, and runtime errors remain distinct. +- PR smoke runs finish within a practical budget; coverage/full run in + nightly/release workflows. diff --git a/scripts/benchmark/motion_generation/README.md b/scripts/benchmark/motion_generation/README.md index bddfd4831..de0c4db68 100644 --- a/scripts/benchmark/motion_generation/README.md +++ b/scripts/benchmark/motion_generation/README.md @@ -2,6 +2,8 @@ Free-space motion-generation suite with cuRobo as the default primary baseline. +Design background and roadmap: see [`BENCHMARK_DESIGN.md`](./BENCHMARK_DESIGN.md). + ## Run ```bash From e000569a32478bb585da06b7cdb5b77a23234d7d Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 03:07:02 +0800 Subject: [PATCH 08/17] WIP --- .../lab/sim/planners/toppra_planner.py | 6 +- .../motion_generation/BENCHMARK_DESIGN.md | 4 +- scripts/benchmark/motion_generation/README.md | 1 + .../motion_generation/aggregation.py | 22 ++- scripts/benchmark/motion_generation/compat.py | 159 ----------------- .../motion_generation/metrics/trajectory.py | 2 +- .../motion_generation/planners/curobo.py | 1 + .../motion_generation/planners/toppra.py | 8 + .../benchmark/motion_generation/reporting.py | 1 - .../motion_generation/run_benchmark.py | 62 ++----- .../motion_generation/scenarios/free_space.py | 12 +- .../test_motion_generation_benchmark.py | 91 +++++++++- .../planners/test_neural_planner_benchmark.py | 161 ------------------ 13 files changed, 144 insertions(+), 386 deletions(-) delete mode 100644 scripts/benchmark/motion_generation/compat.py delete mode 100644 tests/benchmark/planners/test_neural_planner_benchmark.py diff --git a/embodichain/lab/sim/planners/toppra_planner.py b/embodichain/lab/sim/planners/toppra_planner.py index 104e2c077..3cc866738 100644 --- a/embodichain/lab/sim/planners/toppra_planner.py +++ b/embodichain/lab/sim/planners/toppra_planner.py @@ -396,13 +396,17 @@ def _shutdown_pool(self) -> None: self._pool = None + def close(self) -> None: + """Release TOPPRA worker processes owned by this planner.""" + self._shutdown_pool() + def __del__(self): # Only matters for in-process GC of an abandoned planner (and as a # non-Linux fallback). Process-exit cleanup is handled by the kernel # via PR_SET_PDEATHSIG installed in each worker, which survives the # os._exit(0) path that SimulationManager.destroy() takes. try: - self._shutdown_pool() + self.close() except Exception: pass diff --git a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md index 2fb6a863e..75bcef44c 100644 --- a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md +++ b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md @@ -171,7 +171,7 @@ Refactor the current monolithic script incrementally into: ```text scripts/benchmark/motion_generation/ -├── run_benchmark.py # thin CLI and compatibility entry point +├── run_benchmark.py # thin CLI entry point ├── config.py # suite, planner, and scenario configuration ├── registry.py # planner/scenario/metric registries ├── runner.py # case matrix, warmup, trials, and resume @@ -842,7 +842,7 @@ Aggregate by `(track, scenario, algorithm, condition_bin)`: ```text track, scenario, comparison_mode, algorithm, algorithm_role, constraint_information, cases, coverage_rate, success_rate, -planning_success_rate, motion_valid_rate, execution_success_rate, +planning_success_rate, execution_success_rate, task_success_rate, final_pos_err_mm, final_rot_err_deg, waypoint_completion_rate, joint_violation_rate, dynamic_violation_rate, collision_rate, min_clearance_m, path_efficiency, jerk_cost, diff --git a/scripts/benchmark/motion_generation/README.md b/scripts/benchmark/motion_generation/README.md index de0c4db68..9ec7ce42d 100644 --- a/scripts/benchmark/motion_generation/README.md +++ b/scripts/benchmark/motion_generation/README.md @@ -10,6 +10,7 @@ Design background and roadmap: see [`BENCHMARK_DESIGN.md`](./BENCHMARK_DESIGN.md embodichain benchmark motion-generation --suite smoke embodichain benchmark motion-generation --suite coverage embodichain benchmark motion-generation --extra-baselines ik_interpolate toppra +embodichain benchmark motion-generation --path-shapes direct l_turn --start-state-bins nominal near_singularity ``` Artifacts land under `outputs/benchmarks/motion_generation//` diff --git a/scripts/benchmark/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py index e83e781e0..155104282 100644 --- a/scripts/benchmark/motion_generation/aggregation.py +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -66,6 +66,16 @@ def _top_failure(outcomes: list[CaseOutcome]) -> str | None: return failures.most_common(1)[0][0] if failures else None +def _peak_gpu(records: Iterable[TrialRecord]) -> float | None: + """Return the maximum observed peak GPU MB, or ``None`` when unavailable.""" + peaks = [ + float(record.peak_gpu_mb) + for record in records + if record.peak_gpu_mb is not None and math.isfinite(float(record.peak_gpu_mb)) + ] + return max(peaks) if peaks else None + + def _track_ids(records: list[TrialRecord], cases: list[BenchmarkCase]) -> list[str]: """Return deterministic track ids observed in cases or records.""" tracks = {case.track for case in cases} @@ -153,9 +163,7 @@ def _performance_rows( ), "cpu_delta_mb": _mean(record.cpu_delta_mb for record in group), "gpu_delta_mb": _mean(record.gpu_delta_mb for record in group), - "peak_gpu_mb": max( - (record.peak_gpu_mb or 0.0 for record in group), default=0.0 - ), + "peak_gpu_mb": _peak_gpu(group), } ) @@ -272,6 +280,7 @@ def _metric_rows( "start_state_bin": start_state_bin, "cases": unique_cases_by_group[group_key], "coverage_rate": min(1.0, len(outcomes) / max(expected, 1)), + # Free-space primary success is external motion validity. "success_rate": _rate(outcome.motion_valid for outcome in outcomes), "planning_success_rate": _rate( outcome.planning_success for outcome in outcomes @@ -279,9 +288,6 @@ def _metric_rows( "ordered_waypoint_success_rate": _rate( outcome.ordered_waypoints_reached for outcome in outcomes ), - "motion_valid_rate": _rate( - outcome.motion_valid for outcome in outcomes - ), "waypoint_completion_rate": _mean( outcome.completed_waypoint_ratio for outcome in outcomes ), @@ -348,9 +354,7 @@ def _leaderboard_rows( latency_p95 = _percentile( (record.cost_time_ms for record in measured), 95.0 ) - peak_gpu = max( - (record.peak_gpu_mb or 0.0 for record in measured), default=None - ) + peak_gpu = _peak_gpu(measured) track_entries.append( { "track": track, diff --git a/scripts/benchmark/motion_generation/compat.py b/scripts/benchmark/motion_generation/compat.py deleted file mode 100644 index ba4a1e6de..000000000 --- a/scripts/benchmark/motion_generation/compat.py +++ /dev/null @@ -1,159 +0,0 @@ -# ---------------------------------------------------------------------------- -# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# ---------------------------------------------------------------------------- - -"""Temporary helpers retained for callers of the pre-refactor benchmark module.""" - -from __future__ import annotations - -import math -from collections import defaultdict - -__all__ = [ - "IMPL_IK", - "IMPL_NEURAL", - "IMPL_TOPPRA", - "QUALITY_SUMMARY_COLUMNS", - "aggregate_legacy_rows", - "format_waypoint_grouped_tables", -] - -IMPL_NEURAL = "neural_planner" -IMPL_IK = "ik_interpolate" -IMPL_TOPPRA = "ik_toppra" - -QUALITY_SUMMARY_COLUMNS = ( - "impl", - "num_trials", - "success_rate", - "final_translation_err_mm_mean", - "final_rotation_err_deg_mean", - "mean_waypoint_pos_err_mm_mean", - "max_waypoint_pos_err_mm_mean", - "mean_waypoint_rot_err_deg_mean", - "max_waypoint_rot_err_deg_mean", -) - -_IMPL_REPORT_ORDER = {IMPL_NEURAL: 0, IMPL_IK: 1, IMPL_TOPPRA: 2} - - -def _percentile(values: list[float], percentile: float) -> float: - """Return the legacy nearest-rank percentile.""" - ordered = sorted(values) - index = max( - 0, - min( - len(ordered) - 1, - math.ceil(percentile / 100.0 * len(ordered)) - 1, - ), - ) - return ordered[index] - - -def _mean_finite(rows: list[dict[str, object]], key: str) -> str: - """Format the mean of legacy finite numeric values.""" - values = [float(row[key]) for row in rows if math.isfinite(float(row[key]))] - return f"{sum(values) / len(values):.6f}" if values else "inf" - - -def aggregate_legacy_rows( - trial_rows: list[dict[str, object]], -) -> list[dict[str, object]]: - """Aggregate the legacy row schema during the CLI migration window.""" - groups: dict[tuple[str, int], list[dict[str, object]]] = defaultdict(list) - for row in trial_rows: - if not bool(row["warmup"]): - groups[(str(row["impl"]), int(row["num_waypoints"]))].append(row) - - summaries: list[dict[str, object]] = [] - for (impl, waypoint_count), rows in groups.items(): - costs = [float(row["cost_time_ms"]) for row in rows] - summaries.append( - { - "impl": impl, - "num_waypoints": waypoint_count, - "num_trials": len(rows), - "success_rate": f"{sum(bool(row['success']) for row in rows) / len(rows):.2%}", - "cost_time_ms_mean": f"{sum(costs) / len(costs):.6f}", - "cost_time_ms_p95": f"{_percentile(costs, 95.0):.6f}", - "rollout_steps_mean": f"{sum(int(row['rollout_steps']) for row in rows) / len(rows):.2f}", - "cpu_delta_mb_mean": f"{sum(float(row['cpu_delta_mb']) for row in rows) / len(rows):.6f}", - "gpu_delta_mb_mean": f"{sum(float(row['gpu_delta_mb']) for row in rows) / len(rows):.6f}", - "peak_gpu_mb_mean": f"{sum(float(row['peak_gpu_mb']) for row in rows) / len(rows):.6f}", - "peak_gpu_mb_max": f"{max(float(row['peak_gpu_mb']) for row in rows):.6f}", - "final_translation_err_mm_mean": _mean_finite( - rows, "final_translation_err_mm" - ), - "final_rotation_err_deg_mean": _mean_finite( - rows, "final_rotation_err_deg" - ), - "mean_waypoint_pos_err_mm_mean": _mean_finite( - rows, "mean_waypoint_pos_err_mm" - ), - "max_waypoint_pos_err_mm_mean": _mean_finite( - rows, "max_waypoint_pos_err_mm" - ), - "mean_waypoint_rot_err_deg_mean": _mean_finite( - rows, "mean_waypoint_rot_err_deg" - ), - "max_waypoint_rot_err_deg_mean": _mean_finite( - rows, "max_waypoint_rot_err_deg" - ), - } - ) - return sorted( - summaries, - key=lambda row: ( - int(row["num_waypoints"]), - _IMPL_REPORT_ORDER.get(str(row["impl"]), 99), - ), - ) - - -def _format_table(rows: list[dict[str, object]]) -> list[str]: - """Render the small legacy table used by compatibility tests.""" - if not rows: - return ["No data."] - headers = list(rows[0]) - lines = [ - "| " + " | ".join(headers) + " |", - "| " + " | ".join(["---"] * len(headers)) + " |", - ] - lines.extend( - "| " + " | ".join(str(row[header]) for header in headers) + " |" for row in rows - ) - return lines - - -def format_waypoint_grouped_tables( - summary_rows: list[dict[str, object]], columns: tuple[str, ...] -) -> list[str]: - """Render legacy summaries grouped by waypoint count.""" - groups: dict[int, list[dict[str, object]]] = defaultdict(list) - for row in summary_rows: - groups[int(row["num_waypoints"])].append(row) - lines: list[str] = [] - for group_index, waypoint_count in enumerate(sorted(groups)): - if group_index: - lines.append("") - rows = sorted( - groups[waypoint_count], - key=lambda row: _IMPL_REPORT_ORDER.get(str(row["impl"]), 99), - ) - lines.extend([f"### num_waypoints = {waypoint_count}", ""]) - lines.extend( - _format_table([{column: row[column] for column in columns} for row in rows]) - ) - return lines or ["No data."] diff --git a/scripts/benchmark/motion_generation/metrics/trajectory.py b/scripts/benchmark/motion_generation/metrics/trajectory.py index 3f898c3d6..94660aeae 100644 --- a/scripts/benchmark/motion_generation/metrics/trajectory.py +++ b/scripts/benchmark/motion_generation/metrics/trajectory.py @@ -184,7 +184,7 @@ def compute_waypoint_errors( position_threshold_m: float = 0.05, rotation_threshold_rad: float = 0.3, ) -> dict[str, float]: - """Return ordered, same-sample waypoint errors for compatibility callers.""" + """Return ordered, same-sample waypoint errors for one trajectory.""" if isinstance(trajectory_poses, list): trajectory_tensor = ( torch.stack(trajectory_poses) diff --git a/scripts/benchmark/motion_generation/planners/curobo.py b/scripts/benchmark/motion_generation/planners/curobo.py index 78dffd91c..87595ae1d 100644 --- a/scripts/benchmark/motion_generation/planners/curobo.py +++ b/scripts/benchmark/motion_generation/planners/curobo.py @@ -138,6 +138,7 @@ def close(self) -> None: close_fn = getattr(self.motion_generator.planner, "close", None) if close_fn is not None: close_fn() + self.motion_generator = None register_planner_adapter("curobo", CuroboAdapter) diff --git a/scripts/benchmark/motion_generation/planners/toppra.py b/scripts/benchmark/motion_generation/planners/toppra.py index cdf6662bc..7bc4a4a70 100644 --- a/scripts/benchmark/motion_generation/planners/toppra.py +++ b/scripts/benchmark/motion_generation/planners/toppra.py @@ -87,5 +87,13 @@ def plan(self, case: BenchmarkCase) -> PlanResult: ), ) + def close(self) -> None: + """Release TOPPRA worker pools and drop the motion generator.""" + if self.motion_generator is not None: + close_fn = getattr(self.motion_generator.planner, "close", None) + if close_fn is not None: + close_fn() + self.motion_generator = None + register_planner_adapter("toppra", ToppraAdapter) diff --git a/scripts/benchmark/motion_generation/reporting.py b/scripts/benchmark/motion_generation/reporting.py index 1617fe1b2..6518e7163 100644 --- a/scripts/benchmark/motion_generation/reporting.py +++ b/scripts/benchmark/motion_generation/reporting.py @@ -61,7 +61,6 @@ "success_rate", "planning_success_rate", "ordered_waypoint_success_rate", - "motion_valid_rate", "waypoint_completion_rate", "final_pos_err_mm", "final_rot_err_deg", diff --git a/scripts/benchmark/motion_generation/run_benchmark.py b/scripts/benchmark/motion_generation/run_benchmark.py index bf23616a8..4a9ff9ee6 100644 --- a/scripts/benchmark/motion_generation/run_benchmark.py +++ b/scripts/benchmark/motion_generation/run_benchmark.py @@ -30,37 +30,17 @@ from pathlib import Path from typing import TYPE_CHECKING -from .compat import ( - IMPL_IK, - IMPL_NEURAL, - IMPL_TOPPRA, - QUALITY_SUMMARY_COLUMNS, - aggregate_legacy_rows, - format_waypoint_grouped_tables, -) from .config import PlannerSpecCfg, SuiteCfg, load_suite -from .metrics.trajectory import compute_waypoint_errors, get_pose_err if TYPE_CHECKING: from .runner import BenchmarkRunResult __all__ = [ - "IMPL_IK", - "IMPL_NEURAL", - "IMPL_TOPPRA", - "QUALITY_SUMMARY_COLUMNS", - "_aggregate_rows", - "_format_waypoint_grouped_tables", "add_parser_arguments", - "compute_waypoint_errors", - "get_pose_err", "run_all_benchmarks", "run_from_args", ] -_aggregate_rows = aggregate_legacy_rows -_format_waypoint_grouped_tables = format_waypoint_grouped_tables - def add_parser_arguments(parser: argparse.ArgumentParser) -> None: """Add free-space benchmark options to an existing argument parser.""" @@ -90,6 +70,8 @@ def add_parser_arguments(parser: argparse.ArgumentParser) -> None: ) parser.add_argument("--batch-sizes", nargs="+", type=int, default=None) parser.add_argument("--num-waypoints", nargs="+", type=int, default=None) + parser.add_argument("--path-shapes", nargs="+", default=None) + parser.add_argument("--start-state-bins", nargs="+", default=None) parser.add_argument("--seeds", nargs="+", type=int, default=None) parser.add_argument("--num-trials", type=int, default=None) parser.add_argument("--warmup-trials", type=int, default=None) @@ -114,16 +96,6 @@ def add_parser_arguments(parser: argparse.ArgumentParser) -> None: default=None, help="Reserved NMG checkpoint path; the current NMG adapter remains a stub.", ) - parser.add_argument( - "--compare-ik", - action="store_true", - help="Compatibility alias for --extra-baselines ik_interpolate.", - ) - parser.add_argument( - "--compare-toppra", - action="store_true", - help="Compatibility alias for --extra-baselines toppra.", - ) parser.add_argument( "--output-root", default="outputs/benchmarks", help="Artifact root directory." ) @@ -133,11 +105,6 @@ def add_parser_arguments(parser: argparse.ArgumentParser) -> None: parser.add_argument( "--no-headless", action="store_false", dest="headless", help="Open a viewer." ) - parser.add_argument( - "--save-trial-details", - action="store_true", - help="Deprecated: numeric trial details are always saved to trials.jsonl.", - ) def _planner_by_id(suite: SuiteCfg, planner_id: str) -> PlannerSpecCfg: @@ -176,6 +143,8 @@ def _apply_overrides( *, batch_sizes: list[int] | None = None, num_waypoints: list[int] | None = None, + path_shapes: list[str] | None = None, + start_state_bins: list[str] | None = None, seeds: list[int] | None = None, num_trials: int | None = None, warmup_trials: int | None = None, @@ -192,6 +161,10 @@ def _apply_overrides( suite.free_space.batch_sizes = batch_sizes if num_waypoints is not None: suite.free_space.waypoint_counts = num_waypoints + if path_shapes is not None: + suite.free_space.path_shapes = path_shapes + if start_state_bins is not None: + suite.free_space.start_state_bins = start_state_bins if seeds is not None: suite.free_space.seeds = seeds if num_trials is not None: @@ -228,6 +201,8 @@ def run_all_benchmarks( algorithms: list[str] | None = None, extra_baselines: list[str] | None = None, batch_sizes: list[int] | None = None, + path_shapes: list[str] | None = None, + start_state_bins: list[str] | None = None, seeds: list[int] | None = None, num_trials: int | None = None, warmup_trials: int | None = None, @@ -237,9 +212,6 @@ def run_all_benchmarks( rotation_threshold_rad: float | None = None, nmg_pos_eps: float | None = None, nmg_rot_eps: float | None = None, - compare_ik: bool = False, - compare_toppra: bool = False, - include_trial_details: bool = False, # noqa: ARG001 - compatibility parameter output_root: str | Path = "outputs/benchmarks", ) -> BenchmarkRunResult: """Resolve configuration and run all selected free-space benchmarks.""" @@ -250,6 +222,8 @@ def run_all_benchmarks( suite, batch_sizes=batch_sizes, num_waypoints=num_waypoints_list, + path_shapes=path_shapes, + start_state_bins=start_state_bins, seeds=seeds, num_trials=num_trials, warmup_trials=warmup_trials, @@ -261,12 +235,7 @@ def run_all_benchmarks( nmg_rot_eps=nmg_rot_eps, checkpoint_path=checkpoint_path, ) - extras = list(extra_baselines or []) - if compare_ik and "ik_interpolate" not in extras: - extras.append("ik_interpolate") - if compare_toppra and "toppra" not in extras: - extras.append("toppra") - specs = _resolve_planners(suite, algorithms, extras) + specs = _resolve_planners(suite, algorithms, list(extra_baselines or [])) return BenchmarkRunner( suite, specs, @@ -287,6 +256,8 @@ def run_from_args(args: argparse.Namespace) -> BenchmarkRunResult: algorithms=args.algorithms, extra_baselines=args.extra_baselines, batch_sizes=args.batch_sizes, + path_shapes=args.path_shapes, + start_state_bins=args.start_state_bins, seeds=args.seeds, num_trials=args.num_trials, warmup_trials=args.warmup_trials, @@ -296,9 +267,6 @@ def run_from_args(args: argparse.Namespace) -> BenchmarkRunResult: rotation_threshold_rad=args.rotation_threshold_rad, nmg_pos_eps=args.nmg_pos_eps, nmg_rot_eps=args.nmg_rot_eps, - compare_ik=args.compare_ik, - compare_toppra=args.compare_toppra, - include_trial_details=args.save_trial_details, output_root=args.output_root, ) diff --git a/scripts/benchmark/motion_generation/scenarios/free_space.py b/scripts/benchmark/motion_generation/scenarios/free_space.py index d8d853a04..2dafaf9d5 100644 --- a/scripts/benchmark/motion_generation/scenarios/free_space.py +++ b/scripts/benchmark/motion_generation/scenarios/free_space.py @@ -51,7 +51,12 @@ def _start_qpos_for_bin( limits: torch.Tensor, generator: torch.Generator, ) -> torch.Tensor: - """Create one deterministic start posture for a named condition bin.""" + """Create one deterministic start posture for a named condition bin. + + ``near_singularity`` uses a fixed elbow-extended Franka posture. It is a + reproducible low-manipulability seed for free-space-common v1, not a + runtime singularity search. + """ lower, upper = limits[:, 0], limits[:, 1] midpoint = (lower + upper) * 0.5 span = upper - lower @@ -70,6 +75,7 @@ def _start_qpos_for_bin( ).to(limits) return _clamp_with_margin(midpoint + signs * span * 0.42, limits) if name == "near_singularity": + # Elbow nearly extended (q3≈0): a fixed Franka near-singularity seed. candidate = torch.tensor( [0.0, 0.0, 0.0, -0.15, 0.0, 0.20, 0.0], dtype=limits.dtype ).to(limits) @@ -196,7 +202,9 @@ def _build_case( class FreeSpaceScenario(ScenarioProvider): required_capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) - def batch_sizes(self, suite: SuiteCfg, track: TrackCfg) -> list[int]: # noqa: ARG002 + def batch_sizes( + self, suite: SuiteCfg, track: TrackCfg + ) -> list[int]: # noqa: ARG002 return list(suite.free_space.batch_sizes) def generate_cases( diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index dec891d5f..bde9addf2 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -29,6 +29,7 @@ from scripts.benchmark.motion_generation.config import load_suite from scripts.benchmark.motion_generation.metrics.trajectory import ( compute_case_outcomes, + compute_waypoint_errors, match_ordered_waypoints, ) from scripts.benchmark.motion_generation import ( @@ -60,6 +61,25 @@ def _translated_pose(x: float) -> torch.Tensor: return pose +def test_compute_waypoint_errors_uses_ordered_trajectory_hits(): + waypoints = torch.stack( + [ + torch.eye(4), + torch.tensor( + [ + [1.0, 0.0, 0.0, 0.1], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ), + ] + ) + errors = compute_waypoint_errors([torch.eye(4), waypoints[1]], waypoints) + assert errors["mean_waypoint_pos_err_mm"] == pytest.approx(0.0) + assert errors["max_waypoint_pos_err_mm"] == pytest.approx(0.0) + + def test_ordered_waypoints_reject_out_of_order_hits(): waypoints = torch.stack([_translated_pose(0.1), _translated_pose(0.0)]) trajectory = torch.stack([_translated_pose(0.0), _translated_pose(0.1)]) @@ -178,7 +198,7 @@ def test_top_failure_ignores_planner_internal_codes_when_motion_valid(): aggregates = aggregate_results([measured], metadata, [case], measured_trials=1) row = aggregates["success_and_metrics"][0] - assert row["motion_valid_rate"] == pytest.approx(1.0) + assert row["success_rate"] == pytest.approx(1.0) assert row["planning_success_rate"] == pytest.approx(0.0) assert row["top_failure"] is None assert row["start_state_bin"] == "nominal" @@ -380,8 +400,8 @@ def test_success_metrics_are_stratified_by_start_state_bin(): by_bin = {row["start_state_bin"]: row for row in rows} assert set(by_bin) == {"nominal", "near_limit"} - assert by_bin["nominal"]["motion_valid_rate"] == pytest.approx(1.0) - assert by_bin["near_limit"]["motion_valid_rate"] == pytest.approx(0.0) + assert by_bin["nominal"]["success_rate"] == pytest.approx(1.0) + assert by_bin["near_limit"]["success_rate"] == pytest.approx(0.0) assert by_bin["near_limit"]["top_failure"] == "waypoint_miss" @@ -522,3 +542,68 @@ def test_curobo_prepare_backend_exposes_actual_graph_mode(): planner._get_backend.assert_called_once_with("arm", 8, MoveType.EEF_MOVE) assert result["use_cuda_graph"] is False assert result["batch_size"] == 8 + + +def test_metric_rows_use_success_rate_and_null_peak_gpu(): + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ) + ] + measured = TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-1", + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=11, + repeat=0, + batch_size=1, + waypoint_count=1, + path_shape="direct", + start_state_bin="nominal", + phase=TrialPhase.MEASURED, + cost_time_ms=10.0, + peak_gpu_mb=None, + outcomes=(_outcome(),), + ) + aggregates = aggregate_results([measured], metadata, [_case()], measured_trials=1) + + assert aggregates["success_and_metrics"][0]["success_rate"] == pytest.approx(1.0) + assert "motion_valid_rate" not in aggregates["success_and_metrics"][0] + assert aggregates["time_and_memory"][0]["peak_gpu_mb"] is None + assert aggregates["leaderboard"][0]["peak_gpu_mb"] is None + + +def test_toppra_adapter_close_releases_planner(): + from scripts.benchmark.motion_generation.config import PlannerSpecCfg + from scripts.benchmark.motion_generation.planners.base import PlannerContext + from scripts.benchmark.motion_generation.planners.toppra import ToppraAdapter + + planner = Mock() + adapter = ToppraAdapter( + PlannerSpecCfg( + id="toppra", + adapter="toppra", + role=AlgorithmRole.DIAGNOSTIC_BASELINE.value, + ), + PlannerContext( + robot=Mock(), + control_part="arm", + device=torch.device("cpu"), + sample_interval=40, + ), + ) + adapter.motion_generator = Mock(planner=planner) + + adapter.close() + + planner.close.assert_called_once_with() + assert adapter.motion_generator is None diff --git a/tests/benchmark/planners/test_neural_planner_benchmark.py b/tests/benchmark/planners/test_neural_planner_benchmark.py deleted file mode 100644 index 70f74b08c..000000000 --- a/tests/benchmark/planners/test_neural_planner_benchmark.py +++ /dev/null @@ -1,161 +0,0 @@ -# ---------------------------------------------------------------------------- -# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# ---------------------------------------------------------------------------- - -"""Smoke tests for NeuralPlanner benchmark aggregation and reporting.""" - -from __future__ import annotations - -import pytest -import torch - -from scripts.benchmark.motion_generation.run_benchmark import ( - IMPL_IK, - IMPL_NEURAL, - IMPL_TOPPRA, - QUALITY_SUMMARY_COLUMNS, - _aggregate_rows, - _format_waypoint_grouped_tables, - compute_waypoint_errors, -) - - -def test_compute_waypoint_errors_uses_best_trajectory_hit(): - waypoints = torch.stack( - [ - torch.eye(4), - torch.tensor( - [ - [1.0, 0.0, 0.0, 0.1], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0], - [0.0, 0.0, 0.0, 1.0], - ] - ), - ] - ) - trajectory_poses = [torch.eye(4), waypoints[1]] - errors = compute_waypoint_errors(trajectory_poses, waypoints) - assert errors["mean_waypoint_pos_err_mm"] == pytest.approx(0.0) - assert errors["max_waypoint_pos_err_mm"] == pytest.approx(0.0) - - -def test_aggregate_rows_excludes_warmup_and_computes_p95(): - trial_rows = [ - { - "impl": IMPL_NEURAL, - "num_waypoints": 3, - "warmup": True, - "cost_time_ms": "999.0", - "success": True, - "final_translation_err_mm": "0.0", - "final_rotation_err_deg": "0.0", - "mean_waypoint_pos_err_mm": "0.0", - "max_waypoint_pos_err_mm": "0.0", - "mean_waypoint_rot_err_deg": "0.0", - "max_waypoint_rot_err_deg": "0.0", - "rollout_steps": 1, - "cpu_delta_mb": "0.0", - "gpu_delta_mb": "0.0", - "peak_gpu_mb": "1.0", - }, - { - "impl": IMPL_NEURAL, - "num_waypoints": 3, - "warmup": False, - "cost_time_ms": "10.0", - "success": True, - "final_translation_err_mm": "1.0", - "final_rotation_err_deg": "2.0", - "mean_waypoint_pos_err_mm": "3.0", - "max_waypoint_pos_err_mm": "4.0", - "mean_waypoint_rot_err_deg": "5.0", - "max_waypoint_rot_err_deg": "6.0", - "rollout_steps": 5, - "cpu_delta_mb": "1.0", - "gpu_delta_mb": "2.0", - "peak_gpu_mb": "3.0", - }, - { - "impl": IMPL_NEURAL, - "num_waypoints": 3, - "warmup": False, - "cost_time_ms": "20.0", - "success": False, - "final_translation_err_mm": "7.0", - "final_rotation_err_deg": "8.0", - "mean_waypoint_pos_err_mm": "9.0", - "max_waypoint_pos_err_mm": "10.0", - "mean_waypoint_rot_err_deg": "11.0", - "max_waypoint_rot_err_deg": "12.0", - "rollout_steps": 6, - "cpu_delta_mb": "3.0", - "gpu_delta_mb": "4.0", - "peak_gpu_mb": "5.0", - }, - ] - row = _aggregate_rows(trial_rows)[0] - assert row["num_trials"] == 2 - assert row["success_rate"] == "50.00%" - assert float(row["cost_time_ms_mean"]) == pytest.approx(15.0) - assert float(row["cost_time_ms_p95"]) == pytest.approx(20.0) - assert float(row["mean_waypoint_pos_err_mm_mean"]) == pytest.approx(6.0) - - -def test_format_waypoint_grouped_tables_splits_by_num_waypoints(): - summary_rows = [ - { - "impl": IMPL_IK, - "num_waypoints": 3, - "num_trials": 8, - "success_rate": "100.00%", - "final_translation_err_mm_mean": "1.0", - "final_rotation_err_deg_mean": "0.0", - "mean_waypoint_pos_err_mm_mean": "1.0", - "max_waypoint_pos_err_mm_mean": "1.0", - "mean_waypoint_rot_err_deg_mean": "0.0", - "max_waypoint_rot_err_deg_mean": "0.0", - }, - { - "impl": IMPL_NEURAL, - "num_waypoints": 1, - "num_trials": 8, - "success_rate": "100.00%", - "final_translation_err_mm_mean": "2.0", - "final_rotation_err_deg_mean": "0.0", - "mean_waypoint_pos_err_mm_mean": "2.0", - "max_waypoint_pos_err_mm_mean": "2.0", - "mean_waypoint_rot_err_deg_mean": "0.0", - "max_waypoint_rot_err_deg_mean": "0.0", - }, - { - "impl": IMPL_TOPPRA, - "num_waypoints": 3, - "num_trials": 8, - "success_rate": "100.00%", - "final_translation_err_mm_mean": "0.5", - "final_rotation_err_deg_mean": "0.0", - "mean_waypoint_pos_err_mm_mean": "0.5", - "max_waypoint_pos_err_mm_mean": "0.5", - "mean_waypoint_rot_err_deg_mean": "0.0", - "max_waypoint_rot_err_deg_mean": "0.0", - }, - ] - text = "\n".join( - _format_waypoint_grouped_tables(summary_rows, QUALITY_SUMMARY_COLUMNS) - ) - assert text.index("### num_waypoints = 1") < text.index("### num_waypoints = 3") - assert "| num_waypoints |" not in text - assert text.index(IMPL_NEURAL) < text.index(IMPL_IK) From 54d5854c3411ee02249dfda7c015207e6a89df59 Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 03:16:42 +0800 Subject: [PATCH 09/17] Use case-level macro average for leaderboard success --- .../motion_generation/aggregation.py | 45 ++++++- scripts/benchmark/motion_generation/runner.py | 3 + .../test_motion_generation_benchmark.py | 117 ++++++++++++++++++ 3 files changed, 161 insertions(+), 4 deletions(-) diff --git a/scripts/benchmark/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py index 155104282..228a73332 100644 --- a/scripts/benchmark/motion_generation/aggregation.py +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -58,6 +58,38 @@ def _rate(values: Iterable[bool]) -> float | None: return sum(materialized) / len(materialized) if materialized else None +def _case_macro_rate( + measured: list[TrialRecord], + track_cases: list[BenchmarkCase], + attribute: str, +) -> float: + """Macro-average a boolean outcome attribute equally across mandatory cases. + + Within each case, env rows (and measured repeats) are micro-averaged first. + Cases then receive equal weight regardless of ``batch_size``, so a B=64 + case cannot dominate a B=1 case on the leaderboard. Missing cases + contribute ``0.0`` so selective skipping cannot inflate the rate. + """ + if not track_cases: + return 0.0 + + 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: + outcomes = outcomes_by_case.get(case.case_id, []) + if not outcomes: + case_rates.append(0.0) + continue + case_rates.append( + sum(bool(getattr(outcome, attribute)) for outcome in outcomes) + / len(outcomes) + ) + return sum(case_rates) / len(case_rates) + + def _top_failure(outcomes: list[CaseOutcome]) -> str | None: """Return the most frequent non-empty external failure code.""" failures = Counter( @@ -329,7 +361,12 @@ def _leaderboard_rows( cases: list[BenchmarkCase], measured_trials: int, ) -> list[dict[str, object]]: - """Build a complete success/coverage/latency ordered leaderboard per track.""" + """Build a complete success/coverage/latency ordered leaderboard per track. + + Success rates are macro-averaged over mandatory cases (equal case weight). + ``coverage_rate`` remains an outcome-count completeness check used for + eligibility. + """ entries: list[dict[str, object]] = [] for track in _track_ids(records, cases): track_cases = [case for case in cases if case.track == track] @@ -347,9 +384,9 @@ def _leaderboard_rows( ] outcomes = [outcome for record in measured for outcome in record.outcomes] coverage = min(1.0, len(outcomes) / max(expected_outcomes, 1)) - motion_rate = _rate(outcome.motion_valid for outcome in outcomes) or 0.0 - planning_rate = ( - _rate(outcome.planning_success for outcome in outcomes) or 0.0 + motion_rate = _case_macro_rate(measured, track_cases, "motion_valid") + planning_rate = _case_macro_rate( + measured, track_cases, "planning_success" ) latency_p95 = _percentile( (record.cost_time_ms for record in measured), 95.0 diff --git a/scripts/benchmark/motion_generation/runner.py b/scripts/benchmark/motion_generation/runner.py index e44474211..b47ce2311 100644 --- a/scripts/benchmark/motion_generation/runner.py +++ b/scripts/benchmark/motion_generation/runner.py @@ -445,6 +445,9 @@ def run(self) -> BenchmarkRunResult: "CPU/GPU memory values are process/PyTorch allocator deltas around timed calls.", "Continuous error and path metrics are conditioned on externally motion-valid trajectories.", "Collision, dynamic, execution, and task metrics are N/A in free-space-common v1.", + "Leaderboard overall_success_rate / motion_valid_rate / planning_success_rate " + "are macro averages over mandatory cases (equal case weight after within-case " + "env/repeat micro-average); coverage_rate remains outcome-count completeness.", *self.notes, ], ) diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index bde9addf2..8621d48c5 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -18,6 +18,7 @@ from __future__ import annotations +from dataclasses import replace from unittest.mock import Mock import pytest @@ -506,6 +507,122 @@ def test_aggregation_excludes_warmup_and_keeps_unavailable_algorithm(): assert nmg["coverage_rate"] == pytest.approx(0.0) +def test_leaderboard_uses_case_macro_average_not_env_micro_average(): + """B=64 failures must not dominate a B=1 success on the leaderboard.""" + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ) + ] + small = BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-b1", + seed=11, + batch_size=1, + num_waypoints=1, + path_shape="direct", + start_state_bin="nominal", + start_qpos=torch.zeros(1, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4), + reference_qpos=torch.zeros(1, 1, 7), + ) + large = BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-b64", + seed=23, + batch_size=64, + num_waypoints=1, + path_shape="direct", + start_state_bin="nominal", + start_qpos=torch.zeros(64, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4).expand(64, 1, 4, 4).clone(), + reference_qpos=torch.zeros(64, 1, 7), + ) + failed = CaseOutcome( + env_index=0, + planning_success=False, + finite=True, + ordered_waypoints_reached=False, + motion_valid=False, + completed_waypoint_ratio=0.0, + final_translation_err_mm=None, + final_rotation_err_deg=None, + waypoint_translation_err_mm_mean=None, + waypoint_translation_err_mm_p95=None, + waypoint_translation_err_mm_max=None, + waypoint_rotation_err_deg_mean=None, + waypoint_rotation_err_deg_p95=None, + waypoint_rotation_err_deg_max=None, + joint_limit_violation=False, + max_normalized_joint_violation=0.0, + joint_path_length_rad=None, + cartesian_path_length_m=None, + path_efficiency=None, + failure_code="waypoint_miss", + ) + records = [ + TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=small.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=small.seed, + repeat=0, + batch_size=1, + waypoint_count=1, + path_shape="direct", + start_state_bin="nominal", + phase=TrialPhase.MEASURED, + cost_time_ms=10.0, + outcomes=(_outcome(),), + ), + TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=large.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=large.seed, + repeat=0, + batch_size=64, + waypoint_count=1, + path_shape="direct", + start_state_bin="nominal", + phase=TrialPhase.MEASURED, + cost_time_ms=40.0, + outcomes=tuple( + replace(failed, env_index=env_index) for env_index in range(64) + ), + ), + ] + + row = aggregate_results(records, metadata, [small, large], measured_trials=1)[ + "leaderboard" + ][0] + + # Env micro-average would be 1/65 ≈ 0.015; case macro-average is 0.5. + assert row["overall_success_rate"] == pytest.approx(0.5) + assert row["motion_valid_rate"] == pytest.approx(0.5) + assert row["planning_success_rate"] == pytest.approx(0.5) + assert row["coverage_rate"] == pytest.approx(1.0) + assert row["eligible"] is True + + def test_report_contains_exactly_three_markdown_tables(tmp_path): suite = load_suite("smoke") aggregates = { From bb151e6b97ab943a82f784bc73f6ff5785c7ed84 Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 03:21:35 +0800 Subject: [PATCH 10/17] Scope cold_plan_ms to matching waypoint rows --- .../motion_generation/aggregation.py | 19 +++++++++++++++++-- scripts/benchmark/motion_generation/runner.py | 3 +++ 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/scripts/benchmark/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py index 228a73332..31308320f 100644 --- a/scripts/benchmark/motion_generation/aggregation.py +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -121,14 +121,21 @@ def _lifecycle_value( algorithm_id: str, batch_size: int, phase: TrialPhase, + waypoint_count: int | None = None, ) -> float | None: - """Return the first lifecycle cost for one track, algorithm, and batch size.""" + """Return the first lifecycle cost for one track, algorithm, and batch size. + + When ``waypoint_count`` is provided, only records for that waypoint shape + match. This keeps first-case ``cold_plan_ms`` from being reused on every + waypoint row in the Time & Memory table. + """ for record in records: if ( record.track == track and record.algorithm_id == algorithm_id and record.batch_size == batch_size and record.phase is phase + and (waypoint_count is None or record.waypoint_count == waypoint_count) ): return record.cost_time_ms return None @@ -170,14 +177,22 @@ def _performance_rows( "batch_size": batch_size, "waypoint_count": waypoint_count, "num_trials": len(group), + # Construct/prepare are one-time deployment costs for the batch. "planner_construct_ms": _lifecycle_value( records, track, algorithm_id, batch_size, TrialPhase.CONSTRUCT ), "backend_prepare_ms": _lifecycle_value( records, track, algorithm_id, batch_size, TrialPhase.PREPARE ), + # Cold plan is the first real case only; attach it to that + # waypoint shape rather than repeating it on every W row. "cold_plan_ms": _lifecycle_value( - records, track, algorithm_id, batch_size, TrialPhase.COLD + records, + track, + algorithm_id, + batch_size, + TrialPhase.COLD, + waypoint_count=waypoint_count, ), "cost_time_ms": mean_cost, "warm_plan_ms_p50": _percentile(costs, 50.0), diff --git a/scripts/benchmark/motion_generation/runner.py b/scripts/benchmark/motion_generation/runner.py index b47ce2311..17f23e4ab 100644 --- a/scripts/benchmark/motion_generation/runner.py +++ b/scripts/benchmark/motion_generation/runner.py @@ -448,6 +448,9 @@ def run(self) -> BenchmarkRunResult: "Leaderboard overall_success_rate / motion_valid_rate / planning_success_rate " "are macro averages over mandatory cases (equal case weight after within-case " "env/repeat micro-average); coverage_rate remains outcome-count completeness.", + "cold_plan_ms is reported only on the Time & Memory row whose waypoint_count " + "matches the first real case measured for that batch; other waypoint rows " + "show N/A. planner_construct_ms / backend_prepare_ms are one-time batch costs.", *self.notes, ], ) From 41dfcd90778cb4deaa0b6782e831b2c2dd79d22d Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 03:21:47 +0800 Subject: [PATCH 11/17] Add test --- .../test_motion_generation_benchmark.py | 114 ++++++++++++++++++ 1 file changed, 114 insertions(+) diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index 8621d48c5..096244cdc 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -623,6 +623,120 @@ def test_leaderboard_uses_case_macro_average_not_env_micro_average(): assert row["eligible"] is True +def test_cold_plan_ms_only_attaches_to_matching_waypoint_row(): + """Cold latency from W=1 must not be copied onto W=5 Time & Memory rows.""" + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ) + ] + case_w1 = BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-w1", + seed=11, + batch_size=1, + num_waypoints=1, + path_shape="direct", + start_state_bin="nominal", + start_qpos=torch.zeros(1, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4), + reference_qpos=torch.zeros(1, 1, 7), + ) + case_w5 = BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-w5", + seed=11, + batch_size=1, + num_waypoints=5, + path_shape="direct", + start_state_bin="nominal", + start_qpos=torch.zeros(1, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4).expand(1, 5, 4, 4).clone(), + reference_qpos=torch.zeros(1, 5, 7), + ) + + def _measured(case: BenchmarkCase, cost: float) -> TrialRecord: + return TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=case.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=case.seed, + repeat=0, + batch_size=case.batch_size, + waypoint_count=case.num_waypoints, + path_shape=case.path_shape, + start_state_bin=case.start_state_bin, + phase=TrialPhase.MEASURED, + cost_time_ms=cost, + outcomes=(_outcome(),), + ) + + records = [ + TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=case_w1.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=case_w1.seed, + repeat=-1, + batch_size=1, + waypoint_count=1, + path_shape="direct", + start_state_bin="nominal", + phase=TrialPhase.COLD, + cost_time_ms=123.0, + ), + TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=case_w1.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=case_w1.seed, + repeat=-1, + batch_size=1, + waypoint_count=1, + path_shape="direct", + start_state_bin="nominal", + phase=TrialPhase.CONSTRUCT, + cost_time_ms=50.0, + ), + _measured(case_w1, 10.0), + _measured(case_w5, 20.0), + ] + + rows = aggregate_results(records, metadata, [case_w1, case_w5], measured_trials=1)[ + "time_and_memory" + ] + by_waypoints = {row["waypoint_count"]: row for row in rows} + + assert by_waypoints[1]["cold_plan_ms"] == pytest.approx(123.0) + assert by_waypoints[5]["cold_plan_ms"] is None + # One-time construct cost remains visible on every batch row. + assert by_waypoints[1]["planner_construct_ms"] == pytest.approx(50.0) + assert by_waypoints[5]["planner_construct_ms"] == pytest.approx(50.0) + + def test_report_contains_exactly_three_markdown_tables(tmp_path): suite = load_suite("smoke") aggregates = { From b526989ab0c5a15a15d909251f00a3e708df9068 Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 03:43:31 +0800 Subject: [PATCH 12/17] WIP --- .../motion_generation/aggregation.py | 81 ++++-- .../motion_generation/metrics/performance.py | 2 +- .../motion_generation/metrics/trajectory.py | 50 +--- scripts/benchmark/motion_generation/runner.py | 47 +++- .../motion_generation/scenarios/free_space.py | 2 +- .../test_motion_generation_benchmark.py | 257 ++++++++++++++++++ 6 files changed, 365 insertions(+), 74 deletions(-) diff --git a/scripts/benchmark/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py index 31308320f..e45968a10 100644 --- a/scripts/benchmark/motion_generation/aggregation.py +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -52,12 +52,6 @@ def _percentile(values: Iterable[float | None], percentile: float) -> float | No return finite[index] -def _rate(values: Iterable[bool]) -> float | None: - """Return a boolean rate or ``None`` for an empty sequence.""" - materialized = list(values) - return sum(materialized) / len(materialized) if materialized else None - - def _case_macro_rate( measured: list[TrialRecord], track_cases: list[BenchmarkCase], @@ -90,6 +84,32 @@ def _case_macro_rate( return sum(case_rates) / len(case_rates) +def _case_macro_mean( + measured: list[TrialRecord], + track_cases: list[BenchmarkCase], + attribute: str, +) -> float | None: + """Macro-average a numeric outcome attribute across cases with values.""" + 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_means: list[float] = [] + for case in track_cases: + values = [ + float(getattr(outcome, attribute)) + for outcome in outcomes_by_case.get(case.case_id, []) + if getattr(outcome, attribute) is not None + and math.isfinite(float(getattr(outcome, attribute))) + ] + if values: + case_means.append(sum(values) / len(values)) + return sum(case_means) / len(case_means) if case_means else None + + def _top_failure(outcomes: list[CaseOutcome]) -> str | None: """Return the most frequent non-empty external failure code.""" failures = Counter( @@ -258,9 +278,14 @@ def _metric_rows( cases: list[BenchmarkCase], measured_trials: int, ) -> list[dict[str, object]]: - """Aggregate external success and quality metrics by scenario condition.""" - outcome_groups: dict[ - tuple[str, str, str, int, int, str, str], list[CaseOutcome] + """Aggregate external success and quality metrics by scenario condition. + + Boolean success columns use the same case-level macro average as the + leaderboard (missing cases contribute ``0.0``). Continuous quality metrics + remain conditioned on externally motion-valid outcomes. + """ + measured_by_key: dict[ + tuple[str, str, str, int, int, str, str], list[TrialRecord] ] = defaultdict(list) for record in records: if record.phase is not TrialPhase.MEASURED: @@ -274,10 +299,12 @@ def _metric_rows( record.path_shape, record.start_state_bin, ) - outcome_groups[key].extend(record.outcomes) + measured_by_key[key].append(record) expected_by_group: Counter[tuple[str, str, int, int, str, str]] = Counter() - unique_cases_by_group: Counter[tuple[str, str, int, int, str, str]] = Counter() + cases_by_group: dict[tuple[str, str, int, int, str, str], list[BenchmarkCase]] = ( + defaultdict(list) + ) for case in cases: key = ( case.track, @@ -288,7 +315,7 @@ def _metric_rows( case.start_state_bin, ) expected_by_group[key] += case.batch_size * measured_trials - unique_cases_by_group[key] += case.batch_size + cases_by_group[key].append(case) rows: list[dict[str, object]] = [] for info in metadata: @@ -301,7 +328,8 @@ def _metric_rows( path_shape, start_state_bin, ) = group_key - outcomes = outcome_groups.get( + group_cases = cases_by_group[group_key] + measured = measured_by_key.get( ( track, info.algorithm_id, @@ -313,6 +341,7 @@ def _metric_rows( ), [], ) + outcomes = [outcome for record in measured for outcome in record.outcomes] valid_outcomes = [outcome for outcome in outcomes if outcome.motion_valid] expected = expected_by_group[group_key] rows.append( @@ -325,18 +354,20 @@ def _metric_rows( "waypoint_count": waypoint_count, "path_shape": path_shape, "start_state_bin": start_state_bin, - "cases": unique_cases_by_group[group_key], + "cases": len(group_cases), "coverage_rate": min(1.0, len(outcomes) / max(expected, 1)), # Free-space primary success is external motion validity. - "success_rate": _rate(outcome.motion_valid for outcome in outcomes), - "planning_success_rate": _rate( - outcome.planning_success for outcome in outcomes + "success_rate": _case_macro_rate( + measured, group_cases, "motion_valid" ), - "ordered_waypoint_success_rate": _rate( - outcome.ordered_waypoints_reached for outcome in outcomes + "planning_success_rate": _case_macro_rate( + measured, group_cases, "planning_success" ), - "waypoint_completion_rate": _mean( - outcome.completed_waypoint_ratio for outcome in outcomes + "ordered_waypoint_success_rate": _case_macro_rate( + measured, group_cases, "ordered_waypoints_reached" + ), + "waypoint_completion_rate": _case_macro_mean( + measured, group_cases, "completed_waypoint_ratio" ), "final_pos_err_mm": _mean( outcome.final_translation_err_mm for outcome in valid_outcomes @@ -352,8 +383,8 @@ def _metric_rows( outcome.waypoint_rotation_err_deg_p95 for outcome in valid_outcomes ), - "joint_violation_rate": _rate( - outcome.joint_limit_violation for outcome in outcomes + "joint_violation_rate": _case_macro_rate( + measured, group_cases, "joint_limit_violation" ), "joint_path_length_rad": _mean( outcome.joint_path_length_rad for outcome in valid_outcomes @@ -400,9 +431,7 @@ def _leaderboard_rows( outcomes = [outcome for record in measured for outcome in record.outcomes] coverage = min(1.0, len(outcomes) / max(expected_outcomes, 1)) motion_rate = _case_macro_rate(measured, track_cases, "motion_valid") - planning_rate = _case_macro_rate( - measured, track_cases, "planning_success" - ) + planning_rate = _case_macro_rate(measured, track_cases, "planning_success") latency_p95 = _percentile( (record.cost_time_ms for record in measured), 95.0 ) diff --git a/scripts/benchmark/motion_generation/metrics/performance.py b/scripts/benchmark/motion_generation/metrics/performance.py index 692ec29d3..7b36b01b7 100644 --- a/scripts/benchmark/motion_generation/metrics/performance.py +++ b/scripts/benchmark/motion_generation/metrics/performance.py @@ -74,7 +74,7 @@ def timed_call(callable_fn: Callable[[], _T]) -> TimedCall[_T]: peak_gpu_mb = ( torch.cuda.max_memory_allocated() / 1024**2 if torch.cuda.is_available() - else 0.0 + else None ) return TimedCall( result=result, diff --git a/scripts/benchmark/motion_generation/metrics/trajectory.py b/scripts/benchmark/motion_generation/metrics/trajectory.py index 94660aeae..5f39bd699 100644 --- a/scripts/benchmark/motion_generation/metrics/trajectory.py +++ b/scripts/benchmark/motion_generation/metrics/trajectory.py @@ -88,36 +88,6 @@ def get_pose_err(matrix_a: torch.Tensor, matrix_b: torch.Tensor) -> tuple[float, return float(translation.mean().item()), float(rotation.mean().item()) -def _minimum_cost_monotonic_indices(cost: torch.Tensor) -> list[int]: - """Match each waypoint to one strictly later sample with minimum total cost.""" - waypoint_count, sample_count = cost.shape - if waypoint_count == 0 or sample_count == 0 or sample_count < waypoint_count: - return [] - - dp = torch.full_like(cost, float("inf")) - parents = torch.full( - (waypoint_count, sample_count), -1, dtype=torch.long, device=cost.device - ) - dp[0] = cost[0] - for waypoint_index in range(1, waypoint_count): - for sample_index in range(waypoint_index, sample_count): - previous = dp[waypoint_index - 1, :sample_index] - best_cost, best_index = torch.min(previous, dim=0) - dp[waypoint_index, sample_index] = ( - best_cost + cost[waypoint_index, sample_index] - ) - parents[waypoint_index, sample_index] = best_index - - final_index = int(torch.argmin(dp[-1]).item()) - if not torch.isfinite(dp[-1, final_index]): - return [] - indices = [final_index] - for waypoint_index in range(waypoint_count - 1, 0, -1): - final_index = int(parents[waypoint_index, final_index].item()) - indices.append(final_index) - return list(reversed(indices)) - - def match_ordered_waypoints( trajectory_poses: torch.Tensor, waypoints: torch.Tensor, @@ -125,7 +95,14 @@ def match_ordered_waypoints( position_threshold_m: float, rotation_threshold_rad: float, ) -> dict[str, object]: - """Evaluate ordered arrival and joint position/rotation waypoint errors.""" + """Evaluate ordered arrival and threshold-constrained waypoint errors. + + Success and continuous waypoint errors share the same greedy matching: + each waypoint must be hit after the previous arrival by a sample that + jointly satisfies the position and rotation thresholds. Reported errors + are taken at those arrival samples so ``motion_valid`` cannot disagree + with waypoint p95/max exceeding the external thresholds. + """ trajectory_poses = torch.as_tensor(trajectory_poses) waypoints = torch.as_tensor(waypoints) if trajectory_poses.numel() == 0 or waypoints.numel() == 0: @@ -153,17 +130,13 @@ def match_ordered_waypoints( arrival_indices.append(sample_index) next_sample = sample_index + 1 - normalized_cost = ( - pos_error / position_threshold_m + rot_error / rotation_threshold_rad - ) - matched_indices = _minimum_cost_monotonic_indices(normalized_cost) position_errors = [ float(pos_error[index, sample].item()) - for index, sample in enumerate(matched_indices) + for index, sample in enumerate(arrival_indices) ] rotation_errors = [ float(rot_error[index, sample].item()) - for index, sample in enumerate(matched_indices) + for index, sample in enumerate(arrival_indices) ] completed = len(arrival_indices) total = int(waypoints.shape[0]) @@ -171,7 +144,8 @@ def match_ordered_waypoints( "ordered_waypoints_reached": completed == total, "completed_waypoint_ratio": completed / max(total, 1), "arrival_indices": arrival_indices, - "matched_indices": matched_indices, + # Alias kept for callers; same threshold-greedy matching as success. + "matched_indices": list(arrival_indices), "position_errors_m": position_errors, "rotation_errors_rad": rotation_errors, } diff --git a/scripts/benchmark/motion_generation/runner.py b/scripts/benchmark/motion_generation/runner.py index 17f23e4ab..5c8c98e4e 100644 --- a/scripts/benchmark/motion_generation/runner.py +++ b/scripts/benchmark/motion_generation/runner.py @@ -178,14 +178,16 @@ def _record_unavailable( metadata: PlannerMetadata, case: BenchmarkCase, reason: str, + *, + failure_code: str, ) -> None: - """Record an unsupported runtime without converting it into failure.""" + """Record an unsupported/unavailable planner without counting a failure.""" self._append( writer, TrialRecord( **self._base_record(metadata, case, TrialPhase.AVAILABILITY), status="unsupported", - failure_code="unsupported_capability", + failure_code=failure_code, failure_message=reason, ), ) @@ -256,7 +258,9 @@ def _run_plan_call( failure_code = "planner_contract_error" failure_message = f"Expected PlanResult, got {type(result).__name__}." outcomes = make_failure_outcomes(case.batch_size, failure_code) - elif phase is TrialPhase.WARMUP: + elif phase in (TrialPhase.WARMUP, TrialPhase.COLD): + # Cold/warmup timing must not pay for FK validation that is unused + # by aggregation. outcomes = () else: try: @@ -308,6 +312,7 @@ def _run_adapter( robot: "Robot", spec: PlannerSpecCfg, cases: list[BenchmarkCase], + required_capabilities: frozenset[str], ) -> None: """Execute one adapter over every case for a fixed simulator batch.""" context = PlannerContext( @@ -320,10 +325,24 @@ def _run_adapter( metadata = adapter.metadata self.metadata.setdefault(metadata.algorithm_id, metadata) first_case = cases[0] + missing = sorted(required_capabilities - adapter.capabilities) + if missing: + self._record_unavailable( + writer, + metadata, + first_case, + f"missing required capabilities: {', '.join(missing)}", + failure_code="unsupported_capability", + ) + return available, reason = adapter.availability() if not available: self._record_unavailable( - writer, metadata, first_case, reason or "runtime unavailable" + writer, + metadata, + first_case, + reason or "runtime unavailable", + failure_code="runtime_unavailable", ) return @@ -415,7 +434,14 @@ def run(self) -> BenchmarkRunResult: ) self.cases.extend(cases) for spec in self.planner_specs: - self._run_adapter(writer, sim, robot, spec, cases) + self._run_adapter( + writer, + sim, + robot, + spec, + cases, + provider.required_capabilities, + ) finally: if sim is not None: # Benchmarks must aggregate and report after simulator @@ -445,12 +471,17 @@ def run(self) -> BenchmarkRunResult: "CPU/GPU memory values are process/PyTorch allocator deltas around timed calls.", "Continuous error and path metrics are conditioned on externally motion-valid trajectories.", "Collision, dynamic, execution, and task metrics are N/A in free-space-common v1.", - "Leaderboard overall_success_rate / motion_valid_rate / planning_success_rate " - "are macro averages over mandatory cases (equal case weight after within-case " - "env/repeat micro-average); coverage_rate remains outcome-count completeness.", + "Leaderboard and Success-table boolean rates " + "(overall_success_rate / success_rate / motion_valid_rate / " + "planning_success_rate / ordered_waypoint_success_rate) are macro averages " + "over mandatory cases (equal case weight after within-case env/repeat " + "micro-average). Missing cases contribute 0.0. coverage_rate remains " + "outcome-count completeness.", "cold_plan_ms is reported only on the Time & Memory row whose waypoint_count " "matches the first real case measured for that batch; other waypoint rows " "show N/A. planner_construct_ms / backend_prepare_ms are one-time batch costs.", + "Waypoint continuous errors use the same threshold-greedy arrival matching " + "as ordered_waypoints_reached / motion_valid.", *self.notes, ], ) diff --git a/scripts/benchmark/motion_generation/scenarios/free_space.py b/scripts/benchmark/motion_generation/scenarios/free_space.py index 2dafaf9d5..de89b7982 100644 --- a/scripts/benchmark/motion_generation/scenarios/free_space.py +++ b/scripts/benchmark/motion_generation/scenarios/free_space.py @@ -40,7 +40,7 @@ def _clamp_with_margin(qpos: torch.Tensor, limits: torch.Tensor) -> torch.Tensor: - """Clamp qpos inside ten-percent joint-limit margins.""" + """Clamp qpos inside five-percent joint-limit margins.""" lower, upper = limits[:, 0], limits[:, 1] margin = (upper - lower).clamp_min(1.0e-3) * 0.05 return torch.maximum(torch.minimum(qpos, upper - margin), lower + margin) diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index 096244cdc..9135cdd49 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -114,6 +114,31 @@ def test_ordered_waypoint_requires_position_and_rotation_at_same_sample(): assert result["arrival_indices"] == [] +def test_waypoint_errors_use_threshold_greedy_arrivals(): + """Continuous errors must come from the same matching as motion_valid.""" + waypoints = torch.stack([_translated_pose(0.0), _translated_pose(0.10)]) + # Sample 0 hits W0 under threshold. Sample 1 is a high-error later pose that + # unconstrained DP could prefer for W0 while still sequencing W1 later. + trajectory = torch.stack( + [ + _translated_pose(0.0), + _translated_pose(0.04), + _translated_pose(0.10), + ] + ) + result = match_ordered_waypoints( + trajectory, + waypoints, + position_threshold_m=0.05, + rotation_threshold_rad=0.3, + ) + + assert result["ordered_waypoints_reached"] is True + assert result["arrival_indices"] == [0, 2] + assert result["matched_indices"] == result["arrival_indices"] + assert max(result["position_errors_m"]) <= 0.05 + 1.0e-9 + + class _MetricRobot: device = torch.device("cpu") @@ -838,3 +863,235 @@ def test_toppra_adapter_close_releases_planner(): planner.close.assert_called_once_with() assert adapter.motion_generator is None + + +def test_success_table_uses_case_macro_and_counts_cases_not_env_slots(): + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ) + ] + case_a = _case() + case_b = replace(_case(), case_id="case-missing", seed=23) + records = [ + TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=case_a.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=case_a.seed, + repeat=0, + batch_size=1, + waypoint_count=1, + path_shape="direct", + start_state_bin="nominal", + phase=TrialPhase.MEASURED, + cost_time_ms=10.0, + outcomes=(_outcome(),), + ) + ] + row = aggregate_results(records, metadata, [case_a, case_b], measured_trials=1)[ + "success_and_metrics" + ][0] + assert row["cases"] == 2 + # One measured success + one missing case (counts as 0) → macro 0.5. + assert row["success_rate"] == pytest.approx(0.5) + assert row["coverage_rate"] == pytest.approx(0.5) + + large = replace( + _case(), + case_id="case-b64", + seed=23, + batch_size=64, + start_qpos=torch.zeros(64, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4).expand(64, 1, 4, 4).clone(), + reference_qpos=torch.zeros(64, 1, 7), + ) + failed = replace( + _outcome(), + planning_success=False, + ordered_waypoints_reached=False, + motion_valid=False, + completed_waypoint_ratio=0.0, + failure_code="waypoint_miss", + ) + large_record = TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id=large.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=large.seed, + repeat=0, + batch_size=64, + waypoint_count=1, + path_shape="direct", + start_state_bin="nominal", + phase=TrialPhase.MEASURED, + cost_time_ms=40.0, + outcomes=tuple(replace(failed, env_index=i) for i in range(64)), + ) + large_row = aggregate_results([large_record], metadata, [large], measured_trials=1)[ + "success_and_metrics" + ][0] + assert large_row["cases"] == 1 + assert large_row["success_rate"] == pytest.approx(0.0) + + +def test_timed_call_reports_null_peak_gpu_without_cuda(monkeypatch): + from scripts.benchmark.motion_generation.metrics import performance + + monkeypatch.setattr(performance.torch.cuda, "is_available", lambda: False) + measured = performance.timed_call(lambda: 42) + assert measured.result == 42 + assert measured.peak_gpu_mb is None + + +def test_runner_capability_gate_and_fake_adapter_lifecycle(tmp_path): + """Exercise AVAILABILITY gating and MEASURED aggregation without DexSim.""" + from scripts.benchmark.motion_generation.artifacts import TrialJsonlWriter + from scripts.benchmark.motion_generation.config import ( + PlannerSpecCfg, + ProtocolCfg, + SuiteCfg, + ) + from scripts.benchmark.motion_generation.planners.base import PlannerAdapter + from scripts.benchmark.motion_generation.registry import register_planner_adapter + from scripts.benchmark.motion_generation.runner import BenchmarkRunner + + class _CapableFake(PlannerAdapter): + capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) + + def build(self) -> None: + return None + + def plan(self, case: BenchmarkCase) -> PlanResult: + steps = max(case.num_waypoints + 1, 2) + positions = case.start_qpos.unsqueeze(1).expand(-1, steps, -1).clone() + return PlanResult(success=True, positions=positions) + + class _IncapableFake(PlannerAdapter): + capabilities = frozenset({"eef_waypoint"}) + + def build(self) -> None: + return None + + def plan(self, case: BenchmarkCase) -> PlanResult: # noqa: ARG002 + raise AssertionError("incapable adapter must not plan") + + register_planner_adapter("fake_capable", _CapableFake) + register_planner_adapter("fake_incapable", _IncapableFake) + + suite = SuiteCfg( + name="motion_generation", + suite_version="test_fake_v1", + profile="smoke", + protocol=ProtocolCfg( + warmup_trials=0, + measured_trials=1, + sample_interval=4, + validation_samples=4, + position_threshold_m=1.0, + rotation_threshold_rad=1.0, + ), + ) + runner = BenchmarkRunner( + suite, + [ + PlannerSpecCfg( + id="capable", + adapter="fake_capable", + role=AlgorithmRole.DIAGNOSTIC_BASELINE.value, + enabled=True, + ), + PlannerSpecCfg( + id="incapable", + adapter="fake_incapable", + role=AlgorithmRole.CANDIDATE.value, + enabled=True, + ), + ], + device="cpu", + output_root=tmp_path, + ) + case = _case() + case = replace(case, suite_version=suite.suite_version) + runner.cases = [case] + writer = TrialJsonlWriter(tmp_path / "trials.jsonl") + + robot = Mock(device=torch.device("cpu")) + robot.set_qpos = Mock() + robot.clear_dynamics = Mock() + robot.get_qpos_limits = Mock( + return_value=torch.tensor([[-2.0, 2.0]]).repeat(7, 1).unsqueeze(0) + ) + robot.compute_batch_fk = Mock( + side_effect=lambda qpos, name, to_matrix: ( # noqa: ARG005 + torch.eye(4).repeat(qpos.shape[0], qpos.shape[1], 1, 1).to(qpos.device) + ) + ) + sim = Mock() + sim.update = Mock() + + required = frozenset({"eef_waypoint", "batched", "empty_world"}) + runner._run_adapter( + writer, + sim, + robot, + runner.planner_specs[0], + [case], + required, + ) + runner._run_adapter( + writer, + sim, + robot, + runner.planner_specs[1], + [case], + required, + ) + + phases = { + (r.algorithm_id, r.phase, r.status, r.failure_code) for r in runner.records + } + assert ( + "incapable", + TrialPhase.AVAILABILITY, + "unsupported", + "unsupported_capability", + ) in phases + assert any( + r.algorithm_id == "capable" and r.phase is TrialPhase.COLD and r.outcomes == () + for r in runner.records + ) + assert any( + r.algorithm_id == "capable" and r.phase is TrialPhase.MEASURED + for r in runner.records + ) + + metadata = list(runner.metadata.values()) + aggregates = aggregate_results( + runner.records, metadata, [case], suite.protocol.measured_trials + ) + capable = next( + row for row in aggregates["leaderboard"] if row["algorithm"] == "capable" + ) + incapable = next( + row for row in aggregates["leaderboard"] if row["algorithm"] == "incapable" + ) + assert capable["eligible"] is True + assert capable["overall_success_rate"] == pytest.approx(1.0) + assert incapable["eligible"] is False + assert incapable["overall_success_rate"] == pytest.approx(0.0) + assert any("missing required capabilities" in note for note in runner.notes) From e24084ece8bec4b68d8042ffbd3fe53087c5aaad Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 03:51:20 +0800 Subject: [PATCH 13/17] Sort tests --- .../motion_generation/aggregation.py | 30 +- .../motion_generation/metrics/__init__.py | 2 + .../motion_generation/metrics/performance.py | 2 +- .../motion_generation/metrics/stats.py | 41 ++ .../motion_generation/metrics/trajectory.py | 69 +--- .../motion_generation/planners/base.py | 10 + .../motion_generation/planners/curobo.py | 6 +- .../motion_generation/planners/toppra.py | 6 +- .../benchmark/motion_generation/registry.py | 6 + .../benchmark/motion_generation/reporting.py | 2 +- scripts/benchmark/motion_generation/runner.py | 6 +- .../test_motion_generation_benchmark.py | 351 ++++++++++-------- 12 files changed, 278 insertions(+), 253 deletions(-) create mode 100644 scripts/benchmark/motion_generation/metrics/stats.py diff --git a/scripts/benchmark/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py index e45968a10..38a58e9eb 100644 --- a/scripts/benchmark/motion_generation/aggregation.py +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -22,6 +22,7 @@ from collections import Counter, defaultdict from collections.abc import Iterable +from .metrics.stats import nearest_rank_percentile from .models import BenchmarkCase, CaseOutcome, PlannerMetadata, TrialPhase, TrialRecord __all__ = ["aggregate_results"] @@ -37,21 +38,6 @@ def _mean(values: Iterable[float | None]) -> float | None: return sum(finite) / len(finite) if finite else None -def _percentile(values: Iterable[float | None], percentile: float) -> float | None: - """Return a nearest-rank percentile over finite values.""" - finite = sorted( - float(value) - for value in values - if value is not None and math.isfinite(float(value)) - ) - if not finite: - return None - index = max( - 0, min(len(finite) - 1, math.ceil(percentile / 100.0 * len(finite)) - 1) - ) - return finite[index] - - def _case_macro_rate( measured: list[TrialRecord], track_cases: list[BenchmarkCase], @@ -197,15 +183,12 @@ def _performance_rows( "batch_size": batch_size, "waypoint_count": waypoint_count, "num_trials": len(group), - # Construct/prepare are one-time deployment costs for the batch. "planner_construct_ms": _lifecycle_value( records, track, algorithm_id, batch_size, TrialPhase.CONSTRUCT ), "backend_prepare_ms": _lifecycle_value( records, track, algorithm_id, batch_size, TrialPhase.PREPARE ), - # Cold plan is the first real case only; attach it to that - # waypoint shape rather than repeating it on every W row. "cold_plan_ms": _lifecycle_value( records, track, @@ -215,12 +198,12 @@ def _performance_rows( waypoint_count=waypoint_count, ), "cost_time_ms": mean_cost, - "warm_plan_ms_p50": _percentile(costs, 50.0), - "warm_plan_ms_p95": _percentile(costs, 95.0), + "warm_plan_ms_p50": nearest_rank_percentile(costs, 50.0), + "warm_plan_ms_p95": nearest_rank_percentile(costs, 95.0), "latency_per_env_ms": ( mean_cost / batch_size if mean_cost is not None else None ), - "cost_time_per_segment_ms": ( + "cost_time_per_waypoint_ms": ( mean_cost / waypoint_count if mean_cost is not None else None ), "trajectories_per_second": ( @@ -254,7 +237,7 @@ def _performance_rows( "warm_plan_ms_p50": None, "warm_plan_ms_p95": None, "latency_per_env_ms": None, - "cost_time_per_segment_ms": None, + "cost_time_per_waypoint_ms": None, "trajectories_per_second": None, "cpu_delta_mb": None, "gpu_delta_mb": None, @@ -432,7 +415,7 @@ def _leaderboard_rows( coverage = min(1.0, len(outcomes) / max(expected_outcomes, 1)) motion_rate = _case_macro_rate(measured, track_cases, "motion_valid") planning_rate = _case_macro_rate(measured, track_cases, "planning_success") - latency_p95 = _percentile( + latency_p95 = nearest_rank_percentile( (record.cost_time_ms for record in measured), 95.0 ) peak_gpu = _peak_gpu(measured) @@ -445,6 +428,7 @@ def _leaderboard_rows( "planner_config_hash": info.config_hash[:12], "eligible": coverage >= 1.0 - 1.0e-12, "coverage_rate": coverage, + # free-space v1: primary_success == motion_valid "overall_success_rate": motion_rate, "planning_success_rate": planning_rate, "motion_valid_rate": motion_rate, diff --git a/scripts/benchmark/motion_generation/metrics/__init__.py b/scripts/benchmark/motion_generation/metrics/__init__.py index ed41b292f..1a2db20cf 100644 --- a/scripts/benchmark/motion_generation/metrics/__init__.py +++ b/scripts/benchmark/motion_generation/metrics/__init__.py @@ -23,6 +23,7 @@ compute_case_outcomes, compute_waypoint_errors, get_pose_err, + make_failure_outcomes, match_ordered_waypoints, ) @@ -31,6 +32,7 @@ "compute_case_outcomes", "compute_waypoint_errors", "get_pose_err", + "make_failure_outcomes", "match_ordered_waypoints", "timed_call", ] diff --git a/scripts/benchmark/motion_generation/metrics/performance.py b/scripts/benchmark/motion_generation/metrics/performance.py index 7b36b01b7..794d65aff 100644 --- a/scripts/benchmark/motion_generation/metrics/performance.py +++ b/scripts/benchmark/motion_generation/metrics/performance.py @@ -40,7 +40,7 @@ class TimedCall(Generic[_T]): cost_time_ms: float cpu_delta_mb: float gpu_delta_mb: float - peak_gpu_mb: float + peak_gpu_mb: float | None def _sync_cuda() -> None: diff --git a/scripts/benchmark/motion_generation/metrics/stats.py b/scripts/benchmark/motion_generation/metrics/stats.py new file mode 100644 index 000000000..5152abc48 --- /dev/null +++ b/scripts/benchmark/motion_generation/metrics/stats.py @@ -0,0 +1,41 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Shared numeric helpers for benchmark metric aggregation.""" + +from __future__ import annotations + +import math +from collections.abc import Iterable + +__all__ = ["nearest_rank_percentile"] + + +def nearest_rank_percentile( + values: Iterable[float | None], percentile: float +) -> float | None: + """Return a nearest-rank percentile over finite values, or ``None`` if empty.""" + finite = sorted( + float(value) + for value in values + if value is not None and math.isfinite(float(value)) + ) + if not finite: + return None + index = max( + 0, min(len(finite) - 1, math.ceil(percentile / 100.0 * len(finite)) - 1) + ) + return finite[index] diff --git a/scripts/benchmark/motion_generation/metrics/trajectory.py b/scripts/benchmark/motion_generation/metrics/trajectory.py index 5f39bd699..bf3b359db 100644 --- a/scripts/benchmark/motion_generation/metrics/trajectory.py +++ b/scripts/benchmark/motion_generation/metrics/trajectory.py @@ -19,6 +19,7 @@ from __future__ import annotations import math +from dataclasses import replace from typing import TYPE_CHECKING import torch @@ -26,6 +27,7 @@ from embodichain.lab.sim.planners.utils import PlanResult from ..models import BenchmarkCase, CaseOutcome +from .stats import nearest_rank_percentile if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -39,21 +41,6 @@ ] -def _percentile(values: list[float], percentile: float) -> float: - """Return a nearest-rank percentile for a non-empty list.""" - if not values: - return float("inf") - ordered = sorted(values) - index = max( - 0, - min( - len(ordered) - 1, - math.ceil(percentile / 100.0 * len(ordered)) - 1, - ), - ) - return float(ordered[index]) - - def _pose_error_matrices( waypoints: torch.Tensor, trajectory_poses: torch.Tensor ) -> tuple[torch.Tensor, torch.Tensor]: @@ -110,7 +97,6 @@ def match_ordered_waypoints( "ordered_waypoints_reached": False, "completed_waypoint_ratio": 0.0, "arrival_indices": [], - "matched_indices": [], "position_errors_m": [], "rotation_errors_rad": [], } @@ -144,8 +130,6 @@ def match_ordered_waypoints( "ordered_waypoints_reached": completed == total, "completed_waypoint_ratio": completed / max(total, 1), "arrival_indices": arrival_indices, - # Alias kept for callers; same threshold-greedy matching as success. - "matched_indices": list(arrival_indices), "position_errors_m": position_errors, "rotation_errors_rad": rotation_errors, } @@ -277,12 +261,13 @@ def make_failure_outcomes( failure_code: str, *, planner_failure_code: str | None = None, + planning_success: bool = False, ) -> tuple[CaseOutcome, ...]: - """Create per-env outcomes for an exception before validation was possible.""" + """Create per-env outcomes when validation cannot produce a trajectory.""" return tuple( CaseOutcome( env_index=index, - planning_success=False, + planning_success=planning_success, finite=False, ordered_waypoints_reached=False, motion_valid=False, @@ -322,32 +307,18 @@ def compute_case_outcomes( planning_success = _success_tensor(result.success, case.batch_size) if result.positions is None or result.positions.ndim != 3: return tuple( - CaseOutcome( + replace( + make_failure_outcomes( + 1, + "non_finite_trajectory", + planning_success=bool(planning_success[env_index].item()), + planner_failure_code=( + None + if bool(planning_success[env_index].item()) + else "planner_reported_failure" + ), + )[0], env_index=env_index, - planning_success=bool(planning_success[env_index].item()), - finite=False, - ordered_waypoints_reached=False, - motion_valid=False, - completed_waypoint_ratio=0.0, - final_translation_err_mm=None, - final_rotation_err_deg=None, - waypoint_translation_err_mm_mean=None, - waypoint_translation_err_mm_p95=None, - waypoint_translation_err_mm_max=None, - waypoint_rotation_err_deg_mean=None, - waypoint_rotation_err_deg_p95=None, - waypoint_rotation_err_deg_max=None, - joint_limit_violation=False, - max_normalized_joint_violation=None, - joint_path_length_rad=None, - cartesian_path_length_m=None, - path_efficiency=None, - failure_code="non_finite_trajectory", - planner_failure_code=( - None - if bool(planning_success[env_index].item()) - else "planner_reported_failure" - ), ) for env_index in range(case.batch_size) ) @@ -456,8 +427,8 @@ def compute_case_outcomes( waypoint_translation_err_mm_mean=( sum(pos_errors_mm) / len(pos_errors_mm) if pos_errors_mm else None ), - waypoint_translation_err_mm_p95=( - _percentile(pos_errors_mm, 95.0) if pos_errors_mm else None + waypoint_translation_err_mm_p95=nearest_rank_percentile( + pos_errors_mm, 95.0 ), waypoint_translation_err_mm_max=( max(pos_errors_mm) if pos_errors_mm else None @@ -467,8 +438,8 @@ def compute_case_outcomes( if rot_errors_deg else None ), - waypoint_rotation_err_deg_p95=( - _percentile(rot_errors_deg, 95.0) if rot_errors_deg else None + waypoint_rotation_err_deg_p95=nearest_rank_percentile( + rot_errors_deg, 95.0 ), waypoint_rotation_err_deg_max=( max(rot_errors_deg) if rot_errors_deg else None diff --git a/scripts/benchmark/motion_generation/planners/base.py b/scripts/benchmark/motion_generation/planners/base.py index cbf9e90d5..895e5f11b 100644 --- a/scripts/benchmark/motion_generation/planners/base.py +++ b/scripts/benchmark/motion_generation/planners/base.py @@ -88,5 +88,15 @@ def prepare(self, case: BenchmarkCase) -> dict[str, object] | None: def plan(self, case: BenchmarkCase) -> PlanResult: """Plan one env-batched benchmark case.""" + def _close_motion_generator(self) -> None: + """Release ``motion_generator.planner`` when adapters own one.""" + motion_generator = getattr(self, "motion_generator", None) + if motion_generator is None: + return + close_fn = getattr(getattr(motion_generator, "planner", None), "close", None) + if close_fn is not None: + close_fn() + self.motion_generator = None + def close(self) -> None: """Release backend resources when the implementation exposes them.""" diff --git a/scripts/benchmark/motion_generation/planners/curobo.py b/scripts/benchmark/motion_generation/planners/curobo.py index 87595ae1d..b21081487 100644 --- a/scripts/benchmark/motion_generation/planners/curobo.py +++ b/scripts/benchmark/motion_generation/planners/curobo.py @@ -134,11 +134,7 @@ def plan(self, case: BenchmarkCase) -> PlanResult: def close(self) -> None: """Destroy cached cuRobo graph and planner resources.""" - if self.motion_generator is not None: - close_fn = getattr(self.motion_generator.planner, "close", None) - if close_fn is not None: - close_fn() - self.motion_generator = None + self._close_motion_generator() register_planner_adapter("curobo", CuroboAdapter) diff --git a/scripts/benchmark/motion_generation/planners/toppra.py b/scripts/benchmark/motion_generation/planners/toppra.py index 7bc4a4a70..038dbc0a7 100644 --- a/scripts/benchmark/motion_generation/planners/toppra.py +++ b/scripts/benchmark/motion_generation/planners/toppra.py @@ -89,11 +89,7 @@ def plan(self, case: BenchmarkCase) -> PlanResult: def close(self) -> None: """Release TOPPRA worker pools and drop the motion generator.""" - if self.motion_generator is not None: - close_fn = getattr(self.motion_generator.planner, "close", None) - if close_fn is not None: - close_fn() - self.motion_generator = None + self._close_motion_generator() register_planner_adapter("toppra", ToppraAdapter) diff --git a/scripts/benchmark/motion_generation/registry.py b/scripts/benchmark/motion_generation/registry.py index e43aa6fb0..d10efb3fe 100644 --- a/scripts/benchmark/motion_generation/registry.py +++ b/scripts/benchmark/motion_generation/registry.py @@ -32,6 +32,7 @@ "register_planner_adapter", "register_scenario_provider", "scenario_provider_names", + "unregister_planner_adapter", ] _PLANNER_ADAPTERS: dict[str, type["PlannerAdapter"]] = {} @@ -48,6 +49,11 @@ def register_planner_adapter(name: str, adapter_cls: type["PlannerAdapter"]) -> _PLANNER_ADAPTERS[name] = adapter_cls +def unregister_planner_adapter(name: str) -> None: + """Remove one planner adapter registration when present.""" + _PLANNER_ADAPTERS.pop(name, None) + + def planner_adapter_names() -> tuple[str, ...]: """Return registered adapter names in deterministic order.""" return tuple(sorted(_PLANNER_ADAPTERS)) diff --git a/scripts/benchmark/motion_generation/reporting.py b/scripts/benchmark/motion_generation/reporting.py index 6518e7163..a194d4841 100644 --- a/scripts/benchmark/motion_generation/reporting.py +++ b/scripts/benchmark/motion_generation/reporting.py @@ -40,7 +40,7 @@ "warm_plan_ms_p50", "warm_plan_ms_p95", "latency_per_env_ms", - "cost_time_per_segment_ms", + "cost_time_per_waypoint_ms", "trajectories_per_second", "cpu_delta_mb", "gpu_delta_mb", diff --git a/scripts/benchmark/motion_generation/runner.py b/scripts/benchmark/motion_generation/runner.py index 5c8c98e4e..b496ba7dc 100644 --- a/scripts/benchmark/motion_generation/runner.py +++ b/scripts/benchmark/motion_generation/runner.py @@ -201,7 +201,6 @@ def _record_timed_lifecycle( metadata: PlannerMetadata, case: BenchmarkCase, phase: TrialPhase, - adapter: PlannerAdapter, callable_fn: "Callable[[], object]", ) -> tuple[object | None, Exception | None]: """Measure a construct/prepare operation and persist its outcome.""" @@ -240,7 +239,7 @@ def _run_plan_call( case: BenchmarkCase, phase: TrialPhase, repeat: int, - ) -> bool: + ) -> None: """Time one plan, validate outside timing, and persist the record.""" self._set_case_start(sim, robot, case) measured = timed_call(lambda: _capture(lambda: adapter.plan(case))) @@ -303,7 +302,6 @@ def _run_plan_call( f"{phase.value:<8} {measured.cost_time_ms:>10.3f} ms " f"status={status}" ) - return error is None and status == "ok" def _run_adapter( self, @@ -351,7 +349,6 @@ def _run_adapter( metadata, first_case, TrialPhase.CONSTRUCT, - adapter, adapter.build, ) if build_error is not None: @@ -364,7 +361,6 @@ def _run_adapter( metadata, first_case, TrialPhase.PREPARE, - adapter, lambda: adapter.prepare(first_case), ) if prepare_error is not None: diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index 9135cdd49..66839a9ff 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -135,15 +135,18 @@ def test_waypoint_errors_use_threshold_greedy_arrivals(): assert result["ordered_waypoints_reached"] is True assert result["arrival_indices"] == [0, 2] - assert result["matched_indices"] == result["arrival_indices"] assert max(result["position_errors_m"]) <= 0.05 + 1.0e-9 class _MetricRobot: + """Minimal FK stub: joint xyz maps to TCP translation.""" + device = torch.device("cpu") + limit_lo = -1.0 + limit_hi = 1.0 def get_qpos_limits(self, name: str): # noqa: ARG002 - limits = torch.tensor([[-1.0, 1.0]]).repeat(7, 1) + limits = torch.tensor([[self.limit_lo, self.limit_hi]]).repeat(7, 1) return limits.unsqueeze(0) def compute_batch_fk( @@ -153,13 +156,24 @@ def compute_batch_fk( poses[..., :3, 3] = qpos[..., :3] return poses + def compute_fk( + self, qpos: torch.Tensor, name: str, to_matrix: bool + ): # noqa: ARG002 + poses = torch.eye(4).repeat(qpos.shape[0], 1, 1) + poses[:, :3, 3] = qpos[:, :3] + return poses -def test_motion_valid_is_independent_of_planner_reported_success(): + +def _valid_motion_case_and_positions() -> tuple[BenchmarkCase, torch.Tensor]: case = _case() case.target_waypoints[0, 0, 0, 3] = 0.1 positions = torch.zeros(1, 2, 7) positions[0, 1, 0] = 0.1 + return case, positions + +def test_motion_valid_ignores_planner_reported_failure_in_outcomes_and_aggregates(): + case, positions = _valid_motion_case_and_positions() outcomes = compute_case_outcomes( PlanResult(success=False, positions=positions), case, @@ -170,28 +184,11 @@ def test_motion_valid_is_independent_of_planner_reported_success(): rotation_threshold_rad=1.0e-4, joint_limit_tolerance_rad=1.0e-5, ) - assert outcomes[0].planning_success is False assert outcomes[0].motion_valid is True assert outcomes[0].failure_code is None assert outcomes[0].planner_failure_code == "planner_reported_failure" - -def test_top_failure_ignores_planner_internal_codes_when_motion_valid(): - case = _case() - case.target_waypoints[0, 0, 0, 3] = 0.1 - positions = torch.zeros(1, 2, 7) - positions[0, 1, 0] = 0.1 - outcomes = compute_case_outcomes( - PlanResult(success=False, positions=positions), - case, - _MetricRobot(), - "arm", - validation_samples=8, - position_threshold_m=1.0e-4, - rotation_threshold_rad=1.0e-4, - joint_limit_tolerance_rad=1.0e-5, - ) metadata = [ PlannerMetadata( algorithm_id="curobo", @@ -220,14 +217,47 @@ def test_top_failure_ignores_planner_internal_codes_when_motion_valid(): cost_time_ms=10.0, outcomes=outcomes, ) - - aggregates = aggregate_results([measured], metadata, [case], measured_trials=1) - row = aggregates["success_and_metrics"][0] - + row = aggregate_results([measured], metadata, [case], measured_trials=1)[ + "success_and_metrics" + ][0] assert row["success_rate"] == pytest.approx(1.0) assert row["planning_success_rate"] == pytest.approx(0.0) assert row["top_failure"] is None - assert row["start_state_bin"] == "nominal" + + +def test_missing_positions_and_joint_limit_violation_fail_motion_valid(): + case = _case() + missing = compute_case_outcomes( + PlanResult(success=True, positions=None), + case, + _MetricRobot(), + "arm", + validation_samples=8, + position_threshold_m=1.0e-4, + rotation_threshold_rad=1.0e-4, + joint_limit_tolerance_rad=1.0e-5, + ) + assert missing[0].motion_valid is False + assert missing[0].failure_code == "non_finite_trajectory" + + # Reach the waypoint at x=2 while joint 0 is outside [-1, 1]. + case.target_waypoints[0, 0, 0, 3] = 2.0 + positions = torch.zeros(1, 2, 7) + positions[0, :, 0] = 2.0 + violated = compute_case_outcomes( + PlanResult(success=True, positions=positions), + case, + _MetricRobot(), + "arm", + validation_samples=8, + position_threshold_m=1.0e-4, + rotation_threshold_rad=1.0e-4, + joint_limit_tolerance_rad=1.0e-5, + ) + assert violated[0].ordered_waypoints_reached is True + assert violated[0].joint_limit_violation is True + assert violated[0].motion_valid is False + assert violated[0].failure_code == "joint_limit_violation" def test_nmg_precision_and_external_accuracy_are_independently_configurable(): @@ -255,21 +285,14 @@ def test_nmg_precision_rejects_non_positive_values(override): _apply_overrides(suite, **override) -class _FakeRobot: - device = torch.device("cpu") +class _FrankaLimitRobot(_MetricRobot): + """FK stub with Franka-like joint limits for free-space case generation.""" def get_qpos_limits(self, name: str): # noqa: ARG002 lower = torch.tensor([-2.8, -1.7, -2.8, -3.0, -2.8, 0.0, -2.8]) upper = torch.tensor([2.8, 1.7, 2.8, -0.05, 2.8, 3.7, 2.8]) return torch.stack([lower, upper], dim=-1).unsqueeze(0) - def compute_fk( - self, qpos: torch.Tensor, name: str, to_matrix: bool - ): # noqa: ARG002 - poses = torch.eye(4).repeat(qpos.shape[0], 1, 1) - poses[:, :3, 3] = qpos[:, :3] - return poses - def test_suite_loads_tracks_and_keeps_mutable_free_space_config(): suite = load_suite("smoke") @@ -287,7 +310,7 @@ def test_suite_loads_tracks_and_keeps_mutable_free_space_config(): def test_free_space_manifest_is_seed_stable_and_algorithm_independent(): suite = load_suite("smoke") - robot = _FakeRobot() + robot = _FrankaLimitRobot() provider = create_scenario_provider("free_space") track = suite.enabled_tracks()[0] @@ -309,7 +332,7 @@ def test_free_space_cases_use_one_start_state_bin_each(): suite.free_space.start_state_bins = ["nominal", "near_limit"] track = suite.enabled_tracks()[0] cases = create_scenario_provider("free_space").generate_cases( - suite, track, _FakeRobot(), "arm", batch_size=2 + suite, track, _FrankaLimitRobot(), "arm", batch_size=2 ) assert [case.start_state_bin for case in cases] == ["nominal", "near_limit"] @@ -800,44 +823,6 @@ def test_curobo_prepare_backend_exposes_actual_graph_mode(): assert result["batch_size"] == 8 -def test_metric_rows_use_success_rate_and_null_peak_gpu(): - metadata = [ - PlannerMetadata( - algorithm_id="curobo", - algorithm_role=AlgorithmRole.PRIMARY_BASELINE, - adapter="curobo", - config_hash="abc", - capabilities=frozenset({"eef_waypoint"}), - ) - ] - measured = TrialRecord( - suite_version="test_v1", - track="free-space-common", - scenario_id="reach", - case_id="case-1", - algorithm_id="curobo", - algorithm_role=AlgorithmRole.PRIMARY_BASELINE, - model_revision="curobo-v2", - planner_config_hash="abc", - seed=11, - repeat=0, - batch_size=1, - waypoint_count=1, - path_shape="direct", - start_state_bin="nominal", - phase=TrialPhase.MEASURED, - cost_time_ms=10.0, - peak_gpu_mb=None, - outcomes=(_outcome(),), - ) - aggregates = aggregate_results([measured], metadata, [_case()], measured_trials=1) - - assert aggregates["success_and_metrics"][0]["success_rate"] == pytest.approx(1.0) - assert "motion_valid_rate" not in aggregates["success_and_metrics"][0] - assert aggregates["time_and_memory"][0]["peak_gpu_mb"] is None - assert aggregates["leaderboard"][0]["peak_gpu_mb"] is None - - def test_toppra_adapter_close_releases_planner(): from scripts.benchmark.motion_generation.config import PlannerSpecCfg from scripts.benchmark.motion_generation.planners.base import PlannerContext @@ -967,7 +952,10 @@ def test_runner_capability_gate_and_fake_adapter_lifecycle(tmp_path): SuiteCfg, ) from scripts.benchmark.motion_generation.planners.base import PlannerAdapter - from scripts.benchmark.motion_generation.registry import register_planner_adapter + from scripts.benchmark.motion_generation.registry import ( + register_planner_adapter, + unregister_planner_adapter, + ) from scripts.benchmark.motion_generation.runner import BenchmarkRunner class _CapableFake(PlannerAdapter): @@ -990,25 +978,52 @@ def build(self) -> None: def plan(self, case: BenchmarkCase) -> PlanResult: # noqa: ARG002 raise AssertionError("incapable adapter must not plan") - register_planner_adapter("fake_capable", _CapableFake) - register_planner_adapter("fake_incapable", _IncapableFake) + class _RuntimeUnavailableFake(PlannerAdapter): + capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) - suite = SuiteCfg( - name="motion_generation", - suite_version="test_fake_v1", - profile="smoke", - protocol=ProtocolCfg( - warmup_trials=0, - measured_trials=1, - sample_interval=4, - validation_samples=4, - position_threshold_m=1.0, - rotation_threshold_rad=1.0, - ), + def availability(self) -> tuple[bool, str | None]: + return False, "runtime missing" + + def build(self) -> None: + return None + + def plan(self, case: BenchmarkCase) -> PlanResult: # noqa: ARG002 + raise AssertionError("unavailable adapter must not plan") + + class _ContractBrokenFake(PlannerAdapter): + capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) + + def build(self) -> None: + return None + + def plan(self, case: BenchmarkCase): # noqa: ARG002 + return "not-a-plan-result" + + names = ( + "fake_capable", + "fake_incapable", + "fake_runtime_unavailable", + "fake_contract_broken", ) - runner = BenchmarkRunner( - suite, - [ + register_planner_adapter("fake_capable", _CapableFake) + register_planner_adapter("fake_incapable", _IncapableFake) + register_planner_adapter("fake_runtime_unavailable", _RuntimeUnavailableFake) + register_planner_adapter("fake_contract_broken", _ContractBrokenFake) + try: + suite = SuiteCfg( + name="motion_generation", + suite_version="test_fake_v1", + profile="smoke", + protocol=ProtocolCfg( + warmup_trials=0, + measured_trials=1, + sample_interval=4, + validation_samples=4, + position_threshold_m=1.0, + rotation_threshold_rad=1.0, + ), + ) + specs = [ PlannerSpecCfg( id="capable", adapter="fake_capable", @@ -1021,77 +1036,85 @@ def plan(self, case: BenchmarkCase) -> PlanResult: # noqa: ARG002 role=AlgorithmRole.CANDIDATE.value, enabled=True, ), - ], - device="cpu", - output_root=tmp_path, - ) - case = _case() - case = replace(case, suite_version=suite.suite_version) - runner.cases = [case] - writer = TrialJsonlWriter(tmp_path / "trials.jsonl") - - robot = Mock(device=torch.device("cpu")) - robot.set_qpos = Mock() - robot.clear_dynamics = Mock() - robot.get_qpos_limits = Mock( - return_value=torch.tensor([[-2.0, 2.0]]).repeat(7, 1).unsqueeze(0) - ) - robot.compute_batch_fk = Mock( - side_effect=lambda qpos, name, to_matrix: ( # noqa: ARG005 - torch.eye(4).repeat(qpos.shape[0], qpos.shape[1], 1, 1).to(qpos.device) + PlannerSpecCfg( + id="runtime_down", + adapter="fake_runtime_unavailable", + role=AlgorithmRole.CANDIDATE.value, + enabled=True, + ), + PlannerSpecCfg( + id="broken", + adapter="fake_contract_broken", + role=AlgorithmRole.CANDIDATE.value, + enabled=True, + ), + ] + runner = BenchmarkRunner(suite, specs, device="cpu", output_root=tmp_path) + case = replace(_case(), suite_version=suite.suite_version) + runner.cases = [case] + writer = TrialJsonlWriter(tmp_path / "trials.jsonl") + + robot = Mock(device=torch.device("cpu")) + robot.set_qpos = Mock() + robot.clear_dynamics = Mock() + robot.get_qpos_limits = Mock( + return_value=torch.tensor([[-2.0, 2.0]]).repeat(7, 1).unsqueeze(0) + ) + robot.compute_batch_fk = Mock( + side_effect=lambda qpos, name, to_matrix: ( # noqa: ARG005 + torch.eye(4).repeat(qpos.shape[0], qpos.shape[1], 1, 1).to(qpos.device) + ) + ) + sim = Mock() + sim.update = Mock() + required = frozenset({"eef_waypoint", "batched", "empty_world"}) + for spec in specs: + runner._run_adapter(writer, sim, robot, spec, [case], required) + + phases = { + (r.algorithm_id, r.phase, r.status, r.failure_code) for r in runner.records + } + assert ( + "incapable", + TrialPhase.AVAILABILITY, + "unsupported", + "unsupported_capability", + ) in phases + assert ( + "runtime_down", + TrialPhase.AVAILABILITY, + "unsupported", + "runtime_unavailable", + ) in phases + assert any( + r.algorithm_id == "broken" + and r.phase is TrialPhase.MEASURED + and r.failure_code == "planner_contract_error" + for r in runner.records + ) + assert any( + r.algorithm_id == "capable" + and r.phase is TrialPhase.COLD + and r.outcomes == () + for r in runner.records ) - ) - sim = Mock() - sim.update = Mock() - - required = frozenset({"eef_waypoint", "batched", "empty_world"}) - runner._run_adapter( - writer, - sim, - robot, - runner.planner_specs[0], - [case], - required, - ) - runner._run_adapter( - writer, - sim, - robot, - runner.planner_specs[1], - [case], - required, - ) - - phases = { - (r.algorithm_id, r.phase, r.status, r.failure_code) for r in runner.records - } - assert ( - "incapable", - TrialPhase.AVAILABILITY, - "unsupported", - "unsupported_capability", - ) in phases - assert any( - r.algorithm_id == "capable" and r.phase is TrialPhase.COLD and r.outcomes == () - for r in runner.records - ) - assert any( - r.algorithm_id == "capable" and r.phase is TrialPhase.MEASURED - for r in runner.records - ) - metadata = list(runner.metadata.values()) - aggregates = aggregate_results( - runner.records, metadata, [case], suite.protocol.measured_trials - ) - capable = next( - row for row in aggregates["leaderboard"] if row["algorithm"] == "capable" - ) - incapable = next( - row for row in aggregates["leaderboard"] if row["algorithm"] == "incapable" - ) - assert capable["eligible"] is True - assert capable["overall_success_rate"] == pytest.approx(1.0) - assert incapable["eligible"] is False - assert incapable["overall_success_rate"] == pytest.approx(0.0) - assert any("missing required capabilities" in note for note in runner.notes) + aggregates = aggregate_results( + runner.records, + list(runner.metadata.values()), + [case], + suite.protocol.measured_trials, + ) + capable = next( + row for row in aggregates["leaderboard"] if row["algorithm"] == "capable" + ) + incapable = next( + row for row in aggregates["leaderboard"] if row["algorithm"] == "incapable" + ) + assert capable["eligible"] is True + assert capable["overall_success_rate"] == pytest.approx(1.0) + assert incapable["eligible"] is False + assert any("missing required capabilities" in note for note in runner.notes) + finally: + for name in names: + unregister_planner_adapter(name) From 8321adadf672520a9a0871b6fa6f392e7bd6330d Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 04:06:29 +0800 Subject: [PATCH 14/17] Use case-macro p95 for leaderboard latency --- .../motion_generation/aggregation.py | 39 +++++++-- scripts/benchmark/motion_generation/runner.py | 4 + .../test_motion_generation_benchmark.py | 80 +++++++++++++++++++ 3 files changed, 118 insertions(+), 5 deletions(-) diff --git a/scripts/benchmark/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py index 38a58e9eb..3f06034c4 100644 --- a/scripts/benchmark/motion_generation/aggregation.py +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -96,6 +96,36 @@ def _case_macro_mean( return sum(case_means) / len(case_means) if case_means else None +def _case_macro_latency_p95( + measured: list[TrialRecord], + track_cases: list[BenchmarkCase], +) -> float | None: + """Return a case-macro warm-latency p95 for leaderboard ranking. + + Each mandatory case first collapses its measured repeats to a mean + ``cost_time_ms``. The leaderboard then takes the nearest-rank p95 over + those case means so every case has equal weight regardless of repeat + count, ``batch_size``, or waypoint difficulty. Missing cases are omitted + from the percentile (coverage / ``eligible`` already penalize skips); + stratified absolute latency remains in the Time & Memory table. + """ + if not track_cases: + return None + + costs_by_case: dict[str, list[float]] = defaultdict(list) + for record in measured: + if record.cost_time_ms is None or not math.isfinite(float(record.cost_time_ms)): + continue + costs_by_case[record.case_id].append(float(record.cost_time_ms)) + + case_means = [ + sum(costs_by_case[case.case_id]) / len(costs_by_case[case.case_id]) + for case in track_cases + if case.case_id in costs_by_case + ] + return nearest_rank_percentile(case_means, 95.0) + + def _top_failure(outcomes: list[CaseOutcome]) -> str | None: """Return the most frequent non-empty external failure code.""" failures = Counter( @@ -393,8 +423,9 @@ def _leaderboard_rows( """Build a complete success/coverage/latency ordered leaderboard per track. Success rates are macro-averaged over mandatory cases (equal case weight). - ``coverage_rate`` remains an outcome-count completeness check used for - eligibility. + ``latency_p95_ms`` uses the same case-equal weighting: per-case mean warm + latency, then nearest-rank p95 across cases. ``coverage_rate`` remains an + outcome-count completeness check used for eligibility. """ entries: list[dict[str, object]] = [] for track in _track_ids(records, cases): @@ -415,9 +446,7 @@ def _leaderboard_rows( coverage = min(1.0, len(outcomes) / max(expected_outcomes, 1)) motion_rate = _case_macro_rate(measured, track_cases, "motion_valid") planning_rate = _case_macro_rate(measured, track_cases, "planning_success") - latency_p95 = nearest_rank_percentile( - (record.cost_time_ms for record in measured), 95.0 - ) + latency_p95 = _case_macro_latency_p95(measured, track_cases) peak_gpu = _peak_gpu(measured) track_entries.append( { diff --git a/scripts/benchmark/motion_generation/runner.py b/scripts/benchmark/motion_generation/runner.py index b496ba7dc..f454f92db 100644 --- a/scripts/benchmark/motion_generation/runner.py +++ b/scripts/benchmark/motion_generation/runner.py @@ -473,6 +473,10 @@ def run(self) -> BenchmarkRunResult: "over mandatory cases (equal case weight after within-case env/repeat " "micro-average). Missing cases contribute 0.0. coverage_rate remains " "outcome-count completeness.", + "Leaderboard latency_p95_ms is a case-macro tiebreaker: mean warm " + "cost_time_ms within each case, then nearest-rank p95 across cases " + "(equal case weight; missing cases omitted). Stratified absolute latency " + "stays in Time & Memory (warm_plan_ms_p50/p95 by batch_size/waypoint_count).", "cold_plan_ms is reported only on the Time & Memory row whose waypoint_count " "matches the first real case measured for that batch; other waypoint rows " "show N/A. planner_construct_ms / backend_prepare_ms are one-time batch costs.", diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index 66839a9ff..f00778e82 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -671,6 +671,86 @@ def test_leaderboard_uses_case_macro_average_not_env_micro_average(): assert row["eligible"] is True +def test_leaderboard_latency_p95_uses_case_macro_not_trial_pool(): + """Many fast repeats must not drown a slow case in leaderboard latency_p95.""" + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ) + ] + fast = BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-fast", + seed=11, + batch_size=1, + num_waypoints=1, + path_shape="direct", + start_state_bin="nominal", + start_qpos=torch.zeros(1, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4), + reference_qpos=torch.zeros(1, 1, 7), + ) + slow = BenchmarkCase( + suite_version="test_v1", + track="free-space-common", + scenario_id="reach", + case_id="case-slow", + seed=23, + batch_size=64, + num_waypoints=5, + path_shape="s_curve", + start_state_bin="nominal", + start_qpos=torch.zeros(64, 7), + target_waypoints=torch.eye(4).reshape(1, 1, 4, 4).expand(64, 1, 4, 4).clone(), + reference_qpos=torch.zeros(64, 1, 7), + ) + + def _latency_record( + case: BenchmarkCase, *, cost: float, repeat: int + ) -> TrialRecord: + return TrialRecord( + suite_version="test_v1", + track="free-space-common", + scenario_id=case.scenario_id, + case_id=case.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=case.seed, + repeat=repeat, + batch_size=case.batch_size, + waypoint_count=case.num_waypoints, + path_shape=case.path_shape, + start_state_bin=case.start_state_bin, + phase=TrialPhase.MEASURED, + cost_time_ms=cost, + outcomes=tuple( + replace(_outcome(), env_index=env_index) + for env_index in range(case.batch_size) + ), + ) + + # 20 fast repeats at 10 ms plus one slow case at 1000 ms. + # Trial-pooled nearest-rank p95 over 21 values is still 10 ms; case-macro + # p95 over case means [10, 1000] is 1000 ms. + records = [ + _latency_record(fast, cost=10.0, repeat=index) for index in range(20) + ] + [_latency_record(slow, cost=1000.0, repeat=0)] + + row = aggregate_results(records, metadata, [fast, slow], measured_trials=1)[ + "leaderboard" + ][0] + + assert row["latency_p95_ms"] == pytest.approx(1000.0) + + def test_cold_plan_ms_only_attaches_to_matching_waypoint_row(): """Cold latency from W=1 must not be copied onto W=5 Time & Memory rows.""" metadata = [ From b47d206eb0df184e49a355314a3f5c323a87692f Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 04:14:02 +0800 Subject: [PATCH 15/17] Tighten free-space thresholds and align case starts --- .../motion_generation/aggregation.py | 1 + scripts/benchmark/motion_generation/config.py | 8 ++--- .../motion_generation/metrics/trajectory.py | 4 +-- .../benchmark/motion_generation/reporting.py | 22 ++++++++++++-- scripts/benchmark/motion_generation/runner.py | 9 +++++- .../motion_generation/scenarios/free_space.py | 16 +++++----- .../motion_generation/suites/coverage.yaml | 4 +-- .../motion_generation/suites/smoke.yaml | 4 +-- .../test_motion_generation_benchmark.py | 29 +++++++++++++++++++ 9 files changed, 76 insertions(+), 21 deletions(-) diff --git a/scripts/benchmark/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py index 3f06034c4..25dbd45db 100644 --- a/scripts/benchmark/motion_generation/aggregation.py +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -368,6 +368,7 @@ def _metric_rows( "path_shape": path_shape, "start_state_bin": start_state_bin, "cases": len(group_cases), + "n_valid": len(valid_outcomes), "coverage_rate": min(1.0, len(outcomes) / max(expected, 1)), # Free-space primary success is external motion validity. "success_rate": _case_macro_rate( diff --git a/scripts/benchmark/motion_generation/config.py b/scripts/benchmark/motion_generation/config.py index 592d5b579..61fb07019 100644 --- a/scripts/benchmark/motion_generation/config.py +++ b/scripts/benchmark/motion_generation/config.py @@ -79,8 +79,8 @@ class ProtocolCfg: measured_trials: int = 3 sample_interval: int = 40 validation_samples: int = 128 - position_threshold_m: float = 0.05 - rotation_threshold_rad: float = 0.3 + position_threshold_m: float = 0.01 + rotation_threshold_rad: float = 0.1 joint_limit_tolerance_rad: float = 1.0e-5 @@ -111,7 +111,7 @@ class SuiteCfg: schema_version: int = 1 name: str = "free_space_common" - suite_version: str = "free_space_common_v1" + suite_version: str = "free_space_common_v2" profile: str = "smoke" planners: list[PlannerSpecCfg] = [] protocol: ProtocolCfg = ProtocolCfg() @@ -126,7 +126,7 @@ def from_dict(cls, data: dict[str, Any]) -> "SuiteCfg": suite = cls( schema_version=int(data.get("schema_version", 1)), name=str(data.get("name", "free_space_common")), - suite_version=str(data.get("suite_version", "free_space_common_v1")), + suite_version=str(data.get("suite_version", "free_space_common_v2")), profile=str(data.get("profile", "smoke")), planners=planners, protocol=ProtocolCfg(**data.get("protocol", {})), diff --git a/scripts/benchmark/motion_generation/metrics/trajectory.py b/scripts/benchmark/motion_generation/metrics/trajectory.py index bf3b359db..a977fcc25 100644 --- a/scripts/benchmark/motion_generation/metrics/trajectory.py +++ b/scripts/benchmark/motion_generation/metrics/trajectory.py @@ -139,8 +139,8 @@ def compute_waypoint_errors( trajectory_poses: list[torch.Tensor] | torch.Tensor, waypoints: torch.Tensor, *, - position_threshold_m: float = 0.05, - rotation_threshold_rad: float = 0.3, + position_threshold_m: float = 0.01, + rotation_threshold_rad: float = 0.1, ) -> dict[str, float]: """Return ordered, same-sample waypoint errors for one trajectory.""" if isinstance(trajectory_poses, list): diff --git a/scripts/benchmark/motion_generation/reporting.py b/scripts/benchmark/motion_generation/reporting.py index a194d4841..e5d26af63 100644 --- a/scripts/benchmark/motion_generation/reporting.py +++ b/scripts/benchmark/motion_generation/reporting.py @@ -57,6 +57,7 @@ "path_shape", "start_state_bin", "cases", + "n_valid", "coverage_rate", "success_rate", "planning_success_rate", @@ -138,14 +139,29 @@ def write_markdown_report( f"- suite: `{suite.name}`", f"- suite_version: `{suite.suite_version}`", f"- profile: `{suite.profile}`", - f"- external position threshold: `{suite.protocol.position_threshold_m} m`", - f"- external rotation threshold: `{suite.protocol.rotation_threshold_rad} rad`", + ( + f"- external position threshold: `{suite.protocol.position_threshold_m} m` " + "(motion-validity gate, not a high-precision tolerance)" + ), + ( + f"- external rotation threshold: `{suite.protocol.rotation_threshold_rad} rad` " + "(motion-validity gate, not a high-precision tolerance)" + ), "", "## Time & Memory", "", ] lines.extend(_format_table(aggregates["time_and_memory"], TIME_COLUMNS)) - lines.extend(["", "## Success & Other Metrics", ""]) + lines.extend( + [ + "", + "## Success & Other Metrics", + "", + "Continuous error/path columns are conditioned on `motion_valid` " + "outcomes; use `n_valid` as the denominator before comparing them.", + "", + ] + ) lines.extend(_format_table(aggregates["success_and_metrics"], METRIC_COLUMNS)) lines.extend(["", "## Leaderboard", ""]) lines.extend(_format_table(aggregates["leaderboard"], LEADERBOARD_COLUMNS)) diff --git a/scripts/benchmark/motion_generation/runner.py b/scripts/benchmark/motion_generation/runner.py index f454f92db..6f6badc87 100644 --- a/scripts/benchmark/motion_generation/runner.py +++ b/scripts/benchmark/motion_generation/runner.py @@ -465,7 +465,14 @@ def run(self) -> BenchmarkRunResult: aggregates, notes=[ "CPU/GPU memory values are process/PyTorch allocator deltas around timed calls.", - "Continuous error and path metrics are conditioned on externally motion-valid trajectories.", + "External position/rotation thresholds (default 0.01 m / 0.1 rad) are " + "feasibility gates for ordered_waypoints_reached / motion_valid. " + "Read final_*_err and waypoint_*_p95 for finer precision on the " + "motion-valid subset.", + "Continuous error and path metrics (final_*_err, waypoint_*_p95, " + "path lengths, path_efficiency) average only motion_valid outcomes " + "(success-conditioned / survivor-biased). Always read them with n_valid; " + "a high path_efficiency on n_valid=2 is not comparable to n_valid=200.", "Collision, dynamic, execution, and task metrics are N/A in free-space-common v1.", "Leaderboard and Success-table boolean rates " "(overall_success_rate / success_rate / motion_valid_rate / " diff --git a/scripts/benchmark/motion_generation/scenarios/free_space.py b/scripts/benchmark/motion_generation/scenarios/free_space.py index de89b7982..ce2b7fff4 100644 --- a/scripts/benchmark/motion_generation/scenarios/free_space.py +++ b/scripts/benchmark/motion_generation/scenarios/free_space.py @@ -130,12 +130,16 @@ def _build_case( batch_size: int, num_waypoints: int, path_shape: str, - shape_index: int, start_state_bin: str, bin_index: int, track_id: str, ) -> BenchmarkCase: - """Build one reachable env-batched case using FK reference targets.""" + """Build one reachable env-batched case using FK reference targets. + + Start postures depend only on ``(seed, start_state_bin, env_index)`` so the + same env row keeps a shared start across path shapes and waypoint counts. + Path shape and waypoint count affect targets only. + """ limits = robot.get_qpos_limits(name=control_part)[0].detach().cpu() if limits.shape[0] != _NOMINAL_QPOS.shape[0]: raise ValueError( @@ -146,9 +150,8 @@ def _build_case( starts: list[torch.Tensor] = [] for env_index in range(batch_size): generator = torch.Generator(device="cpu") - generator.manual_seed( - seed * 100_003 + shape_index * 997 + bin_index * 131 + env_index - ) + # Keep starts aligned across path_shape / num_waypoints comparisons. + generator.manual_seed(seed * 100_003 + bin_index * 131 + env_index) starts.append(_start_qpos_for_bin(start_state_bin, limits, generator)) start_qpos_cpu = torch.stack(starts) @@ -218,7 +221,7 @@ def generate_cases( cases: list[BenchmarkCase] = [] for seed in suite.free_space.seeds: for num_waypoints in suite.free_space.waypoint_counts: - for shape_index, path_shape in enumerate(suite.free_space.path_shapes): + for path_shape in suite.free_space.path_shapes: for bin_index, start_state_bin in enumerate( suite.free_space.start_state_bins ): @@ -231,7 +234,6 @@ def generate_cases( batch_size=batch_size, num_waypoints=num_waypoints, path_shape=path_shape, - shape_index=shape_index, start_state_bin=start_state_bin, bin_index=bin_index, track_id=track.id, diff --git a/scripts/benchmark/motion_generation/suites/coverage.yaml b/scripts/benchmark/motion_generation/suites/coverage.yaml index fffc8ab82..ebcd8faa9 100644 --- a/scripts/benchmark/motion_generation/suites/coverage.yaml +++ b/scripts/benchmark/motion_generation/suites/coverage.yaml @@ -50,8 +50,8 @@ protocol: measured_trials: 20 sample_interval: 80 validation_samples: 256 - position_threshold_m: 0.05 - rotation_threshold_rad: 0.3 + position_threshold_m: 0.01 + rotation_threshold_rad: 0.1 joint_limit_tolerance_rad: 0.00001 tracks: diff --git a/scripts/benchmark/motion_generation/suites/smoke.yaml b/scripts/benchmark/motion_generation/suites/smoke.yaml index fc5a7087b..eafbbc5a6 100644 --- a/scripts/benchmark/motion_generation/suites/smoke.yaml +++ b/scripts/benchmark/motion_generation/suites/smoke.yaml @@ -50,8 +50,8 @@ protocol: measured_trials: 3 sample_interval: 40 validation_samples: 128 - position_threshold_m: 0.05 - rotation_threshold_rad: 0.3 + position_threshold_m: 0.01 + rotation_threshold_rad: 0.1 joint_limit_tolerance_rad: 0.00001 tracks: diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index f00778e82..ff83c0853 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -339,6 +339,31 @@ def test_free_space_cases_use_one_start_state_bin_each(): assert len({case.case_id for case in cases}) == 2 +def test_free_space_starts_align_across_path_shape_and_waypoint_count(): + """random_reachable starts must be shared across shapes/W for fair compares.""" + suite = load_suite("smoke") + suite.free_space.batch_sizes = [2] + suite.free_space.waypoint_counts = [1, 5] + suite.free_space.path_shapes = ["direct", "s_curve"] + suite.free_space.seeds = [11] + suite.free_space.start_state_bins = ["random_reachable"] + track = suite.enabled_tracks()[0] + cases = create_scenario_provider("free_space").generate_cases( + suite, track, _FrankaLimitRobot(), "arm", batch_size=2 + ) + + assert len(cases) == 4 + assert {(case.path_shape, case.num_waypoints) for case in cases} == { + ("direct", 1), + ("direct", 5), + ("s_curve", 1), + ("s_curve", 5), + } + reference = cases[0].start_qpos + for case in cases[1:]: + assert torch.equal(case.start_qpos, reference) + + def test_success_metrics_are_stratified_by_start_state_bin(): metadata = [ PlannerMetadata( @@ -880,6 +905,8 @@ def test_report_contains_exactly_three_markdown_tables(tmp_path): assert text.count("## Time & Memory") == 1 assert text.count("## Success & Other Metrics") == 1 assert text.count("## Leaderboard") == 1 + assert "motion-validity gate" in text + assert "n_valid" in text def test_curobo_prepare_backend_exposes_actual_graph_mode(): @@ -967,6 +994,7 @@ def test_success_table_uses_case_macro_and_counts_cases_not_env_slots(): "success_and_metrics" ][0] assert row["cases"] == 2 + assert row["n_valid"] == 1 # One measured success + one missing case (counts as 0) → macro 0.5. assert row["success_rate"] == pytest.approx(0.5) assert row["coverage_rate"] == pytest.approx(0.5) @@ -1011,6 +1039,7 @@ def test_success_table_uses_case_macro_and_counts_cases_not_env_slots(): "success_and_metrics" ][0] assert large_row["cases"] == 1 + assert large_row["n_valid"] == 0 assert large_row["success_rate"] == pytest.approx(0.0) From 1b94887e9095a196418aefcc7ba4f2d9460e4a86 Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 04:45:50 +0800 Subject: [PATCH 16/17] Add tests for joint-limit --- .../motion_generation/metrics/trajectory.py | 13 ++++++----- .../test_motion_generation_benchmark.py | 22 +++++++++++++++++++ 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/scripts/benchmark/motion_generation/metrics/trajectory.py b/scripts/benchmark/motion_generation/metrics/trajectory.py index a977fcc25..fff97a6f1 100644 --- a/scripts/benchmark/motion_generation/metrics/trajectory.py +++ b/scripts/benchmark/motion_generation/metrics/trajectory.py @@ -377,11 +377,14 @@ def compute_case_outcomes( rot_errors_deg = [ float(value) * 180.0 / math.pi for value in matching["rotation_errors_rad"] ] - joint_violation, normalized_violation = _joint_limit_metrics( - native_qpos, - limits[env_index], - joint_limit_tolerance_rad, - ) + if finite: + joint_violation, normalized_violation = _joint_limit_metrics( + native_qpos, + limits[env_index], + joint_limit_tolerance_rad, + ) + else: + joint_violation, normalized_violation = False, None ordered = bool(matching["ordered_waypoints_reached"]) planner_ok = bool(planning_success[env_index].item()) # ``PlanResult.success`` is retained as a planner-stage outcome, but it diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index ff83c0853..d98beaa66 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -260,6 +260,28 @@ def test_missing_positions_and_joint_limit_violation_fail_motion_valid(): assert violated[0].failure_code == "joint_limit_violation" +def test_non_finite_trajectory_skips_joint_limit_metrics(): + case = _case() + positions = torch.zeros(1, 2, 7) + positions[0, 1, 0] = float("inf") + outcomes = compute_case_outcomes( + PlanResult(success=True, positions=positions), + case, + _MetricRobot(), + "arm", + validation_samples=8, + position_threshold_m=1.0e-4, + rotation_threshold_rad=1.0e-4, + joint_limit_tolerance_rad=1.0e-5, + ) + + assert outcomes[0].finite is False + assert outcomes[0].failure_code == "non_finite_trajectory" + assert outcomes[0].joint_limit_violation is False + assert outcomes[0].max_normalized_joint_violation is None + assert outcomes[0].motion_valid is False + + def test_nmg_precision_and_external_accuracy_are_independently_configurable(): suite = load_suite("smoke") _apply_overrides( From d33d276208d07c7939d9a2300ef66218de13514e Mon Sep 17 00:00:00 2001 From: yangchen73 Date: Tue, 4 Aug 2026 04:49:27 +0800 Subject: [PATCH 17/17] Don't need repeated start_qpos/control_part --- scripts/benchmark/motion_generation/planners/curobo.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/scripts/benchmark/motion_generation/planners/curobo.py b/scripts/benchmark/motion_generation/planners/curobo.py index b21081487..2b4e34716 100644 --- a/scripts/benchmark/motion_generation/planners/curobo.py +++ b/scripts/benchmark/motion_generation/planners/curobo.py @@ -125,10 +125,7 @@ def plan(self, case: BenchmarkCase) -> PlanResult: MotionGenOptions( start_qpos=case.start_qpos, control_part=self.context.control_part, - plan_opts=CuroboPlanOptions( - start_qpos=case.start_qpos, - control_part=self.context.control_part, - ), + plan_opts=CuroboPlanOptions(), ), )