From 61de3fcc1ab7b9fbeb67be3741e3fc62167ed225 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 20 Nov 2026 01:01:13 +0800 Subject: [PATCH 1/3] feat(benchmark): validate grasp poses in parallel physics trials --- agent_context/MAP.yaml | 5 +- .../topics/motion-planning/motion-planning.md | 8 + docs/source/api_reference/public_api.rst | 16 + embodichain/cli/benchmark.py | 5 +- embodichain/lab/sim/grasp_validation.py | 430 ++++++++++++++++++ .../graspkit/pg_grasp/_antipodal_backend.py | 6 +- .../graspkit/pg_grasp/pose_generator.py | 6 + .../grasp_pose_generator/README.md | 51 +++ .../grasp_pose_generator/__init__.py | 19 + .../grasp_pose_generator/_reporting.py | 155 +++++++ .../grasp_pose_generator/block.yaml | 20 + .../grasp_pose_generator/candidates.jsonl | 3 + .../grasp_pose_generator/cli.py | 408 +++++++++++++++++ .../grasp_pose_generator/metrics.py | 199 ++++++++ .../grasp_pose_generator/suite.yaml | 21 + .../grasp_pose_generator/run_benchmark.py | 1 + setup.py | 1 + .../test_physical_metrics.py | 157 +++++++ tests/test_benchmark_package.py | 2 + 19 files changed, 1510 insertions(+), 3 deletions(-) create mode 100644 embodichain/lab/sim/grasp_validation.py create mode 100644 extensions/embodichain_benchmarks/grasp_pose_generator/README.md create mode 100644 extensions/embodichain_benchmarks/grasp_pose_generator/__init__.py create mode 100644 extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.py create mode 100644 extensions/embodichain_benchmarks/grasp_pose_generator/block.yaml create mode 100644 extensions/embodichain_benchmarks/grasp_pose_generator/candidates.jsonl create mode 100644 extensions/embodichain_benchmarks/grasp_pose_generator/cli.py create mode 100644 extensions/embodichain_benchmarks/grasp_pose_generator/metrics.py create mode 100644 extensions/embodichain_benchmarks/grasp_pose_generator/suite.yaml create mode 100644 tests/benchmark/grasp_pose_generator/test_physical_metrics.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 5c0e8c02f..f07506acf 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -235,6 +235,7 @@ topics: NativeQueryScope, NativeClosingQuery, ClosingContactPolicy, ConvexMeshQuery, native hand queries, collision world, compute trajectory, trajectory resampling, trajectory warping, retime_to_control_grid, JointTrajectoryPlaybackCfg, play_joint_trajectory, quaternion, xyzw, wxyz, + GraspValidator, grasp-pose-generator, 抓取动力学校验, 抓取泛化评测, CoordinatedGraspSampler, PairedGraspProvider, sample_urdf_collision_surfaces, 双手抓取采样] paths: [topics/motion-planning/motion-planning.md] source_of_truth: @@ -245,6 +246,8 @@ topics: - embodichain/lab/sim/motion/limits.py - embodichain/lab/sim/motion/search.py - embodichain/lab/sim/motion/refinement.py + - embodichain/lab/sim/grasp_validation.py + - extensions/embodichain_benchmarks/grasp_pose_generator/ - extensions/embodichain_benchmarks/motion_generation/ - embodichain/cli/benchmark.py - embodichain/lab/sim/motion/execution.py @@ -264,7 +267,7 @@ topics: - embodichain/compute/trajectory/ - embodichain/lab/sim/utility/action_utils.py - embodichain/toolkits/graspkit/pg_grasp/ - watch_paths: [tests/benchmark/motion_generation/, tests/test_benchmark_package.py, tests/sim/motion/test_motion_generator.py, + watch_paths: [tests/benchmark/grasp_pose_generator/, tests/benchmark/motion_generation/, tests/test_benchmark_package.py, tests/sim/motion/test_motion_generator.py, tests/sim/motion/test_motion_generator_batched.py, embodichain/lab/sim/motion/planners/, tests/sim/motion/expansion/, tests/sim/motion/planners/, tests/compute/, examples/expansion/trajectory/, examples/expansion/README.md, tests/toolkits/test_coordinated_grasp.py, tests/toolkits/test_urdf_collision_surfaces.py, diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 900db5cad..acb635ee7 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -146,3 +146,11 @@ and [design contract](../../../extensions/embodichain_benchmarks/motion_generati For stale obstacles inspect identity/capabilities; for duration or acceleration changes inspect normalization and the destination clock before tuning limits. Tests target production owners rather than importing tutorial implementations. + +For physical single-object grasp evaluation, the installed +`embodichain_benchmarks.grasp_pose_generator` runner freezes ranked object-local +TCPs and delegates independent base/X/Y trials to +`embodichain.lab.sim.grasp_validation.GraspValidator`. The +[physical suite](../../../extensions/embodichain_benchmarks/grasp_pose_generator/README.md) +owns held-out splits, fixed attempt budgets and reports. The legacy CoffeeCup +sweep measures geometric sampling and selection. diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 6a635cf38..3fcd08a20 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -7173,3 +7173,19 @@ and scalar metadata, with explicit batch and row identity. .. autoclass:: ResolvedContactPhase :members: + +Physical grasp validation +========================= + +.. currentmodule:: embodichain.lab.sim.grasp_validation + +.. autoclass:: GraspValidator + :members: validate, close + +.. currentmodule:: embodichain_benchmarks.grasp_pose_generator.metrics + +.. autofunction:: summarize + +.. currentmodule:: embodichain_benchmarks.grasp_pose_generator.cli + +.. autofunction:: main diff --git a/embodichain/cli/benchmark.py b/embodichain/cli/benchmark.py index eaf88986e..0fae69d84 100644 --- a/embodichain/cli/benchmark.py +++ b/embodichain/cli/benchmark.py @@ -194,7 +194,10 @@ def _run_atomic_skill_profile_cli(_: argparse.Namespace) -> None: def _run_grasp_pose_generator_cli(_: argparse.Namespace) -> None: """Run grasp pose generator benchmark CLI entrypoint.""" - from scripts.benchmark.grasp_pose_generator.run_benchmark import main + if sys.argv[1:2] in (["validate"], ["run"]): + from embodichain_benchmarks.grasp_pose_generator.cli import main + else: + from scripts.benchmark.grasp_pose_generator.run_benchmark import main main() diff --git a/embodichain/lab/sim/grasp_validation.py b/embodichain/lab/sim/grasp_validation.py new file mode 100644 index 000000000..ec3815768 --- /dev/null +++ b/embodichain/lab/sim/grasp_validation.py @@ -0,0 +1,430 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Independent parallel-jaw grasp trials on batched Newton arenas.""" + +from __future__ import annotations + +from dataclasses import replace +from pathlib import Path +from tempfile import TemporaryDirectory +import xml.etree.ElementTree as ET + +import numpy as np +import torch + +from embodichain.robot_registry import resolve_robot_cfg +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import NewtonPhysicsCfg, RenderCfg, RigidObjectCfg +from embodichain.lab.sim.sensors import ContactSensorCfg, ArticulationContactFilterCfg +from embodichain.lab.sim.shapes import CubeCfg + +__all__ = ["GraspValidator"] + +DT = 0.01 +_APPROACH_END = 60 +_CLOSE_END = 100 +_LIFT_END = 180 +_HOLD_STEPS = 300 +_LEG_STEPS = 30 +_HOLD_END = _LIFT_END + _HOLD_STEPS + +# Device trace layout, one sample per completed physics step. +_COM = slice(0, 3) +_ROTATION = slice(3, 12) +_CLEARANCE = 12 +_PADS = slice(13, 15) +_SUPPORT = 15 +_ACCEL = 16 + + +def _stage(hand, directory): + """Add three translational drive axes upstream of the native hand.""" + source = Path(hand.urdf_cfg.assemble_urdf()) + xml = ET.parse(source) + root = xml.getroot() + children = {joint.find("child").get("link") for joint in root.findall("joint")} + (mount,) = [ + link.get("name") + for link in root.findall("link") + if link.get("name") not in children + ] + (Path(directory) / "hand_assets").symlink_to( + source.parent, target_is_directory=True + ) + for mesh in root.iter("mesh"): + mesh.set("filename", str(Path("hand_assets") / mesh.get("filename"))) + parent = "stage_base" + ET.SubElement(root, "link", name=parent) + for i, axis in enumerate(("1 0 0", "0 1 0", "0 0 1")): + child = mount if i == 2 else f"stage_link{i}" + if child != mount: + link = ET.SubElement(root, "link", name=child) + else: + link = root.find(f"link[@name='{mount}']") + if link.find("inertial") is None: + inertia = ET.SubElement(link, "inertial") + ET.SubElement(inertia, "mass", value="0.1") + ET.SubElement( + inertia, + "inertia", + ixx=".001", + iyy=".001", + izz=".001", + ixy="0", + ixz="0", + iyz="0", + ) + joint = ET.SubElement(root, "joint", name=f"stage_{i}", type="prismatic") + for tag, attrs in ( + ("parent", {"link": parent}), + ("child", {"link": child}), + ("axis", {"xyz": axis}), + ("limit", {"lower": "-1", "upper": "1", "velocity": "2", "effort": "2000"}), + ): + ET.SubElement(joint, tag, attrs) + parent = child + path = Path(directory) / "stage.urdf" + xml.write(path) + hand.urdf_cfg = None + hand.fpath = str(path) + hand.init_qpos = None + for name, value in ( + ("stiffness", 30000.0), + ("damping", 350.0), + ("max_effort", 2000.0), + ): + old = getattr(hand.joint_drive_props, name) + setattr( + hand.joint_drive_props, + name, + {**{j: old for j in hand.joint_commands["open"]}, "stage_.*": value}, + ) + + +def _smooth(x): + x = min(max(x, 0.0), 1.0) + return x**3 * (10 + x * (6 * x - 15)) + + +class GraspValidator: + """Own a prepared hand, support and dynamic target for independent trials.""" + + def __init__( + self, object_cfg: RigidObjectCfg, gripper: str, num_envs: int = 1 + ) -> None: + if object_cfg.env_variants: + raise ValueError("Each validator batch requires a single target geometry") + self.temp = TemporaryDirectory() + self.sim = SimulationManager( + SimulationManagerCfg( + num_envs=num_envs, + headless=True, + enable_ground_plane=False, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + physics_dt=DT, + num_substeps=4, + solver_cfg={"solver_type": "mujoco_warp"}, + ), + ) + ) + try: + cfg = resolve_robot_cfg(gripper) + _stage(cfg, self.temp.name) + cfg.init_pos = [0, 0, 1.5] + self.hand = self.sim.add_robot(cfg) + self.sim.add_rigid_object( + RigidObjectCfg( + uid="support", + shape=CubeCfg(size=(4.0, 4.0, 0.1)), + init_pos=(0.0, 0.0, -0.05), + body_type="static", + ) + ) + object_cfg = object_cfg.copy() + object_cfg.uid = "target" + self.obj = self.sim.add_rigid_object(object_cfg) + self.sensor = self.sim.add_sensor( + ContactSensorCfg( + uid="contacts", + rigid_uid_list=["support", "target"], + articulation_cfg_list=[ + ArticulationContactFilterCfg( + articulation_uid=cfg.uid, link_name_list=list(cfg.pad_links) + ) + ], + max_contacts_per_env=256, + ) + ) + self.sim.prepare() + if self.obj.is_non_dynamic: + raise ValueError("Grasp target must be dynamic") + self.sim.update(step=150) + if self.obj.body_data.vel.abs().max().item() > 0.02: + raise ValueError("Target did not settle") + self.initial = self.obj.get_local_pose(to_matrix=True)[0].clone() + self.com = self.obj.body_data.com_pose[0, :3].clone() + self.cfg = cfg + self.root_tcp = self._tensor(cfg.default_root_from_tcp) + self.root_contact = self._tensor(cfg.root_from_contact_frame) + self.pad_ids = self.sensor.get_actor_ids(cfg.uid, list(cfg.pad_links)) + self.target_id = self.sensor.get_actor_ids("target") + self.support_id = self.sensor.get_actor_ids("support") + self.opened = self._tensor( + [cfg.joint_commands["open"].get(j, 0.0) for j in self.hand.joint_names] + ) + self.closed = self._tensor( + [cfg.joint_commands["grasp"].get(j, 0.0) for j in self.hand.joint_names] + ) + self.axes = [self.hand.joint_names.index(f"stage_{i}") for i in range(3)] + if not self.sensor.contact_capabilities.impulse: + raise ValueError("Grasp validation requires measured contact impulses") + fingers, support, lost = self._contacts() + if not bool((support & ~lost & ~fingers.any(1)).all()): + raise ValueError( + "Each target must settle on the support without hand contact" + ) + self.shapes = [ + replace( + shape, + local_pose=self._tensor(shape.local_pose), + half_extents=( + None + if shape.half_extents is None + else self._tensor(shape.half_extents) + ), + vertices=( + None if shape.vertices is None else self._tensor(shape.vertices) + ), + ) + for shape in self.obj.get_collision_shapes(env_id=0) + ] + if not self.shapes: + raise ValueError("Target has no collision geometry") + except BaseException: + self.close() + raise + + def _tensor(self, value: object) -> torch.Tensor: + return torch.as_tensor(value, device=self.sim.device, dtype=torch.float32) + + def close(self) -> None: + """Release physics resources and the temporary mounting URDF.""" + self.sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + self.temp.cleanup() + + def _clearance(self, pose): + heights = [] + for shape in self.shapes: + transform = pose @ shape.local_pose + z = transform[:, 2, :3] + kind = shape.shape_type.name.upper() + if kind == "BOX": + depth = (z.abs() * shape.half_extents).sum(-1) + elif kind == "SPHERE": + depth = shape.radius + elif kind == "CAPSULE": + depth = shape.radius + shape.half_height * z[:, 2].abs() + elif shape.vertices is not None and len(shape.vertices): + depth = -(z @ shape.vertices.T).amin(-1) + else: + raise ValueError(f"Unsupported physical clearance geometry {kind}") + heights.append(transform[:, 2, 3] - depth) + return torch.stack(heights).amin(0) + + def _contacts(self): + self.sensor.update() + data = self.sensor.get_data() + a, b = data["user_ids"].unbind(-1) + paired = torch.where( + a == self.target_id, b, torch.where(b == self.target_id, a, -1) + ) + valid = data["is_valid"] & data["impulse_valid"] & (data["impulse"] > 0) + fingers = ( + (paired[:, :, None] == self.pad_ids[:, None, :]) & valid[:, :, None] + ).any(1) + supported = ((paired == self.support_id) & valid).any(1) + lost = self.sensor.get_latest_contact_loss_mask() | ( + data["is_valid"] & ~data["impulse_valid"] + ).any(1) + return fingers, supported, lost + + def validate(self, poses: np.ndarray, directions: list[str]) -> list[dict]: + """Run one full arena batch of object-local TCP poses and named conditions.""" + poses = np.asarray(poses) + n = self.sim.num_envs + if ( + poses.shape != (n, 4, 4) + or len(directions) != n + or not set(directions) <= {"base", "x", "y"} + ): + raise ValueError( + "One object_from_tcp pose and valid condition per arena required" + ) + rotations = poses[:, :3, :3] + if not ( + np.isfinite(poses).all() + and np.allclose(poses[:, 3], [0, 0, 0, 1]) + and np.allclose( + rotations.transpose(0, 2, 1) @ rotations, np.eye(3), atol=1e-4 + ) + and np.allclose(np.linalg.det(rotations), 1, atol=1e-4) + ): + raise ValueError("Candidates must be proper rigid transforms") + goal = self.initial @ self._tensor(poses) @ torch.linalg.inv(self.root_tcp) + axis = (goal @ self.root_contact)[:, :3, 2] + base = goal.clone() + base[:, :3, 3] -= 0.08 * axis + self.hand.set_local_pose(base) + opened = self.opened.expand(n, -1).contiguous() + self.hand.set_qpos(opened, target=False) + self.hand.set_qvel(torch.zeros_like(opened), target=False) + self.hand.set_qpos(opened) + self.obj.set_local_pose(self.initial.expand(n, -1, -1).contiguous()) + zero = torch.zeros_like(axis) + self.obj.set_velocity(zero, zero) + self.sensor.reset() + shift = self._tensor( + [ + {"base": [0, 0, 0], "x": [0.03, 0, 0], "y": [0, 0.03, 0]}[d] + for d in directions + ] + ) + up = self._tensor([0, 0, 0.1]) + history = [] + unavailable = torch.zeros(n, device=axis.device, dtype=torch.bool) + end = _HOLD_END + ( + 2 * _LEG_STEPS if any(d != "base" for d in directions) else 0 + ) + previous = velocity = None + for step in range(1, end + 1): + position = 0.08 * axis * _smooth(step / _APPROACH_END) + up * _smooth( + (step - _CLOSE_END) / (_LIFT_END - _CLOSE_END) + ) + position += shift * ( + _smooth((step - _HOLD_END) / _LEG_STEPS) + - _smooth((step - _HOLD_END - _LEG_STEPS) / _LEG_STEPS) + ) + q = ( + opened + + (self.closed - self.opened) + * _smooth((step - _APPROACH_END) / (_CLOSE_END - _APPROACH_END)) + ).clone() + q[:, self.axes] = (base[:, :3, :3].mT @ position.unsqueeze(-1)).squeeze(-1) + self.hand.set_qpos(q) + self.sim.update(step=1) + if step < _LIFT_END - 1: + continue + tcp = ( + self.hand.get_link_pose(self.cfg.root_link, to_matrix=True) + @ self.root_tcp + ) + pose = self.obj.get_local_pose(to_matrix=True) + rel = tcp[:, :3, :3].mT @ pose[:, :3, :3] + center = (pose[:, :3, :3] @ self.com) + pose[:, :3, 3] + p = (tcp[:, :3, :3].mT @ (center - tcp[:, :3, 3]).unsqueeze(-1)).squeeze(-1) + v = zero if previous is None else (tcp[:, :3, 3] - previous) / DT + acc = zero if velocity is None else (v - velocity) / DT + previous, velocity = tcp[:, :3, 3].clone(), v + if step < _LIFT_END: + continue + fingers, support, lost = self._contacts() + unavailable |= ( + lost if step <= _HOLD_END else lost & (shift.abs().sum(-1) > 0) + ) + history.append( + torch.cat( + [ + p, + rel.flatten(1), + self._clearance(pose)[:, None], + fingers, + support[:, None], + acc, + ], + dim=1, + ) + ) + samples = torch.stack(history).transpose(0, 1).cpu().numpy() + return [ + _result(row, d, bool(bad)) + for row, d, bad in zip(samples, directions, unavailable.cpu().tolist()) + ] + + +def _window(row): + p, rot = row[:, _COM], row[:, _ROTATION].reshape(-1, 3, 3) + cosine = ((rot * rot[0]).sum(axis=(1, 2)) - 1) / 2 + pads = row[:, _PADS].astype(bool) + held = pads.any(1) & (row[:, _SUPPORT] == 0) + entered = bool(held[0] and row[0, _CLEARANCE] >= 0.02) + retained = (row[:, _CLEARANCE] >= 0.02) & held + failed = np.flatnonzero(~held) + end = int(failed[0]) + 1 if len(failed) else len(row) + p, cosine = p[:end], cosine[:end] + return dict( + success=bool(retained.all() and pads[-1].all()), + entered=entered, + drop_s=float(failed[0] * DT) if entered and len(failed) else None, + censored=bool(entered and not len(failed)), + duration_s=(len(row) - 1) * DT, + slip_mm=float(np.linalg.norm(p - p[0], axis=1).max() * 1000), + rotation_deg=float(np.rad2deg(np.arccos(np.clip(cosine, -1, 1))).max()), + bilateral_fraction=float(pads.all(1).mean()), + ) + + +def _result(row, direction, bad): + if direction == "base": + row = row[: _HOLD_STEPS + 1] + if bad or not np.isfinite(row).all(): + return {"status": "missing", "reason": "incomplete physics observations"} + hold = _window(row[: _HOLD_STEPS + 1]) + cutoffs = { + str(s): _window(row[: round(s / DT) + 1])["success"] for s in (0.5, 1.0, 3.0) + } + shake = ( + _window(row[_HOLD_STEPS:]) if direction != "base" and hold["success"] else None + ) + acceleration = row[_HOLD_STEPS + 1 :, _ACCEL:] if direction != "base" else None + return dict( + status="ok", + success=hold["success"] and (shake is None or shake["success"]), + failure_stage=( + ("lift" if not hold["entered"] else "hold") + if not hold["success"] + else ("disturbance" if shake and not shake["success"] else None) + ), + hold=hold, + retention=cutoffs, + disturbance=shake, + peak_acceleration_m_s2=( + float(np.linalg.norm(acceleration, axis=1).max()) + if acceleration is not None + else None + ), + peak_axis_acceleration_m_s2=( + float( + np.abs( + row[_HOLD_STEPS + 1 :, _ACCEL + ("x", "y").index(direction)] + ).max() + ) + if direction != "base" + else None + ), + ) diff --git a/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py b/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py index 3f48ba337..d1e843b4b 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py +++ b/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py @@ -94,6 +94,7 @@ def __init__( filter_ground_collision: bool, approach_alignment_angle: float | None = None, check_collision: bool = True, + disk_cache: bool = True, prepared: _AntipodalMeshBackend | None = None, closing_geometry: SampledHandGeometry | None = None, closing_ground_height_m: float | None = None, @@ -155,6 +156,7 @@ def __init__( cfg=collision_cfg, collision_geometry=collision_geometry, ) + self._disk_cache = disk_cache self._sampler_cfg = sampler_cfg self._center_mode = sampler_cfg.center_mode self._max_deviation_angle = max_deviation_angle @@ -171,7 +173,7 @@ def __init__( ) # Load cached antipodal pairs for the whole mesh if available. - if prepared is None: + if prepared is None and disk_cache: cache_path = self._get_cache_dir(self.vertices, self.triangles) if os.path.exists(cache_path): logger.log_info( @@ -366,6 +368,8 @@ def _generate_hit_point_pairs( def _cache_hit_point_pairs(self, hit_point_pairs: torch.Tensor): self._hit_point_pairs = hit_point_pairs + if not self._disk_cache: + return cache_path = self._get_cache_dir(self.vertices, self.triangles) self._save_cache(cache_path, hit_point_pairs) diff --git a/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py b/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py index 63bf00243..afce8cce5 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py +++ b/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py @@ -268,7 +268,12 @@ class GraspAnnotationCfg: force_refresh: bool = False """Whether the service recomputes annotations when first seeing a mesh.""" + disk_cache: bool = True + """Read and write persistent annotations. Disable for isolated seeded trials.""" + def __post_init__(self) -> None: + if type(self.disk_cache) is not bool: + raise TypeError("disk_cache must be a bool.") if self.selection_mode not in ("whole_mesh", "interactive"): raise ValueError( "selection_mode must be exactly 'whole_mesh' or 'interactive'." @@ -486,6 +491,7 @@ def _backend( filter_ground_collision=collision.filter_ground_collision, check_collision=not proposals_only, prepared=prepared, + **({} if annotation.disk_cache else {"disk_cache": False}), **( { "closing_geometry": self._closing_geometry, diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/README.md b/extensions/embodichain_benchmarks/grasp_pose_generator/README.md new file mode 100644 index 000000000..6ad987f9f --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/README.md @@ -0,0 +1,51 @@ +# Physical grasp validation + +From the repository root, with the simulation dependencies and CUDA available: + +```bash +d=extensions/embodichain_benchmarks/grasp_pose_generator +embodichain benchmark grasp-pose-generator validate --case "$d/block.yaml" --poses "$d/candidates.jsonl" --num-envs 9 --output outputs/benchmarks/grasp-validation +embodichain benchmark grasp-pose-generator run --suite "$d/suite.yaml" --num-envs 30 --output outputs/benchmarks/grasp-suite --resume +``` + +Use the same YAML/JSONL inputs with the installed CLI. Each JSON line contains a +4×4 rigid transform `T_object_tcp` in metres, ordered best first. A case selects +a native parallel-jaw gripper by catalog ID and a target through an ordinary +dynamic RigidObjectCfg. The flat support surface is at arena Z=0. The stand +sets the requested wrist orientation at reset and executes the fixed-orientation +trajectory with three force-limited prismatic drives and native finger drives. + +The suite crosses the object list, grippers, placements and seeds. Object +entries declare a stable ID, CAD family and boolean development holdout. +Optional named `placements` override top-level rigid config fields. +`algorithms` maps method names to ranked JSONL filename templates, supporting +`{object}`, `{gripper}`, `{placement}` and `{seed}`. A null value runs the +built-in antipodal generator on the object's semantic mesh. Use an empty file +to record an algorithm's abstention. Missing files are input errors. + +Each scene checkpoint stores the frozen candidates and measured starting pose. +Each condition starts from the same settled target pose with cleared velocities. +The base trial approaches 8 cm, closes, lifts 10 cm and holds for 3 seconds. +The independent X and Y trials also travel 3 cm horizontally and back in two +0.30 s minimum-jerk legs. The commanded acceleration peak is 1.92 m/s². +Labels use collision clearance, support contact, continuous retention and +terminal bilateral pad contact. Relative COM motion and orientation are +diagnostics. A window records whether the target entered airborne, and whether its drop time was censored at the observation horizon. +Missing contact evidence is retried from checkpoints. Native worker failure +does not remove its scene from the intended population. + +The trials directory contains typed Parquet parts keyed by frozen case identity. +The JSON report includes the case inventory, each gripper's four development +holdout cells, family-cluster bootstrap intervals and paired method differences. +The Markdown leaderboard averages double-held-out scores across hands. Folds +describe evaluation membership. For a pretrained method, its training inventory +must be verified separately before claiming training-set novelty. Fixed +Top-10 precision charges missing slots as failures. Conditional X/Y retention +uses only trials that qualified to start that disturbance. Run with a fresh +output directory after changing externally referenced asset dependencies. + +For fold-specific model tuning, run each frozen model with `held_grippers` +set to its held-out hand and include all comparison hands in `grippers`. +For a single frozen generator the default reports every hand as a separate +holdout. User-supplied candidate metadata (for example score and opening) is +preserved in the checkpoints. diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/__init__.py b/extensions/embodichain_benchmarks/grasp_pose_generator/__init__.py new file mode 100644 index 000000000..7edc523cb --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Physical single-object grasp evaluation suites.""" + +from __future__ import annotations diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.py b/extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.py new file mode 100644 index 000000000..c2c198d91 --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.py @@ -0,0 +1,155 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Write physical grasp trial artifacts and a compact benchmark report.""" + +from __future__ import annotations + +import os +import tempfile + +import numpy as np + + +def _export_trials(out, cases): + """Export one typed, atomically replaceable Parquet part per frozen case.""" + import polars as pl + + dest = out / "trials" + dest.mkdir(exist_ok=True) + identity = ("algorithm", "object", "family", "gripper", "placement") + schema = { + k: pl.String + for k in ( + *identity, + "trial_id", + "condition", + "status", + "failure_stage", + "reason", + ) + } + schema.update( + rank=pl.Int64, + seed=pl.Int64, + object_holdout=pl.Boolean, + success=pl.Boolean, + peak_acceleration_m_s2=pl.Float64, + peak_axis_acceleration_m_s2=pl.Float64, + ) + for exponent in ("0.5", "1.0", "3.0"): + schema[f"retained_{exponent}s"] = pl.Boolean + for phase in ("hold", "disturbance"): + schema[f"{phase}_entered"] = pl.Boolean + schema[f"{phase}_censored"] = pl.Boolean + for key in ( + "drop_s", + "slip_mm", + "rotation_deg", + "bilateral_fraction", + "duration_s", + ): + schema[f"{phase}_{key}"] = pl.Float64 + for case in cases: + if not case["trials"]: + continue + records = [] + for trial in case["trials"]: + row = {k: case[k] for k in (*identity, "seed", "object_holdout")} + row.update( + { + k: trial.get(k) + for k in ( + "trial_id", + "condition", + "status", + "failure_stage", + "reason", + "rank", + "success", + "peak_acceleration_m_s2", + "peak_axis_acceleration_m_s2", + ) + } + ) + row.update( + { + f"retained_{t}s": value + for t, value in trial.get("retention", {}).items() + } + ) + for phase in ("hold", "disturbance"): + for key, value in (trial.get(phase) or {}).items(): + if f"{phase}_{key}" in schema: + row[f"{phase}_{key}"] = value + records.append(row) + path = dest / (case["trial_id_prefix"] + ".parquet") + with tempfile.NamedTemporaryFile(dir=dest, delete=False) as tmp: + pl.DataFrame(records, schema=schema).write_parquet(tmp) + os.replace(tmp.name, path) + + +def _write_report(out, report): + lines = [ + "# Physical grasp benchmark", + "", + "## Time & Memory", + "", + "| Worker case | cost_time_ms | cpu_delta_mb | gpu_delta_mb | peak_gpu_mb |", + "|---|---:|---:|---:|---:|", + *[ + f"| {case['trial_id_prefix'][:12]} | " + + " | ".join(f"{v:.2f}" for v in case["performance"].values()) + + " |" + for case in report["case_inventory"] + if "performance" in case + ], + "", + f"Run wall time {report['wall_s']:.2f} s. Recorded trials {report['attempts']}. Memory uses worker RSS and the Torch CUDA allocator.", + "", + "## Success & Other Metrics", + "", + "| Held-out hand | Split | Algorithm | Success@1 | Hit@5 | Hit@10 | Precision@10 | Robust@1 |", + "|---|---|---|---:|---:|---:|---:|---:|", + ] + leaderboard = {} + for hand, fold in report["folds"].items(): + for label in ("seen", "new_object", "new_gripper", "both_new"): + if fold[label] is None: + continue + for method, stats in fold[label]["algorithms"].items(): + metrics = stats["metrics"] + values = " | ".join(f"{x:.1%}" for x in metrics.values()) + lines.append(f"| {hand} | {label} | {method} | {values} |") + if label == "both_new": + leaderboard.setdefault(method, []).append( + [metrics["Success@1"], metrics["Robust@1"]] + ) + lines += [ + "", + "## Leaderboard", + "", + "| Algorithm | Double-held-out Success@1 | Robust@1 |", + "|---|---:|---:|", + ] + means = {m: np.mean(v, axis=0) for m, v in leaderboard.items()} + for method, scores in sorted(means.items(), key=lambda item: -item[1][0]): + lines.append(f"| {method} | {scores[0]:.1%} | {scores[1]:.1%} |") + lines += [ + "", + f"Missing cases: {report['missing_cases']}. Intervals and stage diagnostics are in report.json.", + ] + (out / "report.md").write_text("\n".join(lines) + "\n") diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/block.yaml b/extensions/embodichain_benchmarks/grasp_pose_generator/block.yaml new file mode 100644 index 000000000..d10a42559 --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/block.yaml @@ -0,0 +1,20 @@ +gripper: FrankaPandaHand +object: block +rigid: + attrs: + mass_props: + mass: 0.015 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + body_type: dynamic + init_pos: + - 0 + - 0 + - 0.061 + shape: + shape_type: Cube + size: + - 0.065 + - 0.065 + - 0.12 diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/candidates.jsonl b/extensions/embodichain_benchmarks/grasp_pose_generator/candidates.jsonl new file mode 100644 index 000000000..3773e471f --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/candidates.jsonl @@ -0,0 +1,3 @@ +{"T_object_tcp":[[1,0,0,0.15],[0,-1,0,0],[0,0,-1,-0.01],[0,0,0,1]]} +{"T_object_tcp":[[1,0,0,-0.15],[0,-1,0,0],[0,0,-1,-0.01],[0,0,0,1]]} +{"T_object_tcp":[[1,0,0,0],[0,-1,0,0],[0,0,-1,-0.01],[0,0,0,1]]} diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/cli.py b/extensions/embodichain_benchmarks/grasp_pose_generator/cli.py new file mode 100644 index 000000000..553928f4f --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/cli.py @@ -0,0 +1,408 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Execute frozen grasp candidates or a full physical evaluation suite.""" + +from __future__ import annotations + +import argparse +from concurrent.futures import ProcessPoolExecutor +from concurrent.futures.process import BrokenProcessPool +from importlib.metadata import version +import multiprocessing +import time +from copy import copy +import hashlib +import json +import os +import tempfile +from itertools import product +from pathlib import Path + +import numpy as np +import yaml + +from .metrics import summarize +from ._reporting import _export_trials, _write_report + +__all__ = ["main"] + +PROTOCOL = dict( + lift_m=0.10, + clearance_m=0.02, + hold_s=3.0, + amplitude_m=0.03, + leg_s=0.30, + cycles=1, + command_peak_m_s2=10 / np.sqrt(3) * 0.03 / 0.30**2, + physics_dt_s=0.01, + solver_substeps=4, +) + + +def _save(path, value): + with tempfile.NamedTemporaryFile(mode="w", dir=path.parent, delete=False) as f: + json.dump(value, f, allow_nan=False) + os.replace(f.name, path) + + +def _poses(values): + poses = np.asarray(values, dtype=float) + if len(poses) == 0: + return np.empty((0, 4, 4)) + if poses.ndim != 3 or poses.shape[1:] != (4, 4): + raise ValueError("Expected an ordered list of 4x4 matrices") + rot = poses[:, :3, :3] + if not ( + np.isfinite(poses).all() + and np.allclose(poses[:, 3], [0, 0, 0, 1]) + and np.allclose(rot.transpose(0, 2, 1) @ rot, np.eye(3), atol=1e-4) + and np.allclose(np.linalg.det(rot), 1, atol=1e-4) + ): + raise ValueError("Expected finite rigid object_from_tcp matrices") + return poses + + +def _generate(v, gripper, seed, direction): + import torch + from embodichain.robot_registry import resolve_gripper_defaults + from embodichain.lab.sim.utility.gripper_geometry import ( + build_gripper_collision_geometry, + grasp_generator_parameters, + ) + from embodichain.toolkits.graspkit.pg_grasp import ( + AntipodalGraspPoseGenerator, + GraspAnnotationCfg, + ) + + defaults = resolve_gripper_defaults(gripper) + gen = AntipodalGraspPoseGenerator( + **grasp_generator_parameters(defaults), + collision_geometry=build_gripper_collision_geometry(defaults), + annotation_cfg=GraspAnnotationCfg(disk_cache=False), + ) + vertices, faces = v.obj.get_semantic_mesh([0]) + np.random.seed(seed) + torch.manual_seed(seed) + poses, widths, cost = gen.get_grasp_candidates( + mesh_vertices=vertices[0], + mesh_triangles=faces[0], + obj_poses=v.initial[None], + approach_direction=torch.as_tensor( + direction, device=vertices.device, dtype=vertices.dtype + ), + )[0] + order = torch.argsort(cost, stable=True) + order = order[torch.isfinite(cost[order])] + local = (torch.linalg.inv(v.initial) @ poses[order]).cpu().tolist() + return [ + dict(T_object_tcp=pose, cost=c, opening_m=w) + for pose, c, w in zip( + local, cost[order].cpu().tolist(), widths[order].cpu().tolist() + ) + ] + + +def _asset_config(value, root, digests): + if isinstance(value, list): + return [_asset_config(v, root, digests) for v in value] + if not isinstance(value, dict): + return value + result = {} + for key, item in value.items(): + if key in ("fpath", "file_path") and isinstance(item, str): + local = root / item + if local.is_file(): + with local.open("rb") as stream: + digests[str(local.resolve())] = hashlib.file_digest( + stream, "sha256" + ).hexdigest() + item = str(local.resolve()) + result[key] = _asset_config(item, root, digests) + return result + + +def _run_case(spec, method, source, root, out, args): + from embodichain.lab.sim.grasp_validation import GraspValidator + from embodichain.lab.sim.cfg import RigidObjectCfg + from embodichain.robot_registry import resolve_gripper_defaults + + meta = { + key: spec[key] + for key in ( + "object", + "gripper", + "family", + "object_holdout", + "seed", + "placement", + ) + } + case = dict(meta, algorithm=method, case_config=spec, count=0, trials=[]) + digests = {} + rigid = _asset_config(spec["rigid"], root, digests) + protocol = { + "version": 3, + "gripper_defaults": resolve_gripper_defaults(spec["gripper"]), + "dexsim": version("dexsim-engine"), + "object_files": digests, + "newton": version("newton"), + } + case["protocol"] = dict(PROTOCOL) + case["provenance"] = protocol + encoded = json.dumps( + [protocol, spec, method, source, args.command], sort_keys=True, allow_nan=False + ).encode() + name = hashlib.sha256(encoded).hexdigest() + path = out / (name + ".json") + if args.resume and path.exists(): + prior = json.loads(path.read_text()) + if prior.get("finished"): + return prior + import psutil + import torch + + clock = time.perf_counter() + process = psutil.Process() + initial_rss = process.memory_info().rss + device = torch.device("cuda:0") + torch.cuda.init() + initial_gpu = torch.cuda.memory_allocated(device) + torch.cuda.reset_peak_memory_stats(device) + cfg = RigidObjectCfg.from_dict(rigid) + v = GraspValidator(cfg, spec["gripper"], args.num_envs) + try: + if args.resume and path.exists(): + case = json.loads(path.read_text()) + poses = _poses(case["poses"]) + else: + candidates = ( + source + if source is not None + else _generate( + v, + spec["gripper"], + spec["seed"], + spec.get("approach_direction", [0, 0, -1]), + ) + ) + if args.command == "run": + candidates = candidates[:10] + poses = _poses([c["T_object_tcp"] for c in candidates]) + case["candidate_metadata"] = [ + {k: v for k, v in c.items() if k != "T_object_tcp"} for c in candidates + ] + case.update( + count=len(poses), + poses=poses.tolist(), + initial=v.initial.cpu().tolist(), + trial_id_prefix=name, + ) + _save(path, case) + if not np.allclose(case["initial"], v.initial.cpu().numpy(), atol=1e-4): + raise ValueError("Settled initial state changed on resume") + v.initial.copy_(v.initial.new_tensor(case["initial"])) + completed = { + (r["rank"], r["condition"]): r + for r in case["trials"] + if r["status"] == "ok" + } + work = [ + item + for item in product(range(1, len(poses) + 1), ("base", "x", "y")) + if item not in completed + ] + for start in range(0, len(work), args.num_envs): + batch = work[start : start + args.num_envs] + padded = batch + [batch[-1]] * (args.num_envs - len(batch)) + results = v.validate( + np.stack([poses[i - 1] for i, _ in padded]), [d for _, d in padded] + ) + for (rank, d), result in zip(batch, results): + completed[rank, d] = dict( + result, rank=rank, condition=d, trial_id=f"{name}/{rank}/{d}" + ) + case["trials"] = list(completed.values()) + _save(path, case) + case["finished"] = all(r["status"] == "ok" for r in case["trials"]) + case["performance"] = { + "cost_time_ms": 1000 * (time.perf_counter() - clock), + "cpu_delta_mb": (process.memory_info().rss - initial_rss) / 2**20, + "gpu_delta_mb": (torch.cuda.memory_allocated(device) - initial_gpu) / 2**20, + "peak_gpu_mb": torch.cuda.max_memory_allocated(device) / 2**20, + } + _save(path, case) + return case + finally: + v.close() + + +def main(argv: list[str] | None = None) -> None: + """Dispatch single-case validation or a crossed object/hand benchmark.""" + parser = argparse.ArgumentParser() + sub = parser.add_subparsers(dest="command", required=True) + validate = sub.add_parser("validate") + validate.add_argument("--case", type=Path, required=True) + validate.add_argument("--poses", type=Path, required=True) + run = sub.add_parser("run") + run.add_argument("--suite", type=Path, required=True) + for p in (run, validate): + p.add_argument("--num-envs", type=int, default=1) + p.add_argument("--output", type=Path, required=True) + p.add_argument("--resume", action="store_true") + args = parser.parse_args(argv) + if args.num_envs < 1: + parser.error("--num-envs must be positive") + args.output.mkdir(parents=True, exist_ok=True) + if args.command == "validate": + spec = yaml.safe_load(args.case.read_text()) + poses = [ + json.loads(line) + for line in args.poses.read_text().splitlines() + if line.strip() + ] + case = _run_case( + dict( + family=spec["object"], object_holdout=False, seed=0, placement="default" + ) + | spec, + "external", + poses, + args.case.parent, + args.output, + args, + ) + print(args.output / (case["trial_id_prefix"] + ".json")) + if not case["finished"]: + raise SystemExit("Incomplete observations; retry using --resume") + return + suite = yaml.safe_load(args.suite.read_text()) + jobs = [] + started = time.perf_counter() + families = {} + for entries in ( + [o["id"] for o in suite["objects"]], + suite["grippers"], + suite.get("seeds", [0]), + list(suite.get("algorithms", {"antipodal": None})), + ): + if not entries or len(set(entries)) != len(entries): + raise ValueError("Suite axes must be nonempty and unique") + for obj in suite["objects"]: + family = obj["family"] + if ( + type(obj["holdout"]) is not bool + or families.setdefault(family, obj["holdout"]) != obj["holdout"] + ): + raise ValueError("Each CAD family must belong to exactly one object split") + for obj, hand, seed, method in product( + suite["objects"], + suite["grippers"], + suite.get("seeds", [0]), + suite.get("algorithms", {"antipodal": None}), + ): + for placement, overrides in obj.get("placements", {"default": {}}).items(): + spec = dict( + object=obj["id"], + family=obj["family"], + object_holdout=obj["holdout"], + gripper=hand, + seed=seed, + placement=placement, + rigid=obj["rigid"] | overrides, + ) + spec["approach_direction"] = obj.get("approach_direction", [0, 0, -1]) + table = suite.get("algorithms", {}).get(method) + source = None + if table is not None: + filename = table.format( + object=obj["id"], gripper=hand, seed=seed, placement=placement + ) + source = [ + json.loads(line) + for line in (args.suite.parent / filename).read_text().splitlines() + if line.strip() + ] + jobs.append((spec, method, source)) + # Native solver failures can terminate the process. The parent owns the + # intended population and retries from atomically committed arena batches. + pool = None + cases = [] + try: + for spec, method, source in jobs: + current = copy(args) + for attempt in range(2): + if pool is None: + pool = ProcessPoolExecutor( + max_workers=1, mp_context=multiprocessing.get_context("spawn") + ) + try: + case = pool.submit( + _run_case, + spec, + method, + source, + args.suite.parent, + args.output, + current, + ).result() + if case["finished"]: + break + except Exception as exc: + if isinstance(exc, BrokenProcessPool): + pool.shutdown(wait=True) + pool = None + case = dict( + spec, algorithm=method, error=str(exc), count=0, trials=[] + ) + current.resume = True + cases.append(case) + finally: + if pool is not None: + pool.shutdown() + + _export_trials(args.output, cases) + held = suite.get("held_grippers", suite["grippers"]) + if not held or not set(held) <= set(suite["grippers"]): + raise ValueError("held_grippers must select evaluated grippers") + report = summarize(cases, held) + report["protocol"] = dict(PROTOCOL) + elapsed = time.perf_counter() - started + report["wall_s"] = elapsed + report["case_inventory"] = [ + { + k: c[k] + for k in ( + "object", + "gripper", + "placement", + "seed", + "algorithm", + "count", + "trial_id_prefix", + "performance", + "error", + ) + if k in c + } + for c in cases + ] + report["attempts"] = sum(len(c["trials"]) for c in cases) + _save(args.output / "report.json", report) + _write_report(args.output, report) + print(args.output / "report.json") + if not report["complete"]: + raise SystemExit("Incomplete observations; retry using --resume") diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/metrics.py b/extensions/embodichain_benchmarks/grasp_pose_generator/metrics.py new file mode 100644 index 000000000..2ffc68b4a --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/metrics.py @@ -0,0 +1,199 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Fixed-budget physical grasp statistics and paired family bootstrap.""" + +from __future__ import annotations + +from collections import defaultdict +from itertools import combinations + +import numpy as np + +__all__ = ["summarize"] + + +def _case_metrics(case): + rows = case["trials"] + if case.get("error") or any(r["status"] != "ok" for r in rows): + return None + outcomes = {(r["rank"], r["condition"]): r["success"] for r in rows} + expected = { + (rank, d) for rank in range(1, case["count"] + 1) for d in ("base", "x", "y") + } + if outcomes.keys() != expected or len(outcomes) != len(rows): + return None + base = [outcomes.get((i, "base"), False) for i in range(1, 11)] + return np.array( + [ + base[0], + any(base[:5]), + any(base), + sum(base) / 10, + all(outcomes.get((1, d), False) for d in ("base", "x", "y")), + ], + dtype=float, + ) + + +def summarize( + cases: list[dict], held_grippers: list[str], *, repeats: int = 2000 +) -> dict: + """Aggregate paired scenes, with equal object/hand weights within each split.""" + metrics = ["Success@1", "Hit@5", "Hit@10", "Precision@10", "Robust@1"] + methods = sorted({c["algorithm"] for c in cases}) + pairs = defaultdict(list) + families = {} + diagnostics = defaultdict(list) + if not cases or repeats < 1: + raise ValueError("Cases and positive bootstrap repetitions are required") + missing = 0 + population = defaultdict(set) + for c in cases: + key = ( + c["object"], + c["gripper"], + c.get("placement", "default"), + c.get("seed", 0), + ) + if key in population[c["algorithm"]]: + raise ValueError(f"Duplicate benchmark case {key}") + population[c["algorithm"]].add(key) + value = _case_metrics(c) + if value is None: + missing += 1 + continue + group = (c["object"], c["gripper"], c["object_holdout"]) + families[group[0]] = c["family"] + pairs[c["algorithm"], group].append(value) + diagnostics[c["algorithm"]].extend(c["trials"]) + intended = set.union(*population.values()) + missing += sum(len(intended - scenes) for scenes in population.values()) + report = { + "complete": not missing, + "missing_cases": missing, + "folds": {}, + "diagnostics": {}, + } + for method, rows in diagnostics.items(): + info = {} + nominal = [r for r in rows if r["condition"] == "base"] + info["base_retention"] = { + str(t): ( + np.mean([r["retention"][str(t)] for r in nominal]).item() + if nominal + else None + ) + for t in (0.5, 1.0, 3.0) + } + for d in ("x", "y"): + trials = [r for r in rows if r["condition"] == d and r["hold"]["success"]] + info[d] = { + "started": len(trials), + "passed": sum(r["success"] for r in trials), + "conditional_retention": ( + sum(r["success"] for r in trials) / len(trials) if trials else None + ), + } + for phase in ("hold", "disturbance"): + windows = [ + r[phase] + for r in rows + if r.get(phase) is not None + and r[phase].get("entered", True) + and (phase != "hold" or r["condition"] == "base") + ] + info[phase] = {"drops": sum(w["drop_s"] is not None for w in windows)} + for metric in ("slip_mm", "rotation_deg"): + info[phase][metric + "_p50_p95"] = ( + np.percentile([w[metric] for w in windows], [50, 95]).tolist() + if windows + else None + ) + report["diagnostics"][method] = info + if missing: + return report + for held in held_grippers: + fold = {} + for obj_new, hand_new, label in ( + (False, False, "seen"), + (True, False, "new_object"), + (False, True, "new_gripper"), + (True, True, "both_new"), + ): + groups = sorted( + {g for _, g in pairs if g[2] == obj_new and (g[1] == held) == hand_new} + ) + if not groups: + fold[label] = None + continue + values = np.array( + [[np.mean(pairs[m, g], axis=0) for g in groups] for m in methods] + ) + family_ids = np.array([families[g[0]] for g in groups]) + family_names = np.unique(family_ids) + rng = np.random.default_rng(0) + scores = values.mean(1) + bootstrap = [] + # Bound temporary storage independently of the number of draws. + indices = np.searchsorted(family_names, family_ids) + for start in range(0, repeats, 32): + counts = rng.multinomial( + len(family_names), + np.full(len(family_names), 1 / len(family_names)), + size=min(32, repeats - start), + ) + weights = counts[:, indices] + bootstrap.append((weights / weights.sum(1, keepdims=True)) @ values) + draws = np.concatenate(bootstrap, axis=1) + stats = {} + for i, m in enumerate(methods): + stats[m] = { + "metrics": dict(zip(metrics, scores[i].tolist())), + "ci95": dict( + zip( + metrics, + np.percentile(draws[i], [2.5, 97.5], axis=0).T.tolist(), + ) + ), + } + comparisons = {} + for i, j in combinations(range(len(methods)), 2): + comparisons[f"{methods[i]} minus {methods[j]}"] = dict( + zip( + metrics, + np.percentile( + draws[i] - draws[j], [2.5, 97.5], axis=0 + ).T.tolist(), + ) + ) + fold[label] = { + "object_gripper_pairs": len(groups), + "families": len(family_names), + "algorithms": stats, + "paired_difference_ci95": comparisons, + } + if fold["seen"] and fold["both_new"]: + fold["drop_percentage_points"] = { + m: 100 + * ( + fold["seen"]["algorithms"][m]["metrics"]["Success@1"] + - fold["both_new"]["algorithms"][m]["metrics"]["Success@1"] + ) + for m in methods + } + report["folds"][held] = fold + return report diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/suite.yaml b/extensions/embodichain_benchmarks/grasp_pose_generator/suite.yaml new file mode 100644 index 000000000..3e6de7096 --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/suite.yaml @@ -0,0 +1,21 @@ +grippers: [FrankaPandaHand, DHPGI14080, RobotiqArg2F85] +seeds: [0] +algorithms: + antipodal: null +objects: + - id: cube + family: calibration_cube + holdout: false + rigid: + body_type: dynamic + attrs: {mass_props: {mass: 0.035}, material_props: {static_friction: 0.99, dynamic_friction: 0.97}} + shape: {shape_type: Cube, size: [0.04, 0.04, 0.04]} + init_pos: [0, 0, 0.025] + - id: brick + family: rectangular_brick + holdout: true + rigid: + body_type: dynamic + attrs: {mass_props: {mass: 0.035}, material_props: {static_friction: 0.99, dynamic_friction: 0.97}} + shape: {shape_type: Cube, size: [0.035, 0.06, 0.045]} + init_pos: [0, 0, 0.026] diff --git a/scripts/benchmark/grasp_pose_generator/run_benchmark.py b/scripts/benchmark/grasp_pose_generator/run_benchmark.py index e9a32de5e..531486fec 100644 --- a/scripts/benchmark/grasp_pose_generator/run_benchmark.py +++ b/scripts/benchmark/grasp_pose_generator/run_benchmark.py @@ -63,6 +63,7 @@ def _parse_args(argv: Sequence[str] | None = None) -> argparse.Namespace: parser = argparse.ArgumentParser( prog="embodichain benchmark grasp-pose-generator", description="Benchmark CoffeeCup grasp pose generation parameter sweeps.", + epilog="For physical grasp evaluation, use grasp-pose-generator validate --help or grasp-pose-generator run --help.", ) parser.add_argument( "--n-samples", diff --git a/setup.py b/setup.py index 203a535f3..e43de9575 100644 --- a/setup.py +++ b/setup.py @@ -176,6 +176,7 @@ def get_package_data() -> dict[str, list[str]]: "_graph_assets/*", ], **{name: list(extension_patterns) for name in _extension_package_dirs()}, + "embodichain_benchmarks.grasp_pose_generator": ["candidates.jsonl"], "embodichain_tasks.configs": [ "tasks/manipulation/cloth_fold/task_program/trajectory.npz", "**/*.usda", diff --git a/tests/benchmark/grasp_pose_generator/test_physical_metrics.py b/tests/benchmark/grasp_pose_generator/test_physical_metrics.py new file mode 100644 index 000000000..9c56f23b3 --- /dev/null +++ b/tests/benchmark/grasp_pose_generator/test_physical_metrics.py @@ -0,0 +1,157 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Grasp metric contracts for fixed attempt budgets and physical retention.""" + +from __future__ import annotations + +import json +from importlib.resources import files +from pathlib import Path +import subprocess +import sys + +import pytest +import numpy as np +from embodichain_benchmarks.grasp_pose_generator.metrics import summarize +from embodichain.lab.sim.grasp_validation import _result + + +def _trace(length=361): + row = np.zeros((length, 19)) + row[:, 3:12] = np.eye(3).reshape(9) + row[:, 12:15] = [0.09, 1, 1] + return row + + +def test_rotating_retained_target_and_directional_drop(): + row = _trace() + row[320:, 3:12] = np.diag([-1, -1, 1]).reshape(9) + kept = _result(row, "x", False) + assert kept["success"] and kept["disturbance"]["rotation_deg"] == 180 + row[335:, 13:15] = 0 + lost = _result(row, "y", False) + assert lost["hold"]["success"] and not lost["success"] + assert np.isclose(lost["disturbance"]["drop_s"], 0.35) + assert _result(row, "x", True)["status"] == "missing" + + +def test_ranking_missing_evidence_and_zero_padding(): + rows = [] + for rank in (1, 2, 3): + for direction in ("base", "x", "y"): + trace = _trace() + if rank != 3: + trace[:, 13:15] = 0 + rows.append( + dict(_result(trace, direction, False), rank=rank, condition=direction) + ) + case = dict( + count=3, + trials=rows, + object="cube", + family="boxes", + object_holdout=True, + gripper="hand", + algorithm="test", + ) + + def scores(): + return summarize([case], ["hand"])["folds"]["hand"]["both_new"]["algorithms"][ + "test" + ]["metrics"] + + assert scores() == { + "Success@1": 0, + "Hit@5": 1, + "Hit@10": 1, + "Precision@10": 0.1, + "Robust@1": 0, + } + rows[0].update(_result(_trace(), "base", False)) + rows[1].update(_result(_trace(), "x", False)) + assert scores()["Success@1"] == 1 and scores()["Robust@1"] == 0 + rows.pop() + assert not summarize([case], ["hand"])["complete"] + + +def test_macro_weighting_and_paired_intervals(): + records = [] + good = [ + dict(_result(_trace(), d, False), rank=1, condition=d) + for d in ("base", "x", "y") + ] + for method in ("first", "second"): + for object_id, placements in (("easy", 3), ("hard", 1)): + for seed in range(placements): + rows = good if object_id == "easy" else [] + records.append( + dict( + object=object_id, + family=object_id, + gripper="hand", + object_holdout=True, + algorithm=method, + seed=seed, + count=int(bool(rows)), + trials=rows, + ) + ) + results = summarize(records, ["hand"])["folds"]["hand"]["both_new"] + assert results["algorithms"]["first"]["metrics"]["Success@1"] == 0.5 + assert results["paired_difference_ci95"]["first minus second"]["Success@1"] == [ + 0.0, + 0.0, + ] + assert not summarize(records[:-1], ["hand"])["complete"] + + +@pytest.mark.gpu +@pytest.mark.subprocess_sim +def test_gpu_cli_isolates_missed_and_held_grasps(tmp_path: Path) -> None: + """Two misses and a centred grasp share a native nine-arena batch.""" + fixtures = files("embodichain_benchmarks.grasp_pose_generator") + subprocess.run( + [ + sys.executable, + "-m", + "embodichain", + "benchmark", + "grasp-pose-generator", + "validate", + "--case", + str(fixtures / "block.yaml"), + "--poses", + str(fixtures / "candidates.jsonl"), + "--num-envs", + "9", + "--output", + str(tmp_path), + ], + check=True, + timeout=120, + ) + (checkpoint,) = tmp_path.glob("*.json") + case = json.loads(checkpoint.read_text()) + assert case["finished"] + trials = {(r["rank"], r["condition"]): r for r in case["trials"]} + assert len(trials) == 9 + for direction in ("base", "x", "y"): + assert not trials[1, direction]["success"] + assert not trials[2, direction]["success"] + assert trials[3, direction]["success"] + for direction in ("x", "y"): + assert 1.5 < trials[3, direction]["peak_acceleration_m_s2"] < 2.5 diff --git a/tests/test_benchmark_package.py b/tests/test_benchmark_package.py index dccc1f2be..2bc6a9e72 100644 --- a/tests/test_benchmark_package.py +++ b/tests/test_benchmark_package.py @@ -88,6 +88,8 @@ def installed_benchmark(tmp_path_factory: pytest.TempPathFactory) -> tuple[Path, for value in metadata.get_all("Requires-Dist", []) ) assert "embodichain_benchmarks/__init__.py" in entries + for resource in ("suite.yaml", "block.yaml", "candidates.jsonl", "cli.py"): + assert f"embodichain_benchmarks/grasp_pose_generator/{resource}" in entries assert "embodichain_benchmarks/motion_generation/api.py" in entries assert "embodichain/cli/benchmark.py" in entries assert "embodichain_benchmarks/assembly/robot_reference.py" in entries From b576a9357f9f9735a9666940a0792cbd6c8d486c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 20 Nov 2026 12:57:18 +0800 Subject: [PATCH 2/3] feat(benchmark): support Default and Newton DexUni grasp trials --- embodichain/lab/sim/grasp_validation.py | 90 +++++++++++++++---- .../grasp_pose_generator/README.md | 12 +++ .../grasp_pose_generator/_reporting.py | 4 +- .../grasp_pose_generator/cli.py | 37 ++++++-- .../test_physical_metrics.py | 48 +++++++++- 5 files changed, 165 insertions(+), 26 deletions(-) diff --git a/embodichain/lab/sim/grasp_validation.py b/embodichain/lab/sim/grasp_validation.py index ec3815768..3511c7fe7 100644 --- a/embodichain/lab/sim/grasp_validation.py +++ b/embodichain/lab/sim/grasp_validation.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Independent parallel-jaw grasp trials on batched Newton arenas.""" +"""Independent parallel-jaw grasp trials on batched physics arenas.""" from __future__ import annotations @@ -28,7 +28,12 @@ from embodichain.robot_registry import resolve_robot_cfg from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.cfg import NewtonPhysicsCfg, RenderCfg, RigidObjectCfg +from embodichain.lab.sim.cfg import ( + DefaultPhysicsCfg, + NewtonPhysicsCfg, + RenderCfg, + RigidObjectCfg, +) from embodichain.lab.sim.sensors import ContactSensorCfg, ArticulationContactFilterCfg from embodichain.lab.sim.shapes import CubeCfg @@ -121,13 +126,38 @@ def _smooth(x): class GraspValidator: - """Own a prepared hand, support and dynamic target for independent trials.""" + """Own a prepared hand, support and dynamic target for independent trials. + + Args: + object_cfg: Dynamic target shared by the replicated arenas. + gripper: Catalog ID of a calibrated parallel-jaw hand. + num_envs: Number of independent trial arenas. + physics: GPU backend, either ``default`` or ``newton``. Newton runs + DexUni with two-way coupling between the hand and target. + """ def __init__( - self, object_cfg: RigidObjectCfg, gripper: str, num_envs: int = 1 + self, + object_cfg: RigidObjectCfg, + gripper: str, + num_envs: int = 1, + *, + physics: str = "default", ) -> None: if object_cfg.env_variants: raise ValueError("Each validator batch requires a single target geometry") + if physics == "default": + physics_cfg = DefaultPhysicsCfg( + device="cuda:0", physics_dt=DT, num_substeps=4 + ) + elif physics == "newton": + physics_cfg = NewtonPhysicsCfg( + physics_dt=DT, + num_substeps=4, + solver_cfg={"solver_type": "dexuni", "coupling": "two_way"}, + ) + else: + raise ValueError("physics must be default or newton") self.temp = TemporaryDirectory() self.sim = SimulationManager( SimulationManagerCfg( @@ -135,11 +165,7 @@ def __init__( headless=True, enable_ground_plane=False, render_cfg=RenderCfg(renderer="no-render"), - physics_cfg=NewtonPhysicsCfg( - physics_dt=DT, - num_substeps=4, - solver_cfg={"solver_type": "mujoco_warp"}, - ), + physics_cfg=physics_cfg, ) ) try: @@ -152,7 +178,8 @@ def __init__( uid="support", shape=CubeCfg(size=(4.0, 4.0, 0.1)), init_pos=(0.0, 0.0, -0.05), - body_type="static", + # Direct GPU exposes an actor ID for a stationary kinematic support. + body_type="kinematic" if physics == "default" else "static", ) ) object_cfg = object_cfg.copy() @@ -173,8 +200,24 @@ def __init__( self.sim.prepare() if self.obj.is_non_dynamic: raise ValueError("Grasp target must be dynamic") - self.sim.update(step=150) - if self.obj.body_data.vel.abs().max().item() > 0.02: + # Measure residual motion over the final 0.5 s. Resting contacts + # on Default GPU can report nonzero instantaneous angular velocity. + self.sim.update(step=100) + reference = self.obj.get_local_pose(to_matrix=True).clone() + settled = torch.ones( + self.sim.num_envs, dtype=torch.bool, device=self.sim.device + ) + for _ in range(5): + self.sim.update(step=10) + current = self.obj.get_local_pose(to_matrix=True) + displacement = torch.linalg.vector_norm( + current[:, :3, 3] - reference[:, :3, 3], dim=1 + ) + cosine = ( + (current[:, :3, :3] * reference[:, :3, :3]).sum((1, 2)) - 1 + ) * 0.5 + settled &= (displacement <= 0.001) & (cosine >= np.cos(0.005)) + if not settled.all().item(): raise ValueError("Target did not settle") self.initial = self.obj.get_local_pose(to_matrix=True)[0].clone() self.com = self.obj.body_data.com_pose[0, :3].clone() @@ -191,7 +234,11 @@ def __init__( [cfg.joint_commands["grasp"].get(j, 0.0) for j in self.hand.joint_names] ) self.axes = [self.hand.joint_names.index(f"stage_{i}") for i in range(3)] - if not self.sensor.contact_capabilities.impulse: + self._geometry_contacts = physics == "newton" + if ( + not self._geometry_contacts + and not self.sensor.contact_capabilities.impulse + ): raise ValueError("Grasp validation requires measured contact impulses") fingers, support, lost = self._contacts() if not bool((support & ~lost & ~fingers.any(1)).all()): @@ -254,14 +301,23 @@ def _contacts(self): paired = torch.where( a == self.target_id, b, torch.where(b == self.target_id, a, -1) ) - valid = data["is_valid"] & data["impulse_valid"] & (data["impulse"] > 0) + impulse = data["impulse"] + measured = data["impulse_valid"] + evidence = measured & torch.isfinite(impulse) + touching = evidence & (impulse > 0) + if self._geometry_contacts: + # DexUni can publish geometric VBD rows alongside measured impulses. + distance = data["distance"] + geometry = ~measured & torch.isfinite(distance) + evidence |= geometry + touching |= geometry & (distance <= 0) + valid = data["is_valid"] & touching fingers = ( (paired[:, :, None] == self.pad_ids[:, None, :]) & valid[:, :, None] ).any(1) supported = ((paired == self.support_id) & valid).any(1) - lost = self.sensor.get_latest_contact_loss_mask() | ( - data["is_valid"] & ~data["impulse_valid"] - ).any(1) + lost = self.sensor.get_latest_contact_loss_mask() + lost = lost | (data["is_valid"] & ~evidence).any(1) return fingers, supported, lost def validate(self, poses: np.ndarray, directions: list[str]) -> list[dict]: diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/README.md b/extensions/embodichain_benchmarks/grasp_pose_generator/README.md index 6ad987f9f..4a18bb955 100644 --- a/extensions/embodichain_benchmarks/grasp_pose_generator/README.md +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/README.md @@ -8,6 +8,16 @@ embodichain benchmark grasp-pose-generator validate --case "$d/block.yaml" --pos embodichain benchmark grasp-pose-generator run --suite "$d/suite.yaml" --num-envs 30 --output outputs/benchmarks/grasp-suite --resume ``` +Both commands accept `--physics default` (the default GPU backend) and +`--physics newton` (Newton with the DexUni solver). Each invocation evaluates +one backend. Use distinct output directories to keep both aggregate reports. +Checkpoint identities include the backend and solver, so a resumed run +reuses results only from the matching physics configuration. Default uses a +stationary kinematic table for GPU contact actor identification. DexUni uses +two-way coupling. Both paths use positive impulses for measured contact rows. +For geometry-only DexUni rows, a finite signed separation at or below zero +qualifies contact. Reports record the contact policy alongside the solver. + Use the same YAML/JSONL inputs with the installed CLI. Each JSON line contains a 4×4 rigid transform `T_object_tcp` in metres, ordered best first. A case selects a native parallel-jaw gripper by catalog ID and a target through an ordinary @@ -23,6 +33,8 @@ Optional named `placements` override top-level rigid config fields. built-in antipodal generator on the object's semantic mesh. Use an empty file to record an algorithm's abstention. Missing files are input errors. +Settling checks the target pose five times in the last 0.5 s before the snapshot, +with tolerances of 1 mm translation and 0.005 rad rotation. Each scene checkpoint stores the frozen candidates and measured starting pose. Each condition starts from the same settled target pose with cleared velocities. The base trial approaches 8 cm, closes, lifts 10 cm and holds for 3 seconds. diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.py b/extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.py index c2c198d91..1b7c61ef6 100644 --- a/extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.py +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.py @@ -30,7 +30,7 @@ def _export_trials(out, cases): dest = out / "trials" dest.mkdir(exist_ok=True) - identity = ("algorithm", "object", "family", "gripper", "placement") + identity = ("physics", "algorithm", "object", "family", "gripper", "placement") schema = { k: pl.String for k in ( @@ -106,6 +106,8 @@ def _write_report(out, report): lines = [ "# Physical grasp benchmark", "", + f"Physics: {report['protocol']['physics']} (solver: {report['protocol']['solver']})", + "", "## Time & Memory", "", "| Worker case | cost_time_ms | cpu_delta_mb | gpu_delta_mb | peak_gpu_mb |", diff --git a/extensions/embodichain_benchmarks/grasp_pose_generator/cli.py b/extensions/embodichain_benchmarks/grasp_pose_generator/cli.py index 553928f4f..0c07200e5 100644 --- a/extensions/embodichain_benchmarks/grasp_pose_generator/cli.py +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/cli.py @@ -53,6 +53,19 @@ ) +def _physics_protocol(physics): + if physics == "newton": + return dict( + physics="newton", + solver="dexuni", + coupling="two_way", + contact_evidence="positive_impulse_or_nonpositive_signed_distance", + ) + return dict( + physics="default", solver="default", contact_evidence="positive_impulse" + ) + + def _save(path, value): with tempfile.NamedTemporaryFile(mode="w", dir=path.parent, delete=False) as f: json.dump(value, f, allow_nan=False) @@ -151,17 +164,25 @@ def _run_case(spec, method, source, root, out, args): "placement", ) } - case = dict(meta, algorithm=method, case_config=spec, count=0, trials=[]) + case = dict( + meta, + physics=args.physics, + algorithm=method, + case_config=spec, + count=0, + trials=[], + ) digests = {} rigid = _asset_config(spec["rigid"], root, digests) protocol = { - "version": 3, + "version": 4, + **_physics_protocol(args.physics), "gripper_defaults": resolve_gripper_defaults(spec["gripper"]), "dexsim": version("dexsim-engine"), "object_files": digests, "newton": version("newton"), } - case["protocol"] = dict(PROTOCOL) + case["protocol"] = dict(PROTOCOL, **_physics_protocol(args.physics)) case["provenance"] = protocol encoded = json.dumps( [protocol, spec, method, source, args.command], sort_keys=True, allow_nan=False @@ -183,7 +204,7 @@ def _run_case(spec, method, source, root, out, args): initial_gpu = torch.cuda.memory_allocated(device) torch.cuda.reset_peak_memory_stats(device) cfg = RigidObjectCfg.from_dict(rigid) - v = GraspValidator(cfg, spec["gripper"], args.num_envs) + v = GraspValidator(cfg, spec["gripper"], args.num_envs, physics=args.physics) try: if args.resume and path.exists(): case = json.loads(path.read_text()) @@ -260,6 +281,12 @@ def main(argv: list[str] | None = None) -> None: run = sub.add_parser("run") run.add_argument("--suite", type=Path, required=True) for p in (run, validate): + p.add_argument( + "--physics", + choices=("default", "newton"), + default="default", + help="Select Default physics or Newton with the DexUni solver.", + ) p.add_argument("--num-envs", type=int, default=1) p.add_argument("--output", type=Path, required=True) p.add_argument("--resume", action="store_true") @@ -379,7 +406,7 @@ def main(argv: list[str] | None = None) -> None: if not held or not set(held) <= set(suite["grippers"]): raise ValueError("held_grippers must select evaluated grippers") report = summarize(cases, held) - report["protocol"] = dict(PROTOCOL) + report["protocol"] = dict(PROTOCOL, **_physics_protocol(args.physics)) elapsed = time.perf_counter() - started report["wall_s"] = elapsed report["case_inventory"] = [ diff --git a/tests/benchmark/grasp_pose_generator/test_physical_metrics.py b/tests/benchmark/grasp_pose_generator/test_physical_metrics.py index 9c56f23b3..e2e87a5f6 100644 --- a/tests/benchmark/grasp_pose_generator/test_physical_metrics.py +++ b/tests/benchmark/grasp_pose_generator/test_physical_metrics.py @@ -23,11 +23,13 @@ from pathlib import Path import subprocess import sys +from types import SimpleNamespace import pytest import numpy as np +import torch from embodichain_benchmarks.grasp_pose_generator.metrics import summarize -from embodichain.lab.sim.grasp_validation import _result +from embodichain.lab.sim.grasp_validation import GraspValidator, _result def _trace(length=361): @@ -119,9 +121,35 @@ def test_macro_weighting_and_paired_intervals(): assert not summarize(records[:-1], ["hand"])["complete"] +def test_dexuni_contact_evidence_uses_distance_only_for_unmeasured_rows(): + validator = GraspValidator.__new__(GraspValidator) + validator._geometry_contacts = True + validator.target_id = torch.tensor([[1]]) + validator.pad_ids = torch.tensor([[2, 3]]) + validator.support_id = torch.tensor([[4]]) + data = dict( + user_ids=torch.tensor([[[1, 2], [3, 1], [1, 4]]]), + is_valid=torch.ones(1, 3, dtype=torch.bool), + impulse_valid=torch.tensor([[True, False, False]]), + impulse=torch.tensor([[0.0, 0.0, 0.0]]), + distance=torch.tensor([[-0.001, 0.0, 0.001]]), + ) + validator.sensor = SimpleNamespace( + update=lambda: None, + get_data=lambda: data, + get_latest_contact_loss_mask=lambda: torch.tensor([False]), + ) + fingers, support, lost = validator._contacts() + assert fingers.tolist() == [[False, True]] + assert not support.item() and not lost.item() + data["distance"][0, 1] = float("nan") + assert validator._contacts()[2].item() + + @pytest.mark.gpu @pytest.mark.subprocess_sim -def test_gpu_cli_isolates_missed_and_held_grasps(tmp_path: Path) -> None: +@pytest.mark.parametrize("physics", ["default", "newton"]) +def test_gpu_cli_isolates_missed_and_held_grasps(tmp_path: Path, physics: str) -> None: """Two misses and a centred grasp share a native nine-arena batch.""" fixtures = files("embodichain_benchmarks.grasp_pose_generator") subprocess.run( @@ -132,6 +160,8 @@ def test_gpu_cli_isolates_missed_and_held_grasps(tmp_path: Path) -> None: "benchmark", "grasp-pose-generator", "validate", + "--physics", + physics, "--case", str(fixtures / "block.yaml"), "--poses", @@ -146,6 +176,10 @@ def test_gpu_cli_isolates_missed_and_held_grasps(tmp_path: Path) -> None: ) (checkpoint,) = tmp_path.glob("*.json") case = json.loads(checkpoint.read_text()) + assert case["protocol"]["physics"] == physics + assert case["protocol"]["solver"] == ( + "dexuni" if physics == "newton" else "default" + ) assert case["finished"] trials = {(r["rank"], r["condition"]): r for r in case["trials"]} assert len(trials) == 9 @@ -154,4 +188,12 @@ def test_gpu_cli_isolates_missed_and_held_grasps(tmp_path: Path) -> None: assert not trials[2, direction]["success"] assert trials[3, direction]["success"] for direction in ("x", "y"): - assert 1.5 < trials[3, direction]["peak_acceleration_m_s2"] < 2.5 + trial = trials[3, direction] + assert np.isfinite(trial["peak_acceleration_m_s2"]) + assert ( + 1.5 + < trial["peak_axis_acceleration_m_s2"] + <= trial["peak_acceleration_m_s2"] + ) + if physics == "default": + assert trial["peak_acceleration_m_s2"] < 2.5 From e27f0f9e0cf97ecfee279174671a4442d8d25982 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 20 Nov 2026 12:58:54 +0800 Subject: [PATCH 3/3] test(benchmark): check retention across independent DexUni trials --- .../grasp_pose_generator/test_physical_metrics.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tests/benchmark/grasp_pose_generator/test_physical_metrics.py b/tests/benchmark/grasp_pose_generator/test_physical_metrics.py index e2e87a5f6..ac13439bd 100644 --- a/tests/benchmark/grasp_pose_generator/test_physical_metrics.py +++ b/tests/benchmark/grasp_pose_generator/test_physical_metrics.py @@ -186,7 +186,12 @@ def test_gpu_cli_isolates_missed_and_held_grasps(tmp_path: Path, physics: str) - for direction in ("base", "x", "y"): assert not trials[1, direction]["success"] assert not trials[2, direction]["success"] - assert trials[3, direction]["success"] + centred = trials[3, direction] + assert centred["hold"]["entered"] + if physics == "default": + assert centred["success"] + # The coupled hand can slip after pickup. Each condition runs from reset. + assert any(trials[3, direction]["success"] for direction in ("base", "x", "y")) for direction in ("x", "y"): trial = trials[3, direction] assert np.isfinite(trial["peak_acceleration_m_s2"])