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..3511c7fe7 --- /dev/null +++ b/embodichain/lab/sim/grasp_validation.py @@ -0,0 +1,486 @@ +# ---------------------------------------------------------------------------- +# 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 physics 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 ( + DefaultPhysicsCfg, + 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. + + 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, + *, + 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( + num_envs=num_envs, + headless=True, + enable_ground_plane=False, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=physics_cfg, + ) + ) + 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), + # Direct GPU exposes an actor ID for a stationary kinematic support. + body_type="kinematic" if physics == "default" else "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") + # 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() + 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)] + 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()): + 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) + ) + 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() + lost = lost | (data["is_valid"] & ~evidence).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..4a18bb955 --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/README.md @@ -0,0 +1,63 @@ +# 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 +``` + +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 +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. + +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. +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..1b7c61ef6 --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/_reporting.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. +# ---------------------------------------------------------------------------- + +"""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 = ("physics", "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", + "", + 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 |", + "|---|---:|---:|---:|---:|", + *[ + 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..0c07200e5 --- /dev/null +++ b/extensions/embodichain_benchmarks/grasp_pose_generator/cli.py @@ -0,0 +1,435 @@ +# ---------------------------------------------------------------------------- +# 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 _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) + 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, + physics=args.physics, + algorithm=method, + case_config=spec, + count=0, + trials=[], + ) + digests = {} + rigid = _asset_config(spec["rigid"], root, digests) + protocol = { + "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, **_physics_protocol(args.physics)) + 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, physics=args.physics) + 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( + "--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") + 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, **_physics_protocol(args.physics)) + 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..ac13439bd --- /dev/null +++ b/tests/benchmark/grasp_pose_generator/test_physical_metrics.py @@ -0,0 +1,204 @@ +# ---------------------------------------------------------------------------- +# 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 +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 GraspValidator, _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"] + + +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 +@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( + [ + sys.executable, + "-m", + "embodichain", + "benchmark", + "grasp-pose-generator", + "validate", + "--physics", + physics, + "--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["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 + for direction in ("base", "x", "y"): + assert not trials[1, direction]["success"] + assert not trials[2, 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"]) + 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 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