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/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/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/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..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,24 +50,11 @@ def _run_rl_cli(_: argparse.Namespace) -> None: rl_main() -def _run_neural_planner_cli(args: argparse.Namespace) -> None: - """Run NeuralPlanner benchmark with forwarded CLI args.""" - from scripts.benchmark.planners.neural_planner.run_benchmark import ( - run_all_benchmarks, - ) +def _run_motion_generation_cli(args: argparse.Namespace) -> None: + """Run the free-space motion-generation benchmark.""" + from scripts.benchmark.motion_generation.run_benchmark import 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: @@ -124,76 +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 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.", + # -- motion-generation --------------------------------------------------- + from scripts.benchmark.motion_generation.run_benchmark import ( + add_parser_arguments, ) - neural_planner_parser.add_argument( - "--sample-interval", - type=int, - default=20, - help="Resampled trajectory length for ik_interpolate and ik_toppra.", - ) - 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.", + + motion_generation_parser = subparsers.add_parser( + "motion-generation", + help="Benchmark free-space motion generation with cuRobo as baseline.", ) - neural_planner_parser.set_defaults(func=_run_neural_planner_cli) + 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/curobo_extraction/__init__.py b/scripts/benchmark/curobo_extraction/__init__.py new file mode 100644 index 000000000..02562579c --- /dev/null +++ b/scripts/benchmark/curobo_extraction/__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. +# ---------------------------------------------------------------------------- + +"""cuRobo post-processing hot-path microbenchmark.""" + +from __future__ import annotations + +__all__: list[str] = [] 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..75bcef44c 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,8 +170,8 @@ Reuse established patterns from the current benchmark system: Refactor the current monolithic script incrementally into: ```text -scripts/benchmark/planners/neural_planner/ -├── run_benchmark.py # thin CLI and compatibility entry point +scripts/benchmark/motion_generation/ +├── 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 new file mode 100644 index 000000000..9ec7ce42d --- /dev/null +++ b/scripts/benchmark/motion_generation/README.md @@ -0,0 +1,36 @@ +# Motion Generation Benchmark + +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 +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//` +(`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 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/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py new file mode 100644 index 000000000..25dbd45db --- /dev/null +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -0,0 +1,501 @@ +# ---------------------------------------------------------------------------- +# 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 .metrics.stats import nearest_rank_percentile +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 _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 _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 _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( + outcome.failure_code for outcome in outcomes if outcome.failure_code + ) + 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} + 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, + waypoint_count: int | None = None, +) -> float | None: + """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 + + +def _performance_rows( + records: list[TrialRecord], + metadata: list[PlannerMetadata], + cases: list[BenchmarkCase], +) -> list[dict[str, object]]: + """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.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): + 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": 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, track, algorithm_id, batch_size, TrialPhase.CONSTRUCT + ), + "backend_prepare_ms": _lifecycle_value( + records, track, algorithm_id, batch_size, TrialPhase.PREPARE + ), + "cold_plan_ms": _lifecycle_value( + records, + track, + algorithm_id, + batch_size, + TrialPhase.COLD, + waypoint_count=waypoint_count, + ), + "cost_time_ms": mean_cost, + "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_waypoint_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": _peak_gpu(group), + } + ) + + 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_waypoint_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), + ), + ) + + +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. + + 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: + continue + key = ( + record.track, + record.algorithm_id, + record.scenario_id, + record.batch_size, + record.waypoint_count, + record.path_shape, + record.start_state_bin, + ) + measured_by_key[key].append(record) + + expected_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, + 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 + cases_by_group[key].append(case) + + rows: list[dict[str, object]] = [] + for info in metadata: + for group_key in sorted(expected_by_group): + ( + track, + scenario_id, + batch_size, + waypoint_count, + path_shape, + start_state_bin, + ) = group_key + group_cases = cases_by_group[group_key] + measured = measured_by_key.get( + ( + track, + info.algorithm_id, + scenario_id, + batch_size, + waypoint_count, + path_shape, + start_state_bin, + ), + [], + ) + 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( + { + "track": track, + "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, + "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( + measured, group_cases, "motion_valid" + ), + "planning_success_rate": _case_macro_rate( + measured, group_cases, "planning_success" + ), + "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 + ), + "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": _case_macro_rate( + measured, group_cases, "joint_limit_violation" + ), + "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 per track. + + Success rates are macro-averaged over mandatory cases (equal case weight). + ``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): + 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 = _case_macro_rate(measured, track_cases, "motion_valid") + planning_rate = _case_macro_rate(measured, track_cases, "planning_success") + latency_p95 = _case_macro_latency_p95(measured, track_cases) + peak_gpu = _peak_gpu(measured) + 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, + # free-space v1: primary_success == motion_valid + "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, + } + ) + + 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"]), + ) + ) + entries.extend( + {"rank": rank, **entry} for rank, entry in enumerate(track_entries, start=1) + ) + return entries + + +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, cases), + "success_and_metrics": _metric_rows(records, metadata, cases, measured_trials), + "leaderboard": _leaderboard_rows(records, metadata, cases, measured_trials), + } diff --git a/scripts/benchmark/motion_generation/artifacts.py b/scripts/benchmark/motion_generation/artifacts.py new file mode 100644 index 000000000..4544230f6 --- /dev/null +++ b/scripts/benchmark/motion_generation/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_bin": case.start_state_bin, + "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/motion_generation/config.py b/scripts/benchmark/motion_generation/config.py new file mode 100644 index 000000000..61fb07019 --- /dev/null +++ b/scripts/benchmark/motion_generation/config.py @@ -0,0 +1,294 @@ +# ---------------------------------------------------------------------------- +# 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 motion-generation 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", + "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", + "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.01 + rotation_threshold_rad: float = 0.1 + 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 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.""" + + schema_version: int = 1 + name: str = "free_space_common" + suite_version: str = "free_space_common_v2" + 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")), + suite_version=str(data.get("suite_version", "free_space_common_v2")), + profile=str(data.get("profile", "smoke")), + planners=planners, + protocol=ProtocolCfg(**data.get("protocol", {})), + 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( + "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 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: + 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 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: + 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 _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) + 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.""" + suite.sync_track_configs() + data = asdict(suite) + data.pop("free_space", None) + return data + + +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/motion_generation/metrics/__init__.py b/scripts/benchmark/motion_generation/metrics/__init__.py new file mode 100644 index 000000000..1a2db20cf --- /dev/null +++ b/scripts/benchmark/motion_generation/metrics/__init__.py @@ -0,0 +1,38 @@ +# ---------------------------------------------------------------------------- +# 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, + make_failure_outcomes, + match_ordered_waypoints, +) + +__all__ = [ + "TimedCall", + "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 new file mode 100644 index 000000000..794d65aff --- /dev/null +++ b/scripts/benchmark/motion_generation/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 | None + + +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 None + ) + 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/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 new file mode 100644 index 000000000..fff97a6f1 --- /dev/null +++ b/scripts/benchmark/motion_generation/metrics/trajectory.py @@ -0,0 +1,459 @@ +# ---------------------------------------------------------------------------- +# 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 dataclasses import replace +from typing import TYPE_CHECKING + +import torch + +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 + +__all__ = [ + "compute_case_outcomes", + "compute_waypoint_errors", + "get_pose_err", + "make_failure_outcomes", + "match_ordered_waypoints", +] + + +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 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 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: + return { + "ordered_waypoints_reached": False, + "completed_waypoint_ratio": 0.0, + "arrival_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 + + position_errors = [ + float(pos_error[index, sample].item()) + for index, sample in enumerate(arrival_indices) + ] + rotation_errors = [ + float(rot_error[index, sample].item()) + for index, sample in enumerate(arrival_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, + "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.01, + rotation_threshold_rad: float = 0.1, +) -> dict[str, float]: + """Return ordered, same-sample waypoint errors for one trajectory.""" + 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, + *, + planner_failure_code: str | None = None, + planning_success: bool = False, +) -> tuple[CaseOutcome, ...]: + """Create per-env outcomes when validation cannot produce a trajectory.""" + return tuple( + CaseOutcome( + env_index=index, + planning_success=planning_success, + 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, + planner_failure_code=planner_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( + 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, + ) + 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"] + ] + 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 + # 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 + + 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( + 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=nearest_rank_percentile( + pos_errors_mm, 95.0 + ), + 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=nearest_rank_percentile( + rot_errors_deg, 95.0 + ), + 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, + planner_failure_code=planner_failure_code, + ) + ) + return tuple(outcomes) diff --git a/scripts/benchmark/motion_generation/models.py b/scripts/benchmark/motion_generation/models.py new file mode 100644 index 000000000..02868bc78 --- /dev/null +++ b/scripts/benchmark/motion_generation/models.py @@ -0,0 +1,149 @@ +# ---------------------------------------------------------------------------- +# 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_bin: 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 + planner_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 + start_state_bin: 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/motion_generation/planners/__init__.py b/scripts/benchmark/motion_generation/planners/__init__.py new file mode 100644 index 000000000..65b3edabc --- /dev/null +++ b/scripts/benchmark/motion_generation/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/motion_generation/planners/base.py b/scripts/benchmark/motion_generation/planners/base.py new file mode 100644 index 000000000..895e5f11b --- /dev/null +++ b/scripts/benchmark/motion_generation/planners/base.py @@ -0,0 +1,102 @@ +# ---------------------------------------------------------------------------- +# 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_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 new file mode 100644 index 000000000..2b4e34716 --- /dev/null +++ b/scripts/benchmark/motion_generation/planners/curobo.py @@ -0,0 +1,137 @@ +# ---------------------------------------------------------------------------- +# 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(), + ), + ) + + def close(self) -> None: + """Destroy cached cuRobo graph and planner resources.""" + self._close_motion_generator() + + +register_planner_adapter("curobo", CuroboAdapter) diff --git a/scripts/benchmark/motion_generation/planners/ik_interpolate.py b/scripts/benchmark/motion_generation/planners/ik_interpolate.py new file mode 100644 index 000000000..fa241b386 --- /dev/null +++ b/scripts/benchmark/motion_generation/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/motion_generation/planners/neural.py b/scripts/benchmark/motion_generation/planners/neural.py new file mode 100644 index 000000000..20af60396 --- /dev/null +++ b/scripts/benchmark/motion_generation/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/motion_generation/planners/toppra.py b/scripts/benchmark/motion_generation/planners/toppra.py new file mode 100644 index 000000000..038dbc0a7 --- /dev/null +++ b/scripts/benchmark/motion_generation/planners/toppra.py @@ -0,0 +1,95 @@ +# ---------------------------------------------------------------------------- +# 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, + ), + ), + ) + + def close(self) -> None: + """Release TOPPRA worker pools and drop the motion generator.""" + 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 new file mode 100644 index 000000000..d10efb3fe --- /dev/null +++ b/scripts/benchmark/motion_generation/registry.py @@ -0,0 +1,102 @@ +# ---------------------------------------------------------------------------- +# 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 and scenario registries 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 + from .scenarios.base import ScenarioProvider + +__all__ = [ + "create_planner_adapter", + "create_scenario_provider", + "planner_adapter_names", + "register_planner_adapter", + "register_scenario_provider", + "scenario_provider_names", + "unregister_planner_adapter", +] + +_PLANNER_ADAPTERS: dict[str, type["PlannerAdapter"]] = {} +_SCENARIO_PROVIDERS: dict[str, type["ScenarioProvider"]] = {} + + +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 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)) + + +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) + + +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/motion_generation/reporting.py b/scripts/benchmark/motion_generation/reporting.py new file mode 100644 index 000000000..e5d26af63 --- /dev/null +++ b/scripts/benchmark/motion_generation/reporting.py @@ -0,0 +1,172 @@ +# ---------------------------------------------------------------------------- +# 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_waypoint_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", + "start_state_bin", + "cases", + "n_valid", + "coverage_rate", + "success_rate", + "planning_success_rate", + "ordered_waypoint_success_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` " + "(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", + "", + "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)) + 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/motion_generation/run_benchmark.py b/scripts/benchmark/motion_generation/run_benchmark.py new file mode 100644 index 000000000..4a9ff9ee6 --- /dev/null +++ b/scripts/benchmark/motion_generation/run_benchmark.py @@ -0,0 +1,284 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Run the extensible free-space motion-generation benchmark. + +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. + +Run: ``embodichain benchmark motion-generation --suite smoke`` +""" + +from __future__ import annotations + +import argparse +from copy import deepcopy +from pathlib import Path +from typing import TYPE_CHECKING + +from .config import PlannerSpecCfg, SuiteCfg, load_suite + +if TYPE_CHECKING: + from .runner import BenchmarkRunResult + +__all__ = [ + "add_parser_arguments", + "run_all_benchmarks", + "run_from_args", +] + + +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( + "--algorithms", + nargs="+", + default=None, + help="Override enabled suite algorithms by id.", + ) + parser.add_argument( + "--extra-baselines", + nargs="+", + choices=("ik_interpolate", "toppra"), + default=[], + help="Enable optional diagnostic baselines.", + ) + parser.add_argument( + "--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("--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) + 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( + "--nmg-rot-eps", + type=float, + default=None, + help="NMG internal waypoint rotation threshold in radians.", + ) + parser.add_argument( + "--checkpoint-path", + default=None, + help="Reserved NMG checkpoint path; the current NMG adapter remains a stub.", + ) + parser.add_argument( + "--output-root", default="outputs/benchmarks", help="Artifact root directory." + ) + parser.add_argument( + "--headless", action="store_true", default=True, help="Run headlessly." + ) + parser.add_argument( + "--no-headless", action="store_false", dest="headless", help="Open a viewer." + ) + + +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 _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] + ) + 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 _apply_overrides( + suite: SuiteCfg, + *, + 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, + 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: + """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 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: + 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( + num_waypoints_list: list[int] | None = None, + sim_device: str = "auto", + headless: bool = True, + checkpoint_path: str | None = None, + *, + suite_name: str = "smoke", + 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, + 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, + 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, + path_shapes=path_shapes, + start_state_bins=start_state_bins, + seeds=seeds, + num_trials=num_trials, + warmup_trials=warmup_trials, + sample_interval=sample_interval, + 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, + ) + specs = _resolve_planners(suite, algorithms, list(extra_baselines or [])) + 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, + 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, + 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, + output_root=args.output_root, + ) + + +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." + ) + add_parser_arguments(parser) + return parser.parse_args() + + +if __name__ == "__main__": + run_from_args(_parse_args()) diff --git a/scripts/benchmark/motion_generation/runner.py b/scripts/benchmark/motion_generation/runner.py new file mode 100644 index 000000000..6f6badc87 --- /dev/null +++ b/scripts/benchmark/motion_generation/runner.py @@ -0,0 +1,505 @@ +# ---------------------------------------------------------------------------- +# 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 motion-generation benchmark tracks.""" + +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 . import scenarios as _builtin_scenarios # 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, create_scenario_provider +from .reporting import write_markdown_report + +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, + "start_state_bin": case.start_state_bin, + "phase": phase, + } + + def _record_unavailable( + self, + writer: TrialJsonlWriter, + metadata: PlannerMetadata, + case: BenchmarkCase, + reason: str, + *, + failure_code: str, + ) -> None: + """Record an unsupported/unavailable planner without counting a failure.""" + self._append( + writer, + TrialRecord( + **self._base_record(metadata, case, TrialPhase.AVAILABILITY), + status="unsupported", + failure_code=failure_code, + 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, + 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, + ) -> 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))) + 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 in (TrialPhase.WARMUP, TrialPhase.COLD): + # Cold/warmup timing must not pay for FK validation that is unused + # by aggregation. + 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}" + ) + + def _run_adapter( + self, + writer: TrialJsonlWriter, + sim: SimulationManager, + 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( + 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] + 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", + failure_code="runtime_unavailable", + ) + return + + _, build_error = self._record_timed_lifecycle( + writer, + metadata, + first_case, + TrialPhase.CONSTRUCT, + 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, + 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 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 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, + provider.required_capabilities, + ) + 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.", + "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 / " + "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.", + "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.", + "Waypoint continuous errors use the same threshold-greedy arrival matching " + "as ordered_waypoints_reached / motion_valid.", + *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/motion_generation/scenarios/__init__.py b/scripts/benchmark/motion_generation/scenarios/__init__.py new file mode 100644 index 000000000..e959d74df --- /dev/null +++ b/scripts/benchmark/motion_generation/scenarios/__init__.py @@ -0,0 +1,24 @@ +# ---------------------------------------------------------------------------- +# 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 .base import ScenarioProvider +from .free_space import FreeSpaceScenario + +__all__ = ["FreeSpaceScenario", "ScenarioProvider"] diff --git a/scripts/benchmark/motion_generation/scenarios/base.py b/scripts/benchmark/motion_generation/scenarios/base.py new file mode 100644 index 000000000..11435cb71 --- /dev/null +++ b/scripts/benchmark/motion_generation/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/motion_generation/scenarios/free_space.py b/scripts/benchmark/motion_generation/scenarios/free_space.py new file mode 100644 index 000000000..ce2b7fff4 --- /dev/null +++ b/scripts/benchmark/motion_generation/scenarios/free_space.py @@ -0,0 +1,245 @@ +# ---------------------------------------------------------------------------- +# 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, 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__ = ["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], + dtype=torch.float32, +) + + +def _clamp_with_margin(qpos: torch.Tensor, limits: torch.Tensor) -> torch.Tensor: + """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) + + +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. + + ``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 + 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": + # 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) + 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, + start_state_bin: str, + bin_index: int, + track_id: str, +) -> BenchmarkCase: + """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( + "free-space-common v1 expects a 7-DoF Franka arm, got " + f"{limits.shape[0]} DoF." + ) + + starts: list[torch.Tensor] = [] + for env_index in range(batch_size): + generator = torch.Generator(device="cpu") + # 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) + 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, + "track": track_id, + "seed": seed, + "batch_size": batch_size, + "num_waypoints": num_waypoints, + "path_shape": path_shape, + "start_state_bin": start_state_bin, + } + case_id = f"free_space_{stable_hash(identity)[:16]}" + return BenchmarkCase( + suite_version=suite.suite_version, + track=track_id, + 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_bin=start_state_bin, + start_qpos=start_qpos, + target_waypoints=target_waypoints, + reference_qpos=reference_qpos, + ) + + +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 path_shape in 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, + start_state_bin=start_state_bin, + bin_index=bin_index, + track_id=track.id, + ) + ) + return cases + + +register_scenario_provider("free_space", FreeSpaceScenario) diff --git a/scripts/benchmark/motion_generation/suites/coverage.yaml b/scripts/benchmark/motion_generation/suites/coverage.yaml new file mode 100644 index 000000000..ebcd8faa9 --- /dev/null +++ b/scripts/benchmark/motion_generation/suites/coverage.yaml @@ -0,0 +1,66 @@ +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.01 + rotation_threshold_rad: 0.1 + joint_limit_tolerance_rad: 0.00001 + +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/motion_generation/suites/smoke.yaml b/scripts/benchmark/motion_generation/suites/smoke.yaml new file mode 100644 index 000000000..eafbbc5a6 --- /dev/null +++ b/scripts/benchmark/motion_generation/suites/smoke.yaml @@ -0,0 +1,66 @@ +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.01 + rotation_threshold_rad: 0.1 + joint_limit_tolerance_rad: 0.00001 + +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/scripts/benchmark/planners/neural_planner/run_benchmark.py b/scripts/benchmark/planners/neural_planner/run_benchmark.py deleted file mode 100644 index 45adceacd..000000000 --- a/scripts/benchmark/planners/neural_planner/run_benchmark.py +++ /dev/null @@ -1,1284 +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. -# ---------------------------------------------------------------------------- - -"""Benchmark NeuralPlanner in isolation on Franka Panda. - -What this measures - Planning latency, memory, rollout steps, and final TCP pose error for - ``NeuralPlanner`` on fixed demo EEF waypoint sets. - -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 -""" - -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 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 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, -] - -IMPL_NEURAL = "neural_planner" -IMPL_IK = "ik_interpolate" -IMPL_TOPPRA = "ik_toppra" - - -def _parse_args() -> argparse.Namespace: - """Parse command line arguments for neural motion generator benchmarks.""" - parser = argparse.ArgumentParser( - description="Benchmark NeuralPlanner planning latency and quality." - ) - parser.add_argument( - "--device", - choices=("auto", "cpu", "cuda"), - default="auto", - help="Simulation and planner device. Auto uses CUDA when available.", - ) - parser.add_argument( - "--num-waypoints", - nargs="+", - type=int, - default=DEFAULT_NUM_WAYPOINTS, - help="Number of EEF waypoints to sweep.", - ) - parser.add_argument( - "--num-trials", - type=int, - default=DEFAULT_NUM_TRIALS, - help="Measured trials per (impl, num_waypoints) configuration.", - ) - parser.add_argument( - "--warmup-trials", - type=int, - default=DEFAULT_WARMUP_TRIALS, - help="Warmup trials per configuration; excluded from summary aggregation.", - ) - parser.add_argument( - "--sample-interval", - type=int, - default=DEFAULT_SAMPLE_INTERVAL, - help="Resampled trajectory length for ik_interpolate and ik_toppra.", - ) - parser.add_argument( - "--compare-ik", - action="store_true", - help="Also benchmark sequential IK plus joint interpolation.", - ) - parser.add_argument( - "--compare-toppra", - action="store_true", - help="Also benchmark EEF IK interpolation followed by TOPPRA.", - ) - parser.add_argument( - "--save-trial-details", - action="store_true", - help="Include per-trial rows in the markdown report.", - ) - parser.add_argument( - "--checkpoint-path", - type=str, - default=None, - help="Local neural planner checkpoint path. Skips HuggingFace download.", - ) - parser.add_argument( - "--headless", - action="store_true", - default=True, - help="Run simulation headlessly (default: True).", - ) - 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, - ), - ) - - -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 _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 - ) - 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, - } - - -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]], - *, - 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]], -) -> 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, - ) - - -def run_all_benchmarks( - num_waypoints_list: list[int] | None = None, - sim_device: str = "auto", - 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, - 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, - num_trials=num_trials, - warmup_trials=warmup_trials, - sample_interval=sample_interval, - compare_ik=compare_ik, - compare_toppra=compare_toppra, - ) - 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, - ) - print(f"Markdown report saved: {report_path}") - - -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, - ) 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/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py new file mode 100644 index 000000000..d98beaa66 --- /dev/null +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -0,0 +1,1251 @@ +# ---------------------------------------------------------------------------- +# 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 dataclasses import replace +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.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, + compute_waypoint_errors, + match_ordered_waypoints, +) +from scripts.benchmark.motion_generation import ( + scenarios as _scenarios, +) # noqa: F401 +from scripts.benchmark.motion_generation.models import ( + AlgorithmRole, + BenchmarkCase, + CaseOutcome, + PlannerMetadata, + TrialPhase, + TrialRecord, +) +from scripts.benchmark.motion_generation.registry import ( + create_scenario_provider, + scenario_provider_names, +) +from scripts.benchmark.motion_generation.reporting import ( + write_markdown_report, +) +from scripts.benchmark.motion_generation.run_benchmark import ( + _apply_overrides, +) + + +def _translated_pose(x: float) -> torch.Tensor: + pose = torch.eye(4) + pose[0, 3] = x + 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)]) + + 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"] == [] + + +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 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([[self.limit_lo, self.limit_hi]]).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 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 _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, + _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 is None + assert outcomes[0].planner_failure_code == "planner_reported_failure" + + 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, + outcomes=outcomes, + ) + 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 + + +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_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( + 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 _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 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 = _FrankaLimitRobot() + provider = create_scenario_provider("free_space") + track = suite.enabled_tracks()[0] + + 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(): + 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"] + track = suite.enabled_tracks()[0] + cases = create_scenario_provider("free_space").generate_cases( + suite, track, _FrankaLimitRobot(), "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_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( + 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"]["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" + + +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_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), + ) + + +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", + start_state_bin="nominal", + 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_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_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 = [ + 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 = { + "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 + assert "motion-validity gate" in text + assert "n_valid" in text + + +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 + + +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 + + +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 + 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) + + 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["n_valid"] == 0 + 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, + unregister_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") + + class _RuntimeUnavailableFake(PlannerAdapter): + capabilities = frozenset({"eef_waypoint", "batched", "empty_world"}) + + 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", + ) + 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", + role=AlgorithmRole.DIAGNOSTIC_BASELINE.value, + enabled=True, + ), + PlannerSpecCfg( + id="incapable", + adapter="fake_incapable", + role=AlgorithmRole.CANDIDATE.value, + enabled=True, + ), + 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 + ) + + 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) 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 2abdeb19f..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.planners.neural_planner.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)