From e3f9e77a1816f7d01361d92fa48f35eae03ea7a5 Mon Sep 17 00:00:00 2001 From: Jerrybery Date: Mon, 3 Aug 2026 12:18:24 +0800 Subject: [PATCH 1/5] feat(manipulation): add OInK vs Pink IK benchmark (#3232) --- dimos/manipulation/benchmark_ik_backends.md | 98 +++++ dimos/manipulation/benchmark_ik_backends.py | 373 ++++++++++++++++++++ 2 files changed, 471 insertions(+) create mode 100644 dimos/manipulation/benchmark_ik_backends.md create mode 100644 dimos/manipulation/benchmark_ik_backends.py diff --git a/dimos/manipulation/benchmark_ik_backends.md b/dimos/manipulation/benchmark_ik_backends.md new file mode 100644 index 0000000000..f561ce2b9c --- /dev/null +++ b/dimos/manipulation/benchmark_ik_backends.md @@ -0,0 +1,98 @@ +# IK Backend Benchmark: RoboPlan OInK vs Pink + +Benchmark comparing the two manipulation IK backends on representative DimOS +workloads, for [issue #3232](https://github.com/dimensionalOS/dimos/issues/3232). +Related implementation: [PR #3230](https://github.com/dimensionalOS/dimos/pull/3230) +(RoboPlan-native OInK backend). + +## Methodology + +`dimos/manipulation/benchmark_ik_backends.py`: + +- Robots: xArm6 and xArm7 (`make_xarm6_model_config()` / `make_xarm7_model_config()`, + with gripper, `limited=true`). +- One `RoboPlanWorld` per robot is shared by both solvers. Pink is world-agnostic + (builds its own Pinocchio model); RoboPlan OInK *is* the world + (`RoboPlanKinematicsConfig` returns the world cast to `KinematicsSpec`). Same + world, same collision model, same FK for both. +- Targets: joint configurations sampled uniformly within limits, filtered to + collision-free ones, mapped through world FK. Every target is reachable by + construction, so failures measure solver behavior, not unreachable goals. +- Both backends get identical targets and the same seed joint state (live robot + state), `check_collision=True`, `max_attempts=10`, default tolerances + (1 mm / 0.01 rad). +- Latency: `time.perf_counter` around `KinematicsSpec.solve`. 10 warmup solves + per backend discarded. +- Accuracy: successful solutions are independently re-verified by pushing them + through the world's FK and scoring against the target with + `compute_pose_error` (same metric for both backends). +- Resource usage: coarse peak-RSS delta (`resource.ru_maxrss`) across each + backend's measured window; model construction happens in warmup. +- Hardware: 16-core x86_64 (Ubuntu 20.04), Python 3.12, `pin-pink 4.2.0`, + `roboplan 0.0.100`. 200 timed solves per (robot, backend), seed 0. + +## Results (seed 0, 200 samples) + +| robot | backend | success | p50 ms | p95 ms | mean ms | pos err p50 (mm) | pos err p95 (mm) | +|-------|---------|---------|--------|--------|---------|------------------|-------------------| +| xarm6 | pink | 87.5% | 29.6 | 512.2 | 123.8 | 0.74 | 0.97 | +| xarm6 | roboplan_oink | 84.5% | 31.3 | 99.8 | 38.4 | 0.27 | 0.86 | +| xarm7 | pink | 99.0% | 8.6 | 207.5 | 38.9 | 0.74 | 0.96 | +| xarm7 | roboplan_oink | 97.0% | 5.1 | 66.5 | 16.4 | 0.53 | 0.97 | + +Orientation error means are comparable (0.5–1.2 mrad; tolerance is 10 mrad). +Verified max position error is ~1.0 mm for both backends, i.e. both honor the +position tolerance exactly when they report success. Peak RSS delta was ≤ 2 MB +for both backends — per-solve memory pressure is negligible once models are +loaded. + +Failure breakdown: + +| robot | backend | status counts | +|-------|---------|---------------| +| xarm6 | pink | 19 NO_SOLUTION, 3 COLLISION, 3 JOINT_LIMITS | +| xarm6 | roboplan_oink | 31 NO_SOLUTION | +| xarm7 | pink | 2 NO_SOLUTION | +| xarm7 | roboplan_oink | 3 NO_SOLUTION, 3 COLLISION (converged only to colliding endpoints) | + +## Observed tradeoffs + +- **OInK has a much tighter latency tail.** Mean 2–3.2x faster, p95 3–5x lower. + Pink's slow solves are its failures: on xarm6, failed solves average 343 ms + (burnt on restarts that exhaust the 200-iteration budget) vs 92 ms for + successes. OInK failures cost ~100 ms (10 bounded attempts x 100 iterations). + For planning loops that call IK repeatedly, OInK's worst case is ~5x cheaper. +- **Pink succeeds slightly more often.** +3 pp on xarm6 (87.5 vs 84.5), +2 pp on + xarm7 (99.0 vs 97.0). Pink's QP formulation with per-attempt restarts within + joint limits rescues some targets OInK gives up on. +- **The backends fail on different targets.** Only 6 of 200 xarm6 targets fail + under both; zero overlap on xarm7. They are complementary: a fallback chain + (OInK first, Pink on NO_SOLUTION) would beat either alone on both latency and + success rate. +- **Failure modes differ in kind.** Pink fails three ways (no convergence, QP + failure, collision/joint-limit rejection of a converged solution). OInK only + ever returns NO_SOLUTION — it never reports success for an endpoint that + fails collision, and FK verification confirms every OInK success lands inside + tolerance. +- **Accuracy is a wash.** Both meet the 1 mm tolerance on every reported + success; OInK's median position error is somewhat lower (0.27 vs 0.74 mm on + xarm6). +- **Caveats.** Single-machine run; Pink solver config was the default + (`proxqp`, `dt=0.05`, 200 iterations) and OInK's 100 iterations/attempt is + currently hardcoded; targets are uniform-random reachable poses with no + obstacles beyond self-collision, so results don't cover cluttered-scene + workloads; RSS delta is process-wide and coarse. + +A rerun with `--seed 1` reproduced the same pattern (OInK mean 2–3.5x faster, +p95 ~5x tighter; success rates within ±3 pp, including one robot where OInK +scored higher), so the tradeoffs above are not seed-specific. + +## Reproduce + +```bash +uv sync --extra manipulation --inexact +uv run python dimos/manipulation/benchmark_ik_backends.py \ + --samples 200 --warmup 10 --seed 0 --output /tmp/ik_bench.json +``` + +Requires the `xarm_description` LFS data (`get_data("xarm_description")`). diff --git a/dimos/manipulation/benchmark_ik_backends.py b/dimos/manipulation/benchmark_ik_backends.py new file mode 100644 index 0000000000..34636e88ca --- /dev/null +++ b/dimos/manipulation/benchmark_ik_backends.py @@ -0,0 +1,373 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# 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 Pink vs RoboPlan OInK IK backends on representative DimOS manipulation workloads. + +Issue: https://github.com/dimensionalOS/dimos/issues/3232 + +For each supported robot, one RoboPlanWorld instance is shared by both solvers +(Pink is world-agnostic; RoboPlan OInK is the world itself). Targets are sampled +by drawing random collision-free joint configurations and mapping them through +the world's forward kinematics, so every target is reachable by construction. +Both backends receive identical targets and the same seed, are timed with +``time.perf_counter``, and every successful solution is independently verified +by pushing it back through the world's FK and comparing against the target with +``compute_pose_error``. + +Usage: + uv run python dimos/manipulation/benchmark_ik_backends.py --samples 200 + uv run python dimos/manipulation/benchmark_ik_backends.py --robots xarm7 --output /tmp/ik.json +""" + +from __future__ import annotations + +import argparse +from collections.abc import Sequence +from dataclasses import asdict, dataclass +import json +from pathlib import Path +import resource +import statistics +import time +from typing import Any + +import numpy as np + +from dimos.manipulation.planning.factory import create_kinematics, create_world +from dimos.manipulation.planning.kinematics.config import ( + PinkKinematicsConfig, + RoboPlanKinematicsConfig, +) +from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID +from dimos.manipulation.planning.spec.protocols import KinematicsSpec, WorldSpec +from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.robot.manipulators.xarm.config import ( + make_xarm6_model_config, + make_xarm7_model_config, +) +from dimos.utils.transform_utils import pose_to_matrix + +ROBOT_CONFIG_FACTORIES = { + "xarm6": make_xarm6_model_config, + "xarm7": make_xarm7_model_config, +} + +# Sampled targets whose FK pose is in collision are rejected; allow slack for rejection sampling. +MAX_TARGET_TRIES_FACTOR = 20 + + +@dataclass +class SolveRecord: + """One timed IK solve.""" + + robot: str + backend: str + target_index: int + status: str + wall_time_ms: float + position_error: float + orientation_error: float + iterations: int + verified_position_error: float | None + verified_orientation_error: float | None + message: str + + +@dataclass +class BackendRun: + """Records plus coarse resource usage for one backend on one robot.""" + + robot: str + backend: str + records: list[SolveRecord] + peak_rss_delta_mb: float + + +def _sample_reachable_targets( + world: WorldSpec, + robot_id: WorldRobotID, + group_id: str, + count: int, + rng: np.random.Generator, +) -> list[PoseStamped]: + """Sample reachable, collision-free targets via FK of random valid configurations.""" + lower, upper = world.get_joint_limits(robot_id) + reference = world.get_joint_state(world.get_live_context(), robot_id) + targets: list[PoseStamped] = [] + tries = 0 + with world.scratch_context() as ctx: + while len(targets) < count and tries < count * MAX_TARGET_TRIES_FACTOR: + tries += 1 + q = rng.uniform(lower, upper) + joint_state = JointState( + name=list(reference.name), + position=[float(v) for v in q], + ) + if not world.check_config_collision_free(robot_id, joint_state): + continue + world.set_joint_state(ctx, robot_id, joint_state) + pose = world.get_group_ee_pose(ctx, group_id) + # OInK requires world-frame targets; rebuild with an explicit frame_id. + targets.append( + PoseStamped( + position=pose.position, + orientation=pose.orientation, + frame_id="world", + ) + ) + if len(targets) < count: + raise RuntimeError( + f"Only sampled {len(targets)}/{count} collision-free targets after {tries} tries" + ) + return targets + + +def _local_solution_joint_state( + reference: JointState, + solution: JointState, +) -> JointState: + """Re-express a solution in the robot's local joint-name order. + + Pink returns config (local) joint names, OInK returns global ``robot/joint`` + names; positions are matched by local name so FK verification is backend-agnostic. + Joints absent from the solution keep their reference positions. + """ + by_local_name: dict[str, float] = {} + for name, position in zip(solution.name, solution.position, strict=True): + by_local_name[name.rsplit("/", 1)[-1]] = position + return JointState( + name=list(reference.name), + position=[ + by_local_name.get(name, ref) + for name, ref in zip(reference.name, reference.position, strict=True) + ], + ) + + +def _verify_solution( + world: WorldSpec, + robot_id: WorldRobotID, + group_id: str, + reference: JointState, + result: IKResult, + target: PoseStamped, +) -> tuple[float, float] | None: + """Push a successful solution through the world's FK and score against the target.""" + if not result.is_success() or result.joint_state is None: + return None + solution = _local_solution_joint_state(reference, result.joint_state) + with world.scratch_context() as ctx: + world.set_joint_state(ctx, robot_id, solution) + actual = world.get_group_ee_pose(ctx, group_id) + return compute_pose_error(pose_to_matrix(actual), pose_to_matrix(target)) + + +def _peak_rss_mb() -> float: + """Peak RSS of this process in MB (Linux ru_maxrss is KiB).""" + return resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024.0 + + +def _run_backend( + robot_name: str, + backend_name: str, + solver: KinematicsSpec, + world: WorldSpec, + robot_id: WorldRobotID, + group_id: str, + targets: Sequence[PoseStamped], + seed: JointState, + warmup: int, + max_attempts: int, +) -> BackendRun: + reference = world.get_joint_state(world.get_live_context(), robot_id) + + def solve_once(target: PoseStamped) -> tuple[IKResult, float]: + start = time.perf_counter() + result = solver.solve( + world, + robot_id, + target, + seed=seed, + check_collision=True, + max_attempts=max_attempts, + ) + return result, (time.perf_counter() - start) * 1000.0 + + for target in targets[:warmup]: + solve_once(target) + + rss_before = _peak_rss_mb() + records: list[SolveRecord] = [] + for index, target in enumerate(targets[warmup:]): + result, wall_time_ms = solve_once(target) + verified = _verify_solution(world, robot_id, group_id, reference, result, target) + records.append( + SolveRecord( + robot=robot_name, + backend=backend_name, + target_index=index, + status=result.status.name, + wall_time_ms=wall_time_ms, + position_error=result.position_error, + orientation_error=result.orientation_error, + iterations=result.iterations, + verified_position_error=verified[0] if verified else None, + verified_orientation_error=verified[1] if verified else None, + message=result.message, + ) + ) + return BackendRun( + robot=robot_name, + backend=backend_name, + records=records, + peak_rss_delta_mb=_peak_rss_mb() - rss_before, + ) + + +def _percentile(values: Sequence[float], pct: float) -> float: + return float(np.percentile(np.asarray(values), pct)) + + +def _summarize(run: BackendRun) -> dict[str, Any]: + successes = [r for r in run.records if r.status == "SUCCESS"] + latencies = [r.wall_time_ms for r in run.records] + verified_pos = [ + r.verified_position_error for r in successes if r.verified_position_error is not None + ] + verified_ori = [ + r.verified_orientation_error for r in successes if r.verified_orientation_error is not None + ] + status_counts: dict[str, int] = {} + for r in run.records: + status_counts[r.status] = status_counts.get(r.status, 0) + 1 + return { + "robot": run.robot, + "backend": run.backend, + "samples": len(run.records), + "success_rate": len(successes) / len(run.records) if run.records else 0.0, + "status_counts": status_counts, + "latency_ms": { + "mean": statistics.fmean(latencies) if latencies else 0.0, + "p50": _percentile(latencies, 50) if latencies else 0.0, + "p95": _percentile(latencies, 95) if latencies else 0.0, + "max": max(latencies) if latencies else 0.0, + }, + "verified_position_error_m": { + "mean": statistics.fmean(verified_pos) if verified_pos else None, + "max": max(verified_pos) if verified_pos else None, + }, + "verified_orientation_error_rad": { + "mean": statistics.fmean(verified_ori) if verified_ori else None, + "max": max(verified_ori) if verified_ori else None, + }, + "peak_rss_delta_mb": run.peak_rss_delta_mb, + } + + +def _print_summary(summaries: Sequence[dict[str, Any]]) -> None: + header = ( + f"{'robot':<8} {'backend':<14} {'n':>5} {'success':>8} " + f"{'p50 ms':>9} {'p95 ms':>9} {'mean ms':>9} " + f"{'pos mm':>8} {'ori mrad':>9} {'rss ΔMB':>8}" + ) + print(header) + print("-" * len(header)) + for s in summaries: + pos_mm = (s["verified_position_error_m"]["mean"] or 0.0) * 1000.0 + ori_mrad = (s["verified_orientation_error_rad"]["mean"] or 0.0) * 1000.0 + print( + f"{s['robot']:<8} {s['backend']:<14} {s['samples']:>5} " + f"{s['success_rate']:>7.1%} " + f"{s['latency_ms']['p50']:>9.2f} {s['latency_ms']['p95']:>9.2f} " + f"{s['latency_ms']['mean']:>9.2f} " + f"{pos_mm:>8.3f} {ori_mrad:>9.3f} {s['peak_rss_delta_mb']:>8.1f}" + ) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--robots", + nargs="+", + choices=sorted(ROBOT_CONFIG_FACTORIES), + default=sorted(ROBOT_CONFIG_FACTORIES), + help="Robots to benchmark (default: all).", + ) + parser.add_argument("--samples", type=int, default=200, help="Timed solves per backend.") + parser.add_argument( + "--warmup", type=int, default=5, help="Warmup solves discarded per backend." + ) + parser.add_argument("--max-attempts", type=int, default=10, help="IK attempts per solve.") + parser.add_argument("--seed", type=int, default=0, help="RNG seed for target sampling.") + parser.add_argument("--output", type=Path, default=None, help="Optional JSON output path.") + args = parser.parse_args() + + runs: list[BackendRun] = [] + for robot_name in args.robots: + print(f"[setup] building RoboPlanWorld for {robot_name} ...", flush=True) + config = ROBOT_CONFIG_FACTORIES[robot_name]() + world = create_world("roboplan") + robot_id = world.add_robot(config) + world.finalize() + group_id = f"{config.name}/manipulator" + + rng = np.random.default_rng(args.seed) + targets = _sample_reachable_targets( + world, robot_id, group_id, args.samples + args.warmup, rng + ) + seed = world.get_joint_state(world.get_live_context(), robot_id) + + solvers: dict[str, KinematicsSpec] = { + "pink": create_kinematics(config=PinkKinematicsConfig()), + "roboplan_oink": create_kinematics( + config=RoboPlanKinematicsConfig(), world=world, world_backend="roboplan" + ), + } + for backend_name, solver in solvers.items(): + print(f"[run] {robot_name} / {backend_name} ...", flush=True) + runs.append( + _run_backend( + robot_name, + backend_name, + solver, + world, + robot_id, + group_id, + targets, + seed, + args.warmup, + args.max_attempts, + ) + ) + + summaries = [_summarize(run) for run in runs] + _print_summary(summaries) + + if args.output is not None: + payload = { + "seed": args.seed, + "samples": args.samples, + "warmup": args.warmup, + "max_attempts": args.max_attempts, + "summaries": summaries, + "records": [asdict(record) for run in runs for record in run.records], + } + args.output.write_text(json.dumps(payload, indent=2)) + print(f"wrote {args.output}") + + +if __name__ == "__main__": + main() From 65086737ce0f7aa161aabb093effc774ec7c8436 Mon Sep 17 00:00:00 2001 From: Jerrybery Date: Tue, 4 Aug 2026 15:51:24 +0800 Subject: [PATCH 2/5] refactor(manipulation): move IK benchmark to benchmarks/, address review feedback - Relocate script to benchmarks/ik_backends.py (dedicated benchmark folder) - Replace results markdown with docs page on running/extending the benchmark - Switch CLI from argparse to typer (repo standard) - Return a RunSummary dataclass instead of a plain dict - Registry-based solver specs for easy extension to other IK backends - Build a fresh world per backend run so backends never share a mutable scene - Add --max-attempts/--pink-max-iterations knobs for tuning sweeps --- .../ik_backends.py | 294 ++++++++++-------- dimos/manipulation/benchmark_ik_backends.md | 98 ------ .../capabilities/manipulation/ik_benchmark.md | 65 ++++ docs/docs.json | 3 +- 4 files changed, 239 insertions(+), 221 deletions(-) rename dimos/manipulation/benchmark_ik_backends.py => benchmarks/ik_backends.py (52%) delete mode 100644 dimos/manipulation/benchmark_ik_backends.md create mode 100644 docs/capabilities/manipulation/ik_benchmark.md diff --git a/dimos/manipulation/benchmark_ik_backends.py b/benchmarks/ik_backends.py similarity index 52% rename from dimos/manipulation/benchmark_ik_backends.py rename to benchmarks/ik_backends.py index 34636e88ca..9595bbd7df 100644 --- a/dimos/manipulation/benchmark_ik_backends.py +++ b/benchmarks/ik_backends.py @@ -12,43 +12,45 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Benchmark Pink vs RoboPlan OInK IK backends on representative DimOS manipulation workloads. +"""Benchmark manipulation IK backends on representative DimOS workloads. Issue: https://github.com/dimensionalOS/dimos/issues/3232 -For each supported robot, one RoboPlanWorld instance is shared by both solvers -(Pink is world-agnostic; RoboPlan OInK is the world itself). Targets are sampled -by drawing random collision-free joint configurations and mapping them through -the world's forward kinematics, so every target is reachable by construction. -Both backends receive identical targets and the same seed, are timed with -``time.perf_counter``, and every successful solution is independently verified -by pushing it back through the world's FK and comparing against the target with -``compute_pose_error``. +For each robot, targets are sampled by drawing random collision-free joint +configurations and mapping them through the world's forward kinematics, so every +target is reachable by construction. Every backend runs against its own freshly +built world (no shared mutable scene), receives identical targets and an +equivalent seed, is timed with ``time.perf_counter``, and every successful +solution is independently verified by pushing it back through the world's FK +and comparing against the target with ``compute_pose_error``. Usage: - uv run python dimos/manipulation/benchmark_ik_backends.py --samples 200 - uv run python dimos/manipulation/benchmark_ik_backends.py --robots xarm7 --output /tmp/ik.json + uv run python benchmarks/ik_backends.py + uv run python benchmarks/ik_backends.py --robot xarm7 --solver pink --max-attempts 3 + uv run python benchmarks/ik_backends.py --samples 200 --output /tmp/ik.json + +Extend by adding entries to ``ROBOT_CONFIG_FACTORIES`` / ``_solver_registry``. """ from __future__ import annotations -import argparse -from collections.abc import Sequence +from collections.abc import Callable, Sequence from dataclasses import asdict, dataclass import json from pathlib import Path import resource import statistics import time -from typing import Any import numpy as np +import typer from dimos.manipulation.planning.factory import create_kinematics, create_world from dimos.manipulation.planning.kinematics.config import ( PinkKinematicsConfig, RoboPlanKinematicsConfig, ) +from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID from dimos.manipulation.planning.spec.protocols import KinematicsSpec, WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error @@ -60,7 +62,7 @@ ) from dimos.utils.transform_utils import pose_to_matrix -ROBOT_CONFIG_FACTORIES = { +ROBOT_CONFIG_FACTORIES: dict[str, Callable[[], RobotModelConfig]] = { "xarm6": make_xarm6_model_config, "xarm7": make_xarm7_model_config, } @@ -69,6 +71,32 @@ MAX_TARGET_TRIES_FACTOR = 20 +@dataclass(frozen=True) +class SolverSpec: + """Constructs one IK backend instance for a freshly built world.""" + + name: str + create: Callable[[WorldSpec], KinematicsSpec] + + +def _solver_registry(pink_max_iterations: int) -> dict[str, SolverSpec]: + """Available IK backends. Add new solvers here to include them in the benchmark.""" + + def create_pink(world: WorldSpec) -> KinematicsSpec: + del world # Pink is world-agnostic; it builds its own Pinocchio model. + return create_kinematics(config=PinkKinematicsConfig(max_iterations=pink_max_iterations)) + + def create_roboplan_oink(world: WorldSpec) -> KinematicsSpec: + return create_kinematics( + config=RoboPlanKinematicsConfig(), world=world, world_backend="roboplan" + ) + + return { + "pink": SolverSpec("pink", create_pink), + "roboplan_oink": SolverSpec("roboplan_oink", create_roboplan_oink), + } + + @dataclass class SolveRecord: """One timed IK solve.""" @@ -96,6 +124,31 @@ class BackendRun: peak_rss_delta_mb: float +@dataclass +class DistributionStats: + """Summary statistics for one measured distribution.""" + + mean: float | None + p50: float | None + p95: float | None + max: float | None + + +@dataclass +class RunSummary: + """Aggregate result for one (robot, backend) run.""" + + robot: str + backend: str + samples: int + success_rate: float + status_counts: dict[str, int] + latency_ms: DistributionStats + verified_position_error_m: DistributionStats + verified_orientation_error_rad: DistributionStats + peak_rss_delta_mb: float + + def _sample_reachable_targets( world: WorldSpec, robot_id: WorldRobotID, @@ -180,23 +233,30 @@ def _peak_rss_mb() -> float: return resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024.0 +def _build_world(config: RobotModelConfig) -> tuple[WorldSpec, WorldRobotID, str]: + """Build a fresh finalized RoboPlan world for one robot.""" + world = create_world("roboplan") + robot_id = world.add_robot(config) + world.finalize() + return world, robot_id, f"{config.name}/manipulator" + + def _run_backend( robot_name: str, - backend_name: str, - solver: KinematicsSpec, - world: WorldSpec, - robot_id: WorldRobotID, - group_id: str, + solver: SolverSpec, targets: Sequence[PoseStamped], - seed: JointState, warmup: int, max_attempts: int, ) -> BackendRun: + """Run one backend against its own freshly built world.""" + world, robot_id, group_id = _build_world(ROBOT_CONFIG_FACTORIES[robot_name]()) + kinematics = solver.create(world) reference = world.get_joint_state(world.get_live_context(), robot_id) + seed = reference def solve_once(target: PoseStamped) -> tuple[IKResult, float]: start = time.perf_counter() - result = solver.solve( + result = kinematics.solve( world, robot_id, target, @@ -217,7 +277,7 @@ def solve_once(target: PoseStamped) -> tuple[IKResult, float]: records.append( SolveRecord( robot=robot_name, - backend=backend_name, + backend=solver.name, target_index=index, status=result.status.name, wall_time_ms=wall_time_ms, @@ -231,53 +291,51 @@ def solve_once(target: PoseStamped) -> tuple[IKResult, float]: ) return BackendRun( robot=robot_name, - backend=backend_name, + backend=solver.name, records=records, peak_rss_delta_mb=_peak_rss_mb() - rss_before, ) -def _percentile(values: Sequence[float], pct: float) -> float: - return float(np.percentile(np.asarray(values), pct)) +def _distribution_stats(values: Sequence[float]) -> DistributionStats: + if not values: + return DistributionStats(mean=None, p50=None, p95=None, max=None) + arr = np.asarray(values) + return DistributionStats( + mean=statistics.fmean(values), + p50=float(np.percentile(arr, 50)), + p95=float(np.percentile(arr, 95)), + max=float(arr.max()), + ) -def _summarize(run: BackendRun) -> dict[str, Any]: +def _summarize(run: BackendRun) -> RunSummary: successes = [r for r in run.records if r.status == "SUCCESS"] - latencies = [r.wall_time_ms for r in run.records] - verified_pos = [ - r.verified_position_error for r in successes if r.verified_position_error is not None - ] - verified_ori = [ - r.verified_orientation_error for r in successes if r.verified_orientation_error is not None - ] status_counts: dict[str, int] = {} for r in run.records: status_counts[r.status] = status_counts.get(r.status, 0) + 1 - return { - "robot": run.robot, - "backend": run.backend, - "samples": len(run.records), - "success_rate": len(successes) / len(run.records) if run.records else 0.0, - "status_counts": status_counts, - "latency_ms": { - "mean": statistics.fmean(latencies) if latencies else 0.0, - "p50": _percentile(latencies, 50) if latencies else 0.0, - "p95": _percentile(latencies, 95) if latencies else 0.0, - "max": max(latencies) if latencies else 0.0, - }, - "verified_position_error_m": { - "mean": statistics.fmean(verified_pos) if verified_pos else None, - "max": max(verified_pos) if verified_pos else None, - }, - "verified_orientation_error_rad": { - "mean": statistics.fmean(verified_ori) if verified_ori else None, - "max": max(verified_ori) if verified_ori else None, - }, - "peak_rss_delta_mb": run.peak_rss_delta_mb, - } + return RunSummary( + robot=run.robot, + backend=run.backend, + samples=len(run.records), + success_rate=len(successes) / len(run.records) if run.records else 0.0, + status_counts=status_counts, + latency_ms=_distribution_stats([r.wall_time_ms for r in run.records]), + verified_position_error_m=_distribution_stats( + [r.verified_position_error for r in successes if r.verified_position_error is not None] + ), + verified_orientation_error_rad=_distribution_stats( + [ + r.verified_orientation_error + for r in successes + if r.verified_orientation_error is not None + ] + ), + peak_rss_delta_mb=run.peak_rss_delta_mb, + ) -def _print_summary(summaries: Sequence[dict[str, Any]]) -> None: +def _print_summary(summaries: Sequence[RunSummary]) -> None: header = ( f"{'robot':<8} {'backend':<14} {'n':>5} {'success':>8} " f"{'p50 ms':>9} {'p95 ms':>9} {'mean ms':>9} " @@ -286,88 +344,80 @@ def _print_summary(summaries: Sequence[dict[str, Any]]) -> None: print(header) print("-" * len(header)) for s in summaries: - pos_mm = (s["verified_position_error_m"]["mean"] or 0.0) * 1000.0 - ori_mrad = (s["verified_orientation_error_rad"]["mean"] or 0.0) * 1000.0 + pos_mm = (s.verified_position_error_m.mean or 0.0) * 1000.0 + ori_mrad = (s.verified_orientation_error_rad.mean or 0.0) * 1000.0 print( - f"{s['robot']:<8} {s['backend']:<14} {s['samples']:>5} " - f"{s['success_rate']:>7.1%} " - f"{s['latency_ms']['p50']:>9.2f} {s['latency_ms']['p95']:>9.2f} " - f"{s['latency_ms']['mean']:>9.2f} " - f"{pos_mm:>8.3f} {ori_mrad:>9.3f} {s['peak_rss_delta_mb']:>8.1f}" + f"{s.robot:<8} {s.backend:<14} {s.samples:>5} " + f"{s.success_rate:>7.1%} " + f"{s.latency_ms.p50 or 0.0:>9.2f} {s.latency_ms.p95 or 0.0:>9.2f} " + f"{s.latency_ms.mean or 0.0:>9.2f} " + f"{pos_mm:>8.3f} {ori_mrad:>9.3f} {s.peak_rss_delta_mb:>8.1f}" ) -def main() -> None: - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument( - "--robots", - nargs="+", - choices=sorted(ROBOT_CONFIG_FACTORIES), - default=sorted(ROBOT_CONFIG_FACTORIES), - help="Robots to benchmark (default: all).", - ) - parser.add_argument("--samples", type=int, default=200, help="Timed solves per backend.") - parser.add_argument( - "--warmup", type=int, default=5, help="Warmup solves discarded per backend." - ) - parser.add_argument("--max-attempts", type=int, default=10, help="IK attempts per solve.") - parser.add_argument("--seed", type=int, default=0, help="RNG seed for target sampling.") - parser.add_argument("--output", type=Path, default=None, help="Optional JSON output path.") - args = parser.parse_args() +app = typer.Typer(help=__doc__, add_completion=False) + + +@app.command() +def main( + robots: list[str] = typer.Option( + list(ROBOT_CONFIG_FACTORIES), "--robot", "-r", help="Robots to benchmark (repeatable)." + ), + solvers: list[str] = typer.Option( + [], "--solver", "-s", help="IK backends to run (repeatable, default: all)." + ), + samples: int = typer.Option(200, help="Timed solves per backend."), + warmup: int = typer.Option(10, help="Warmup solves discarded per backend."), + max_attempts: int = typer.Option(10, help="IK attempts per solve (all backends)."), + pink_max_iterations: int = typer.Option(200, help="Pink iterations per attempt."), + seed: int = typer.Option(0, help="RNG seed for target sampling."), + output: Path | None = typer.Option(None, help="Optional JSON output path."), +) -> None: + registry = _solver_registry(pink_max_iterations) + for robot in robots: + if robot not in ROBOT_CONFIG_FACTORIES: + raise typer.BadParameter( + f"Unknown robot '{robot}'. Available: {sorted(ROBOT_CONFIG_FACTORIES)}" + ) + selected = solvers or list(registry) + for solver in selected: + if solver not in registry: + raise typer.BadParameter(f"Unknown solver '{solver}'. Available: {sorted(registry)}") runs: list[BackendRun] = [] - for robot_name in args.robots: - print(f"[setup] building RoboPlanWorld for {robot_name} ...", flush=True) - config = ROBOT_CONFIG_FACTORIES[robot_name]() - world = create_world("roboplan") - robot_id = world.add_robot(config) - world.finalize() - group_id = f"{config.name}/manipulator" - - rng = np.random.default_rng(args.seed) + for robot_name in robots: + print( + f"[setup] {robot_name}: sampling {samples + warmup} reachable targets ...", flush=True + ) + sample_world, sample_robot_id, sample_group_id = _build_world( + ROBOT_CONFIG_FACTORIES[robot_name]() + ) + rng = np.random.default_rng(seed) targets = _sample_reachable_targets( - world, robot_id, group_id, args.samples + args.warmup, rng + sample_world, sample_robot_id, sample_group_id, samples + warmup, rng ) - seed = world.get_joint_state(world.get_live_context(), robot_id) - - solvers: dict[str, KinematicsSpec] = { - "pink": create_kinematics(config=PinkKinematicsConfig()), - "roboplan_oink": create_kinematics( - config=RoboPlanKinematicsConfig(), world=world, world_backend="roboplan" - ), - } - for backend_name, solver in solvers.items(): - print(f"[run] {robot_name} / {backend_name} ...", flush=True) + for solver_name in selected: + print(f"[run] {robot_name} / {solver_name} ...", flush=True) runs.append( - _run_backend( - robot_name, - backend_name, - solver, - world, - robot_id, - group_id, - targets, - seed, - args.warmup, - args.max_attempts, - ) + _run_backend(robot_name, registry[solver_name], targets, warmup, max_attempts) ) summaries = [_summarize(run) for run in runs] _print_summary(summaries) - if args.output is not None: + if output is not None: payload = { - "seed": args.seed, - "samples": args.samples, - "warmup": args.warmup, - "max_attempts": args.max_attempts, - "summaries": summaries, + "seed": seed, + "samples": samples, + "warmup": warmup, + "max_attempts": max_attempts, + "pink_max_iterations": pink_max_iterations, + "summaries": [asdict(s) for s in summaries], "records": [asdict(record) for run in runs for record in run.records], } - args.output.write_text(json.dumps(payload, indent=2)) - print(f"wrote {args.output}") + output.write_text(json.dumps(payload, indent=2)) + print(f"wrote {output}") if __name__ == "__main__": - main() + app() diff --git a/dimos/manipulation/benchmark_ik_backends.md b/dimos/manipulation/benchmark_ik_backends.md deleted file mode 100644 index f561ce2b9c..0000000000 --- a/dimos/manipulation/benchmark_ik_backends.md +++ /dev/null @@ -1,98 +0,0 @@ -# IK Backend Benchmark: RoboPlan OInK vs Pink - -Benchmark comparing the two manipulation IK backends on representative DimOS -workloads, for [issue #3232](https://github.com/dimensionalOS/dimos/issues/3232). -Related implementation: [PR #3230](https://github.com/dimensionalOS/dimos/pull/3230) -(RoboPlan-native OInK backend). - -## Methodology - -`dimos/manipulation/benchmark_ik_backends.py`: - -- Robots: xArm6 and xArm7 (`make_xarm6_model_config()` / `make_xarm7_model_config()`, - with gripper, `limited=true`). -- One `RoboPlanWorld` per robot is shared by both solvers. Pink is world-agnostic - (builds its own Pinocchio model); RoboPlan OInK *is* the world - (`RoboPlanKinematicsConfig` returns the world cast to `KinematicsSpec`). Same - world, same collision model, same FK for both. -- Targets: joint configurations sampled uniformly within limits, filtered to - collision-free ones, mapped through world FK. Every target is reachable by - construction, so failures measure solver behavior, not unreachable goals. -- Both backends get identical targets and the same seed joint state (live robot - state), `check_collision=True`, `max_attempts=10`, default tolerances - (1 mm / 0.01 rad). -- Latency: `time.perf_counter` around `KinematicsSpec.solve`. 10 warmup solves - per backend discarded. -- Accuracy: successful solutions are independently re-verified by pushing them - through the world's FK and scoring against the target with - `compute_pose_error` (same metric for both backends). -- Resource usage: coarse peak-RSS delta (`resource.ru_maxrss`) across each - backend's measured window; model construction happens in warmup. -- Hardware: 16-core x86_64 (Ubuntu 20.04), Python 3.12, `pin-pink 4.2.0`, - `roboplan 0.0.100`. 200 timed solves per (robot, backend), seed 0. - -## Results (seed 0, 200 samples) - -| robot | backend | success | p50 ms | p95 ms | mean ms | pos err p50 (mm) | pos err p95 (mm) | -|-------|---------|---------|--------|--------|---------|------------------|-------------------| -| xarm6 | pink | 87.5% | 29.6 | 512.2 | 123.8 | 0.74 | 0.97 | -| xarm6 | roboplan_oink | 84.5% | 31.3 | 99.8 | 38.4 | 0.27 | 0.86 | -| xarm7 | pink | 99.0% | 8.6 | 207.5 | 38.9 | 0.74 | 0.96 | -| xarm7 | roboplan_oink | 97.0% | 5.1 | 66.5 | 16.4 | 0.53 | 0.97 | - -Orientation error means are comparable (0.5–1.2 mrad; tolerance is 10 mrad). -Verified max position error is ~1.0 mm for both backends, i.e. both honor the -position tolerance exactly when they report success. Peak RSS delta was ≤ 2 MB -for both backends — per-solve memory pressure is negligible once models are -loaded. - -Failure breakdown: - -| robot | backend | status counts | -|-------|---------|---------------| -| xarm6 | pink | 19 NO_SOLUTION, 3 COLLISION, 3 JOINT_LIMITS | -| xarm6 | roboplan_oink | 31 NO_SOLUTION | -| xarm7 | pink | 2 NO_SOLUTION | -| xarm7 | roboplan_oink | 3 NO_SOLUTION, 3 COLLISION (converged only to colliding endpoints) | - -## Observed tradeoffs - -- **OInK has a much tighter latency tail.** Mean 2–3.2x faster, p95 3–5x lower. - Pink's slow solves are its failures: on xarm6, failed solves average 343 ms - (burnt on restarts that exhaust the 200-iteration budget) vs 92 ms for - successes. OInK failures cost ~100 ms (10 bounded attempts x 100 iterations). - For planning loops that call IK repeatedly, OInK's worst case is ~5x cheaper. -- **Pink succeeds slightly more often.** +3 pp on xarm6 (87.5 vs 84.5), +2 pp on - xarm7 (99.0 vs 97.0). Pink's QP formulation with per-attempt restarts within - joint limits rescues some targets OInK gives up on. -- **The backends fail on different targets.** Only 6 of 200 xarm6 targets fail - under both; zero overlap on xarm7. They are complementary: a fallback chain - (OInK first, Pink on NO_SOLUTION) would beat either alone on both latency and - success rate. -- **Failure modes differ in kind.** Pink fails three ways (no convergence, QP - failure, collision/joint-limit rejection of a converged solution). OInK only - ever returns NO_SOLUTION — it never reports success for an endpoint that - fails collision, and FK verification confirms every OInK success lands inside - tolerance. -- **Accuracy is a wash.** Both meet the 1 mm tolerance on every reported - success; OInK's median position error is somewhat lower (0.27 vs 0.74 mm on - xarm6). -- **Caveats.** Single-machine run; Pink solver config was the default - (`proxqp`, `dt=0.05`, 200 iterations) and OInK's 100 iterations/attempt is - currently hardcoded; targets are uniform-random reachable poses with no - obstacles beyond self-collision, so results don't cover cluttered-scene - workloads; RSS delta is process-wide and coarse. - -A rerun with `--seed 1` reproduced the same pattern (OInK mean 2–3.5x faster, -p95 ~5x tighter; success rates within ±3 pp, including one robot where OInK -scored higher), so the tradeoffs above are not seed-specific. - -## Reproduce - -```bash -uv sync --extra manipulation --inexact -uv run python dimos/manipulation/benchmark_ik_backends.py \ - --samples 200 --warmup 10 --seed 0 --output /tmp/ik_bench.json -``` - -Requires the `xarm_description` LFS data (`get_data("xarm_description")`). diff --git a/docs/capabilities/manipulation/ik_benchmark.md b/docs/capabilities/manipulation/ik_benchmark.md new file mode 100644 index 0000000000..1b67f73a8a --- /dev/null +++ b/docs/capabilities/manipulation/ik_benchmark.md @@ -0,0 +1,65 @@ +--- +title: "IK Backend Benchmark" +--- + +`benchmarks/ik_backends.py` compares manipulation IK backends (Pink, RoboPlan +OInK, and any future `KinematicsSpec` implementation) on representative DimOS +workloads: latency, convergence reliability, solution accuracy, and coarse +resource usage, across supported robot configurations. + +## How it works + +- **Robots** are drawn from `ROBOT_CONFIG_FACTORIES` (currently xArm6 and + xArm7). Each backend run builds its **own fresh `RoboPlanWorld`** — no shared + mutable scene between backends. +- **Targets** are sampled by drawing joint configurations uniformly within + limits, keeping only collision-free ones, and mapping them through the + world's FK. Every target is reachable by construction, so failures measure + solver behavior rather than unreachable goals. +- **Fairness**: all backends solve identical targets with an equivalent seed + joint state, `check_collision=True`, and the same `max_attempts`. +- **Latency** is `time.perf_counter` around `KinematicsSpec.solve`, after + warmup solves are discarded. +- **Accuracy**: every successful solution is independently re-verified by + pushing it through the world's FK and scoring against the target with + `compute_pose_error` — the same metric for every backend. +- **Resource usage**: coarse peak-RSS delta (`resource.ru_maxrss`) across each + backend's measured window; model construction happens during warmup. + +## Running + +```bash +uv sync --extra manipulation --inexact + +# Full benchmark (both robots, both backends): +uv run python benchmarks/ik_backends.py --samples 200 --warmup 10 --output /tmp/ik.json + +# Single robot / backend, custom attempt budget: +uv run python benchmarks/ik_backends.py --robot xarm7 --solver pink --max-attempts 3 + +# Sweep the attempt budget to trace the success-rate vs latency tradeoff: +for n in 1 2 5 10; do + uv run python benchmarks/ik_backends.py --max-attempts "$n" --output "/tmp/ik_a$n.json" +done +``` + +Key options: `--robot` / `--solver` (repeatable), `--samples`, `--warmup`, +`--max-attempts`, `--pink-max-iterations`, `--seed`, `--output`. + +Requires the `xarm_description` LFS data (fetched automatically by +`get_data("xarm_description")`). + +## Extending + +- **New robot**: add a `RobotModelConfig` factory to `ROBOT_CONFIG_FACTORIES`. +- **New backend**: add a `SolverSpec` to `_solver_registry()` — a name plus a + constructor from a fresh `WorldSpec` to a `KinematicsSpec`. Backends that are + world-agnostic (like Pink) may ignore the world; world-native backends (like + OInK) take it as their solver. + +## Results + +Benchmark numbers are machine- and version-dependent, so they are not checked +into the repo. Reference results and the observed tradeoffs for OInK vs Pink +are discussed on +[issue #3232](https://github.com/dimensionalOS/dimos/issues/3232). diff --git a/docs/docs.json b/docs/docs.json index 9b4246f2be..d408bc2cc3 100644 --- a/docs/docs.json +++ b/docs/docs.json @@ -102,7 +102,8 @@ "capabilities/manipulation/adding_a_custom_arm", "capabilities/manipulation/openarm_integration", "capabilities/manipulation/piper_integration", - "capabilities/manipulation/a750" + "capabilities/manipulation/a750", + "capabilities/manipulation/ik_benchmark" ] }, { From fcaa22e7b8ac2f3809280e8242c50d5c15de395a Mon Sep 17 00:00:00 2001 From: Jerrybery Date: Tue, 4 Aug 2026 16:36:30 +0800 Subject: [PATCH 3/5] feat(benchmarks): add dual_xarm6 multi-target scenario to IK benchmark - Scenario-centric design: one or more robots sharing a world - dual_xarm6 mirrors the dual_xarm6_planner blueprint (two xArm6, 1 m apart) - Multi-robot scenarios solved as joint multi-target solve_pose_targets calls - Scene-wide collision filtering during target sampling - Rename --robot to --scenario; docs updated --- benchmarks/ik_backends.py | 293 +++++++++++------- .../capabilities/manipulation/ik_benchmark.md | 28 +- 2 files changed, 195 insertions(+), 126 deletions(-) diff --git a/benchmarks/ik_backends.py b/benchmarks/ik_backends.py index 9595bbd7df..808ecff1fc 100644 --- a/benchmarks/ik_backends.py +++ b/benchmarks/ik_backends.py @@ -16,25 +16,27 @@ Issue: https://github.com/dimensionalOS/dimos/issues/3232 -For each robot, targets are sampled by drawing random collision-free joint -configurations and mapping them through the world's forward kinematics, so every -target is reachable by construction. Every backend runs against its own freshly -built world (no shared mutable scene), receives identical targets and an -equivalent seed, is timed with ``time.perf_counter``, and every successful -solution is independently verified by pushing it back through the world's FK -and comparing against the target with ``compute_pose_error``. +Scenarios are one or more robots sharing a world (single arm or dual arm). +Targets are sampled by drawing random joint configurations for every robot, +keeping only scene-wide collision-free ones, and mapping them through the +world's forward kinematics, so every target is reachable by construction. +Every backend runs against its own freshly built world (no shared mutable +scene), receives identical targets and an equivalent seed, is timed with +``time.perf_counter``, and every successful solution is independently verified +by pushing it back through the world's FK and comparing against the target +with ``compute_pose_error``. Usage: uv run python benchmarks/ik_backends.py - uv run python benchmarks/ik_backends.py --robot xarm7 --solver pink --max-attempts 3 + uv run python benchmarks/ik_backends.py --scenario dual_xarm6 --solver pink uv run python benchmarks/ik_backends.py --samples 200 --output /tmp/ik.json -Extend by adding entries to ``ROBOT_CONFIG_FACTORIES`` / ``_solver_registry``. +Extend by adding entries to ``SCENARIOS`` / ``_solver_registry``. """ from __future__ import annotations -from collections.abc import Callable, Sequence +from collections.abc import Callable, Mapping, Sequence from dataclasses import asdict, dataclass import json from pathlib import Path @@ -46,6 +48,8 @@ import typer from dimos.manipulation.planning.factory import create_kinematics, create_world +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry from dimos.manipulation.planning.kinematics.config import ( PinkKinematicsConfig, RoboPlanKinematicsConfig, @@ -62,15 +66,37 @@ ) from dimos.utils.transform_utils import pose_to_matrix -ROBOT_CONFIG_FACTORIES: dict[str, Callable[[], RobotModelConfig]] = { - "xarm6": make_xarm6_model_config, - "xarm7": make_xarm7_model_config, -} - # Sampled targets whose FK pose is in collision are rejected; allow slack for rejection sampling. MAX_TARGET_TRIES_FACTOR = 20 +@dataclass(frozen=True) +class ScenarioSpec: + """One benchmark scenario: one or more robots sharing a world.""" + + name: str + make_configs: Callable[[], list[RobotModelConfig]] + + +def _single(make_config: Callable[[], RobotModelConfig]) -> Callable[[], list[RobotModelConfig]]: + return lambda: [make_config()] + + +def _dual_xarm6_configs() -> list[RobotModelConfig]: + # Mirrors the dual_xarm6_planner blueprint: two xArm6 arms 1 m apart. + return [ + make_xarm6_model_config(name="left_arm", y_offset=0.5), + make_xarm6_model_config(name="right_arm", y_offset=-0.5), + ] + + +SCENARIOS: dict[str, ScenarioSpec] = { + "xarm6": ScenarioSpec("xarm6", _single(make_xarm6_model_config)), + "xarm7": ScenarioSpec("xarm7", _single(make_xarm7_model_config)), + "dual_xarm6": ScenarioSpec("dual_xarm6", _dual_xarm6_configs), +} + + @dataclass(frozen=True) class SolverSpec: """Constructs one IK backend instance for a freshly built world.""" @@ -101,7 +127,7 @@ def create_roboplan_oink(world: WorldSpec) -> KinematicsSpec: class SolveRecord: """One timed IK solve.""" - robot: str + scenario: str backend: str target_index: int status: str @@ -116,9 +142,9 @@ class SolveRecord: @dataclass class BackendRun: - """Records plus coarse resource usage for one backend on one robot.""" + """Records plus coarse resource usage for one backend on one scenario.""" - robot: str + scenario: str backend: str records: list[SolveRecord] peak_rss_delta_mb: float @@ -136,9 +162,9 @@ class DistributionStats: @dataclass class RunSummary: - """Aggregate result for one (robot, backend) run.""" + """Aggregate result for one (scenario, backend) run.""" - robot: str + scenario: str backend: str samples: int success_rate: float @@ -149,37 +175,79 @@ class RunSummary: peak_rss_delta_mb: float +@dataclass +class _Scene: + """A finalized world plus its resolved planning groups.""" + + world: WorldSpec + groups: list[PlanningGroup] + robot_ids: dict[str, WorldRobotID] # keyed by robot (config) name + + +def _build_scene(configs: Sequence[RobotModelConfig]) -> _Scene: + """Build a fresh finalized RoboPlan world for the given robot configs.""" + world = create_world("roboplan") + robot_ids = {config.name: world.add_robot(config) for config in configs} + world.finalize() + registry = PlanningGroupRegistry(configs) + groups = [group for config in configs for group in registry.groups_for_robot(config.name)] + return _Scene(world=world, groups=groups, robot_ids=robot_ids) + + +def _combined_seed(scene: _Scene) -> JointState: + """Current live state of every robot, expressed with global joint names.""" + names: list[str] = [] + positions: list[float] = [] + for robot_name, robot_id in scene.robot_ids.items(): + state = scene.world.get_joint_state(scene.world.get_live_context(), robot_id) + names.extend(f"{robot_name}/{name}" for name in state.name) + positions.extend(state.position) + return JointState(name=names, position=positions) + + def _sample_reachable_targets( - world: WorldSpec, - robot_id: WorldRobotID, - group_id: str, + scene: _Scene, count: int, rng: np.random.Generator, -) -> list[PoseStamped]: - """Sample reachable, collision-free targets via FK of random valid configurations.""" - lower, upper = world.get_joint_limits(robot_id) - reference = world.get_joint_state(world.get_live_context(), robot_id) - targets: list[PoseStamped] = [] +) -> list[dict[PlanningGroup, PoseStamped]]: + """Sample reachable, scene-wide collision-free multi-group targets via FK.""" + limits = { + robot_id: scene.world.get_joint_limits(robot_id) for robot_id in scene.robot_ids.values() + } + references = { + robot_id: scene.world.get_joint_state(scene.world.get_live_context(), robot_id) + for robot_id in scene.robot_ids.values() + } + targets: list[dict[PlanningGroup, PoseStamped]] = [] tries = 0 - with world.scratch_context() as ctx: + with scene.world.scratch_context() as ctx: while len(targets) < count and tries < count * MAX_TARGET_TRIES_FACTOR: tries += 1 - q = rng.uniform(lower, upper) - joint_state = JointState( - name=list(reference.name), - position=[float(v) for v in q], - ) - if not world.check_config_collision_free(robot_id, joint_state): + for robot_id, reference in references.items(): + lower, upper = limits[robot_id] + q = rng.uniform(lower, upper) + scene.world.set_joint_state( + ctx, + robot_id, + JointState( + name=list(reference.name), + position=[float(v) for v in q], + ), + ) + # Scene-wide collision check: covers self-, inter-robot, and obstacle collisions. + if not scene.world.is_collision_free(ctx, next(iter(scene.robot_ids.values()))): continue - world.set_joint_state(ctx, robot_id, joint_state) - pose = world.get_group_ee_pose(ctx, group_id) # OInK requires world-frame targets; rebuild with an explicit frame_id. targets.append( - PoseStamped( - position=pose.position, - orientation=pose.orientation, - frame_id="world", - ) + { + group: PoseStamped( + position=pose.position, + orientation=pose.orientation, + frame_id="world", + ) + for group in scene.groups + for pose in [scene.world.get_group_ee_pose(ctx, group.id)] + } ) if len(targets) < count: raise RuntimeError( @@ -188,44 +256,51 @@ def _sample_reachable_targets( return targets -def _local_solution_joint_state( - reference: JointState, - solution: JointState, -) -> JointState: - """Re-express a solution in the robot's local joint-name order. - - Pink returns config (local) joint names, OInK returns global ``robot/joint`` - names; positions are matched by local name so FK verification is backend-agnostic. - Joints absent from the solution keep their reference positions. - """ - by_local_name: dict[str, float] = {} - for name, position in zip(solution.name, solution.position, strict=True): - by_local_name[name.rsplit("/", 1)[-1]] = position - return JointState( - name=list(reference.name), - position=[ - by_local_name.get(name, ref) - for name, ref in zip(reference.name, reference.position, strict=True) - ], - ) - - def _verify_solution( - world: WorldSpec, - robot_id: WorldRobotID, - group_id: str, - reference: JointState, + scene: _Scene, result: IKResult, - target: PoseStamped, + target: Mapping[PlanningGroup, PoseStamped], ) -> tuple[float, float] | None: - """Push a successful solution through the world's FK and score against the target.""" + """Push a successful solution through the world's FK and score against the target. + + Solutions use global ``robot/joint`` names (both backends); errors are the + worst across all pose-targeted groups. + """ if not result.is_success() or result.joint_state is None: return None - solution = _local_solution_joint_state(reference, result.joint_state) - with world.scratch_context() as ctx: - world.set_joint_state(ctx, robot_id, solution) - actual = world.get_group_ee_pose(ctx, group_id) - return compute_pose_error(pose_to_matrix(actual), pose_to_matrix(target)) + positions_by_robot: dict[str, dict[str, float]] = { + robot_name: {} for robot_name in scene.robot_ids + } + for name, position in zip(result.joint_state.name, result.joint_state.position, strict=True): + robot_name, _, local_name = name.rpartition("/") + if robot_name in positions_by_robot: + positions_by_robot[robot_name][local_name] = position + with scene.world.scratch_context() as ctx: + for robot_name, robot_id in scene.robot_ids.items(): + reference = scene.world.get_joint_state(scene.world.get_live_context(), robot_id) + solved = positions_by_robot[robot_name] + scene.world.set_joint_state( + ctx, + robot_id, + JointState( + name=list(reference.name), + position=[ + solved.get(name, ref) + for name, ref in zip(reference.name, reference.position, strict=True) + ], + ), + ) + errors = [ + compute_pose_error( + pose_to_matrix(scene.world.get_group_ee_pose(ctx, group.id)), + pose_to_matrix(target_pose), + ) + for group, target_pose in target.items() + ] + return ( + max(error[0] for error in errors), + max(error[1] for error in errors), + ) def _peak_rss_mb() -> float: @@ -233,32 +308,22 @@ def _peak_rss_mb() -> float: return resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024.0 -def _build_world(config: RobotModelConfig) -> tuple[WorldSpec, WorldRobotID, str]: - """Build a fresh finalized RoboPlan world for one robot.""" - world = create_world("roboplan") - robot_id = world.add_robot(config) - world.finalize() - return world, robot_id, f"{config.name}/manipulator" - - def _run_backend( - robot_name: str, + scenario: ScenarioSpec, solver: SolverSpec, - targets: Sequence[PoseStamped], + targets: Sequence[dict[PlanningGroup, PoseStamped]], warmup: int, max_attempts: int, ) -> BackendRun: """Run one backend against its own freshly built world.""" - world, robot_id, group_id = _build_world(ROBOT_CONFIG_FACTORIES[robot_name]()) - kinematics = solver.create(world) - reference = world.get_joint_state(world.get_live_context(), robot_id) - seed = reference + scene = _build_scene(scenario.make_configs()) + kinematics = solver.create(scene.world) + seed = _combined_seed(scene) - def solve_once(target: PoseStamped) -> tuple[IKResult, float]: + def solve_once(target: Mapping[PlanningGroup, PoseStamped]) -> tuple[IKResult, float]: start = time.perf_counter() - result = kinematics.solve( - world, - robot_id, + result = kinematics.solve_pose_targets( + scene.world, target, seed=seed, check_collision=True, @@ -273,10 +338,10 @@ def solve_once(target: PoseStamped) -> tuple[IKResult, float]: records: list[SolveRecord] = [] for index, target in enumerate(targets[warmup:]): result, wall_time_ms = solve_once(target) - verified = _verify_solution(world, robot_id, group_id, reference, result, target) + verified = _verify_solution(scene, result, target) records.append( SolveRecord( - robot=robot_name, + scenario=scenario.name, backend=solver.name, target_index=index, status=result.status.name, @@ -290,7 +355,7 @@ def solve_once(target: PoseStamped) -> tuple[IKResult, float]: ) ) return BackendRun( - robot=robot_name, + scenario=scenario.name, backend=solver.name, records=records, peak_rss_delta_mb=_peak_rss_mb() - rss_before, @@ -315,7 +380,7 @@ def _summarize(run: BackendRun) -> RunSummary: for r in run.records: status_counts[r.status] = status_counts.get(r.status, 0) + 1 return RunSummary( - robot=run.robot, + scenario=run.scenario, backend=run.backend, samples=len(run.records), success_rate=len(successes) / len(run.records) if run.records else 0.0, @@ -337,7 +402,7 @@ def _summarize(run: BackendRun) -> RunSummary: def _print_summary(summaries: Sequence[RunSummary]) -> None: header = ( - f"{'robot':<8} {'backend':<14} {'n':>5} {'success':>8} " + f"{'scenario':<12} {'backend':<14} {'n':>5} {'success':>8} " f"{'p50 ms':>9} {'p95 ms':>9} {'mean ms':>9} " f"{'pos mm':>8} {'ori mrad':>9} {'rss ΔMB':>8}" ) @@ -347,7 +412,7 @@ def _print_summary(summaries: Sequence[RunSummary]) -> None: pos_mm = (s.verified_position_error_m.mean or 0.0) * 1000.0 ori_mrad = (s.verified_orientation_error_rad.mean or 0.0) * 1000.0 print( - f"{s.robot:<8} {s.backend:<14} {s.samples:>5} " + f"{s.scenario:<12} {s.backend:<14} {s.samples:>5} " f"{s.success_rate:>7.1%} " f"{s.latency_ms.p50 or 0.0:>9.2f} {s.latency_ms.p95 or 0.0:>9.2f} " f"{s.latency_ms.mean or 0.0:>9.2f} " @@ -360,11 +425,11 @@ def _print_summary(summaries: Sequence[RunSummary]) -> None: @app.command() def main( - robots: list[str] = typer.Option( - list(ROBOT_CONFIG_FACTORIES), "--robot", "-r", help="Robots to benchmark (repeatable)." + scenarios: list[str] = typer.Option( + list(SCENARIOS), "--scenario", "-s", help="Scenarios to benchmark (repeatable)." ), solvers: list[str] = typer.Option( - [], "--solver", "-s", help="IK backends to run (repeatable, default: all)." + [], "--solver", help="IK backends to run (repeatable, default: all)." ), samples: int = typer.Option(200, help="Timed solves per backend."), warmup: int = typer.Option(10, help="Warmup solves discarded per backend."), @@ -374,10 +439,10 @@ def main( output: Path | None = typer.Option(None, help="Optional JSON output path."), ) -> None: registry = _solver_registry(pink_max_iterations) - for robot in robots: - if robot not in ROBOT_CONFIG_FACTORIES: + for scenario in scenarios: + if scenario not in SCENARIOS: raise typer.BadParameter( - f"Unknown robot '{robot}'. Available: {sorted(ROBOT_CONFIG_FACTORIES)}" + f"Unknown scenario '{scenario}'. Available: {sorted(SCENARIOS)}" ) selected = solvers or list(registry) for solver in selected: @@ -385,21 +450,19 @@ def main( raise typer.BadParameter(f"Unknown solver '{solver}'. Available: {sorted(registry)}") runs: list[BackendRun] = [] - for robot_name in robots: + for scenario_name in scenarios: + scenario_spec = SCENARIOS[scenario_name] print( - f"[setup] {robot_name}: sampling {samples + warmup} reachable targets ...", flush=True - ) - sample_world, sample_robot_id, sample_group_id = _build_world( - ROBOT_CONFIG_FACTORIES[robot_name]() + f"[setup] {scenario_name}: sampling {samples + warmup} reachable targets ...", + flush=True, ) + sample_scene = _build_scene(scenario_spec.make_configs()) rng = np.random.default_rng(seed) - targets = _sample_reachable_targets( - sample_world, sample_robot_id, sample_group_id, samples + warmup, rng - ) + targets = _sample_reachable_targets(sample_scene, samples + warmup, rng) for solver_name in selected: - print(f"[run] {robot_name} / {solver_name} ...", flush=True) + print(f"[run] {scenario_name} / {solver_name} ...", flush=True) runs.append( - _run_backend(robot_name, registry[solver_name], targets, warmup, max_attempts) + _run_backend(scenario_spec, registry[solver_name], targets, warmup, max_attempts) ) summaries = [_summarize(run) for run in runs] diff --git a/docs/capabilities/manipulation/ik_benchmark.md b/docs/capabilities/manipulation/ik_benchmark.md index 1b67f73a8a..89a09f76ae 100644 --- a/docs/capabilities/manipulation/ik_benchmark.md +++ b/docs/capabilities/manipulation/ik_benchmark.md @@ -9,13 +9,17 @@ resource usage, across supported robot configurations. ## How it works -- **Robots** are drawn from `ROBOT_CONFIG_FACTORIES` (currently xArm6 and - xArm7). Each backend run builds its **own fresh `RoboPlanWorld`** — no shared - mutable scene between backends. +- **Scenarios** are drawn from `SCENARIOS` — one or more robots sharing a + world (currently `xarm6`, `xarm7`, and `dual_xarm6`, a two-arm setup + mirroring the `dual-xarm6-planner-coordinator` blueprint). Multi-robot + scenarios are solved as joint multi-target `solve_pose_targets` calls. Each + backend run builds its **own fresh `RoboPlanWorld`** — no shared mutable + scene between backends. - **Targets** are sampled by drawing joint configurations uniformly within - limits, keeping only collision-free ones, and mapping them through the - world's FK. Every target is reachable by construction, so failures measure - solver behavior rather than unreachable goals. + limits for every robot, keeping only scene-wide collision-free ones, and + mapping them through the world's FK. Every target is reachable by + construction, so failures measure solver behavior rather than unreachable + goals. - **Fairness**: all backends solve identical targets with an equivalent seed joint state, `check_collision=True`, and the same `max_attempts`. - **Latency** is `time.perf_counter` around `KinematicsSpec.solve`, after @@ -31,11 +35,11 @@ resource usage, across supported robot configurations. ```bash uv sync --extra manipulation --inexact -# Full benchmark (both robots, both backends): +# Full benchmark (all scenarios, both backends): uv run python benchmarks/ik_backends.py --samples 200 --warmup 10 --output /tmp/ik.json -# Single robot / backend, custom attempt budget: -uv run python benchmarks/ik_backends.py --robot xarm7 --solver pink --max-attempts 3 +# Single scenario / backend, custom attempt budget: +uv run python benchmarks/ik_backends.py --scenario dual_xarm6 --solver pink --max-attempts 3 # Sweep the attempt budget to trace the success-rate vs latency tradeoff: for n in 1 2 5 10; do @@ -43,7 +47,7 @@ for n in 1 2 5 10; do done ``` -Key options: `--robot` / `--solver` (repeatable), `--samples`, `--warmup`, +Key options: `--scenario` / `--solver` (repeatable), `--samples`, `--warmup`, `--max-attempts`, `--pink-max-iterations`, `--seed`, `--output`. Requires the `xarm_description` LFS data (fetched automatically by @@ -51,7 +55,9 @@ Requires the `xarm_description` LFS data (fetched automatically by ## Extending -- **New robot**: add a `RobotModelConfig` factory to `ROBOT_CONFIG_FACTORIES`. +- **New scenario**: add a `ScenarioSpec` to `SCENARIOS` — a name plus a factory + returning the `RobotModelConfig` list for one world (one entry per arm for + multi-robot setups). - **New backend**: add a `SolverSpec` to `_solver_registry()` — a name plus a constructor from a fresh `WorldSpec` to a `KinematicsSpec`. Backends that are world-agnostic (like Pink) may ignore the world; world-native backends (like From cc4e199cb2648cc701bb3cb967a06372042b567f Mon Sep 17 00:00:00 2001 From: Jerrybery Date: Tue, 4 Aug 2026 16:40:53 +0800 Subject: [PATCH 4/5] fix(benchmarks): measure current RSS instead of process-wide peak ru_maxrss is a process-lifetime high-water mark, so per-backend deltas were zero/understated for every backend after the first one peaked. Read /proc/self/statm for a real before/after delta (greptile P1). --- benchmarks/ik_backends.py | 29 +++++++++++++------ .../capabilities/manipulation/ik_benchmark.md | 2 +- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/benchmarks/ik_backends.py b/benchmarks/ik_backends.py index 808ecff1fc..21d8b782d6 100644 --- a/benchmarks/ik_backends.py +++ b/benchmarks/ik_backends.py @@ -39,6 +39,7 @@ from collections.abc import Callable, Mapping, Sequence from dataclasses import asdict, dataclass import json +import os from pathlib import Path import resource import statistics @@ -147,7 +148,7 @@ class BackendRun: scenario: str backend: str records: list[SolveRecord] - peak_rss_delta_mb: float + rss_delta_mb: float @dataclass @@ -172,7 +173,7 @@ class RunSummary: latency_ms: DistributionStats verified_position_error_m: DistributionStats verified_orientation_error_rad: DistributionStats - peak_rss_delta_mb: float + rss_delta_mb: float @dataclass @@ -303,9 +304,19 @@ def _verify_solution( ) -def _peak_rss_mb() -> float: - """Peak RSS of this process in MB (Linux ru_maxrss is KiB).""" - return resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024.0 +def _current_rss_mb() -> float: + """Current resident set size of this process in MB. + + Reads /proc/self/statm so the per-backend delta is a real before/after + difference; ru_maxrss is a process-lifetime high-water mark and would + report zero growth for every backend after the first one peaks. + """ + try: + with open("/proc/self/statm") as fh: + resident_pages = int(fh.read().split()[1]) + return resident_pages * os.sysconf("SC_PAGESIZE") / 1024.0**2 + except OSError: + return resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024.0 def _run_backend( @@ -334,7 +345,7 @@ def solve_once(target: Mapping[PlanningGroup, PoseStamped]) -> tuple[IKResult, f for target in targets[:warmup]: solve_once(target) - rss_before = _peak_rss_mb() + rss_before = _current_rss_mb() records: list[SolveRecord] = [] for index, target in enumerate(targets[warmup:]): result, wall_time_ms = solve_once(target) @@ -358,7 +369,7 @@ def solve_once(target: Mapping[PlanningGroup, PoseStamped]) -> tuple[IKResult, f scenario=scenario.name, backend=solver.name, records=records, - peak_rss_delta_mb=_peak_rss_mb() - rss_before, + rss_delta_mb=_current_rss_mb() - rss_before, ) @@ -396,7 +407,7 @@ def _summarize(run: BackendRun) -> RunSummary: if r.verified_orientation_error is not None ] ), - peak_rss_delta_mb=run.peak_rss_delta_mb, + rss_delta_mb=run.rss_delta_mb, ) @@ -416,7 +427,7 @@ def _print_summary(summaries: Sequence[RunSummary]) -> None: f"{s.success_rate:>7.1%} " f"{s.latency_ms.p50 or 0.0:>9.2f} {s.latency_ms.p95 or 0.0:>9.2f} " f"{s.latency_ms.mean or 0.0:>9.2f} " - f"{pos_mm:>8.3f} {ori_mrad:>9.3f} {s.peak_rss_delta_mb:>8.1f}" + f"{pos_mm:>8.3f} {ori_mrad:>9.3f} {s.rss_delta_mb:>8.1f}" ) diff --git a/docs/capabilities/manipulation/ik_benchmark.md b/docs/capabilities/manipulation/ik_benchmark.md index 89a09f76ae..769f814b7e 100644 --- a/docs/capabilities/manipulation/ik_benchmark.md +++ b/docs/capabilities/manipulation/ik_benchmark.md @@ -27,7 +27,7 @@ resource usage, across supported robot configurations. - **Accuracy**: every successful solution is independently re-verified by pushing it through the world's FK and scoring against the target with `compute_pose_error` — the same metric for every backend. -- **Resource usage**: coarse peak-RSS delta (`resource.ru_maxrss`) across each +- **Resource usage**: coarse current-RSS delta (`/proc/self/statm`) across each backend's measured window; model construction happens during warmup. ## Running From ec76f414997218d6b739fd076b2f362a35d5b1fd Mon Sep 17 00:00:00 2001 From: Jerrybery Date: Wed, 5 Aug 2026 14:18:17 +0800 Subject: [PATCH 5/5] refactor(benchmarks): move IK benchmark to misc/ik_benchmark/ per review --- docs/capabilities/manipulation/ik_benchmark.md | 8 ++++---- {benchmarks => misc/ik_benchmark}/ik_backends.py | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) rename {benchmarks => misc/ik_benchmark}/ik_backends.py (98%) diff --git a/docs/capabilities/manipulation/ik_benchmark.md b/docs/capabilities/manipulation/ik_benchmark.md index 769f814b7e..f5baa5c404 100644 --- a/docs/capabilities/manipulation/ik_benchmark.md +++ b/docs/capabilities/manipulation/ik_benchmark.md @@ -2,7 +2,7 @@ title: "IK Backend Benchmark" --- -`benchmarks/ik_backends.py` compares manipulation IK backends (Pink, RoboPlan +`misc/ik_benchmark/ik_backends.py` compares manipulation IK backends (Pink, RoboPlan OInK, and any future `KinematicsSpec` implementation) on representative DimOS workloads: latency, convergence reliability, solution accuracy, and coarse resource usage, across supported robot configurations. @@ -36,14 +36,14 @@ resource usage, across supported robot configurations. uv sync --extra manipulation --inexact # Full benchmark (all scenarios, both backends): -uv run python benchmarks/ik_backends.py --samples 200 --warmup 10 --output /tmp/ik.json +uv run python misc/ik_benchmark/ik_backends.py --samples 200 --warmup 10 --output /tmp/ik.json # Single scenario / backend, custom attempt budget: -uv run python benchmarks/ik_backends.py --scenario dual_xarm6 --solver pink --max-attempts 3 +uv run python misc/ik_benchmark/ik_backends.py --scenario dual_xarm6 --solver pink --max-attempts 3 # Sweep the attempt budget to trace the success-rate vs latency tradeoff: for n in 1 2 5 10; do - uv run python benchmarks/ik_backends.py --max-attempts "$n" --output "/tmp/ik_a$n.json" + uv run python misc/ik_benchmark/ik_backends.py --max-attempts "$n" --output "/tmp/ik_a$n.json" done ``` diff --git a/benchmarks/ik_backends.py b/misc/ik_benchmark/ik_backends.py similarity index 98% rename from benchmarks/ik_backends.py rename to misc/ik_benchmark/ik_backends.py index 21d8b782d6..0efc251d41 100644 --- a/benchmarks/ik_backends.py +++ b/misc/ik_benchmark/ik_backends.py @@ -27,9 +27,9 @@ with ``compute_pose_error``. Usage: - uv run python benchmarks/ik_backends.py - uv run python benchmarks/ik_backends.py --scenario dual_xarm6 --solver pink - uv run python benchmarks/ik_backends.py --samples 200 --output /tmp/ik.json + uv run python misc/ik_benchmark/ik_backends.py + uv run python misc/ik_benchmark/ik_backends.py --scenario dual_xarm6 --solver pink + uv run python misc/ik_benchmark/ik_backends.py --samples 200 --output /tmp/ik.json Extend by adding entries to ``SCENARIOS`` / ``_solver_registry``. """