From c12ffe36bdbe491ea4639f17db19612b626fd91f Mon Sep 17 00:00:00 2001 From: Lila Anafi Date: Sat, 19 Sep 2026 23:43:07 -0400 Subject: [PATCH] Add physics-derived grasp/release verification to Pick/Place PickObjectCfg/PlaceObjectCfg previously reported success from end-effector pose thresholds and gripper-hold duration alone, never checking whether the object was actually grasped or released. Adds an opt-in VerifyContactCfg primitive that checks a rigid object's physics-derived is_in_contact state (from an IsInContactPhysicsCfg semantic), wired in via new verify_grasp (PickObjectCfg) and verify_release + held_object (PlaceObjectCfg) fields. Both default to False/None, so existing workflows are unaffected. Verified live: booted Isaac Sim against Matterix-Test-Beaker-Lift-Franka-v1 with a beaker contact sensor filtered on the Franka gripper fingers (panda_leftfinger/panda_rightfinger, cross-checked against isaaclab_tasks' own stack manipulation config for the same USD asset), ran a full pick-and-place episode through StateMachine with verify_grasp=True and verify_release=True -- both VerifyContact steps succeeded against real ContactSensor force data. Also ran pre-commit (black/isort/codespell/license header clean) and flake8+pyupgrade pinned to the versions in .pre-commit-config.yaml under Python 3.11 (clean; pyupgrade auto-fixed one redundant quoted forward-reference). Co-Authored-By: Claude Sonnet 5 --- source/matterix_sm/matterix_sm/__init__.py | 4 + .../compositional_actions/pick_object.py | 19 ++ .../compositional_actions/place_object.py | 30 +++ .../matterix_sm/primitive_actions/__init__.py | 4 + .../primitive_actions/verify_contact.py | 175 ++++++++++++++++++ source/matterix_sm/matterix_sm/scene_data.py | 12 ++ .../matterix_sm/matterix_sm/state_machine.py | 1 + 7 files changed, 245 insertions(+) create mode 100644 source/matterix_sm/matterix_sm/primitive_actions/verify_contact.py diff --git a/source/matterix_sm/matterix_sm/__init__.py b/source/matterix_sm/matterix_sm/__init__.py index 058c2a1..2c2db5a 100644 --- a/source/matterix_sm/matterix_sm/__init__.py +++ b/source/matterix_sm/matterix_sm/__init__.py @@ -53,6 +53,8 @@ OpenGripperCfg, SemanticAction, SemanticActionCfg, + VerifyContact, + VerifyContactCfg, Wait, WaitCfg, ) @@ -91,6 +93,8 @@ "CloseGripperCfg", "SemanticAction", "SemanticActionCfg", + "VerifyContact", + "VerifyContactCfg", "Wait", "WaitCfg", # Semantic support diff --git a/source/matterix_sm/matterix_sm/compositional_actions/pick_object.py b/source/matterix_sm/matterix_sm/compositional_actions/pick_object.py index 59f2181..318d7ef 100644 --- a/source/matterix_sm/matterix_sm/compositional_actions/pick_object.py +++ b/source/matterix_sm/matterix_sm/compositional_actions/pick_object.py @@ -16,6 +16,7 @@ MoveRelativeCfg, MoveToFrameCfg, OpenGripperCfg, + VerifyContactCfg, ) from ..robot_action_spaces import ActionSpaceInfo @@ -32,6 +33,12 @@ class PickObjectCfg(CompositionalActionCfg): object: Name of the object to pick. REQUIRED. post_grasp_offset: Offset for post-grasp lift (x, y, z). Defaults to (0, 0, 0.1). action_space_info: Optional action space metadata. + verify_grasp: If True, append a VerifyContactCfg after the post-grasp lift that + requires `object`'s physics-derived is_in_contact state to be True (i.e. the + object actually moved with the gripper, not just that the gripper closed for + long enough). Requires `object`'s scene config to expose is_in_contact via an + IsInContactPhysicsCfg semantic + ObsTerm -- see VerifyContactCfg's docstring. + Default: False (preserves prior pose/duration-only behavior). Note: num_envs, device, and dt are NOT in compositional configs - they're set by StateMachine @@ -45,6 +52,7 @@ class PickObjectCfg(CompositionalActionCfg): # Optional fields with defaults post_grasp_offset: tuple[float, float, float] = (0.0, 0.0, 0.1) action_space_info: ActionSpaceInfo | None = None + verify_grasp: bool = False def __post_init__(self): """Generate default sub_actions for pick sequence after initialization.""" @@ -80,3 +88,14 @@ def __post_init__(self): action_space_info=self.action_space_info, ), ] + + if self.verify_grasp: + # Checked AFTER the lift, not right after CloseGripper: fingers can register + # contact against an object still resting on the table without actually + # holding it. Requiring contact to persist through the lift is the real test. + self.sub_actions.append( + VerifyContactCfg( + object=self.object, + expected_contact=True, + ) + ) diff --git a/source/matterix_sm/matterix_sm/compositional_actions/place_object.py b/source/matterix_sm/matterix_sm/compositional_actions/place_object.py index 45b210e..38f1b0d 100644 --- a/source/matterix_sm/matterix_sm/compositional_actions/place_object.py +++ b/source/matterix_sm/matterix_sm/compositional_actions/place_object.py @@ -15,6 +15,7 @@ MoveRelativeCfg, MoveToFrameCfg, OpenGripperCfg, + VerifyContactCfg, ) from ..robot_action_spaces import ActionSpaceInfo @@ -31,6 +32,18 @@ class PlaceObjectCfg(CompositionalActionCfg): target: Name of the target object to place on. REQUIRED. post_place_offset: Offset for post-place retreat (x, y, z). Defaults to (0, 0, 0.1). action_space_info: Optional action space metadata. + held_object: Name of the object being placed (i.e. whatever a prior PickObjectCfg + in the same workflow picked up). Only needed when verify_release=True -- + PlaceObjectCfg otherwise has no way to know which object's contact state to + check, since it only ever references `target`, not what the gripper is + holding. Default: None. + verify_release: If True, append a VerifyContactCfg after the post-place retreat + that requires `held_object`'s physics-derived is_in_contact state to be + False (i.e. it actually let go, not stuck to a finger via friction/adhesion). + Requires `held_object` to be set, and its scene config to expose + is_in_contact via an IsInContactPhysicsCfg semantic + ObsTerm -- see + VerifyContactCfg's docstring. Default: False (preserves prior + pose/duration-only behavior). Note: num_envs, device, and dt are NOT in compositional configs - they're set by StateMachine @@ -44,6 +57,8 @@ class PlaceObjectCfg(CompositionalActionCfg): # Optional fields with defaults post_place_offset: tuple[float, float, float] = (0.0, 0.0, 0.1) action_space_info: ActionSpaceInfo | None = None + held_object: str | None = None + verify_release: bool = False def __post_init__(self): """Generate default sub_actions for place sequence after initialization.""" @@ -78,3 +93,18 @@ def __post_init__(self): action_space_info=self.action_space_info, ), ] + + if self.verify_release: + if self.held_object is None: + raise ValueError( + "PlaceObjectCfg.verify_release=True requires held_object to be set " + "to the name of the object being placed -- PlaceObjectCfg only ever " + "references `target`, so without held_object it has no way to know " + "which object's contact state to check on release." + ) + self.sub_actions.append( + VerifyContactCfg( + object=self.held_object, + expected_contact=False, + ) + ) diff --git a/source/matterix_sm/matterix_sm/primitive_actions/__init__.py b/source/matterix_sm/matterix_sm/primitive_actions/__init__.py index c039500..4410530 100644 --- a/source/matterix_sm/matterix_sm/primitive_actions/__init__.py +++ b/source/matterix_sm/matterix_sm/primitive_actions/__init__.py @@ -28,6 +28,7 @@ from .move_to_frame import MoveToFrame, MoveToFrameCfg from .move_to_pose import MoveToPose, MoveToPoseCfg from .semantic_action import SemanticAction, SemanticActionCfg +from .verify_contact import VerifyContact, VerifyContactCfg from .wait import Wait, WaitCfg __all__ = [ @@ -53,4 +54,7 @@ # Wait action "Wait", "WaitCfg", + # Verify contact action + "VerifyContact", + "VerifyContactCfg", ] diff --git a/source/matterix_sm/matterix_sm/primitive_actions/verify_contact.py b/source/matterix_sm/matterix_sm/primitive_actions/verify_contact.py new file mode 100644 index 0000000..82d2b6a --- /dev/null +++ b/source/matterix_sm/matterix_sm/primitive_actions/verify_contact.py @@ -0,0 +1,175 @@ +# Copyright (c) 2022-2026, The Matterix Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""VerifyContact action - checks a rigid object's physics-derived contact state.""" + +from __future__ import annotations + +import torch +from dataclasses import MISSING, field +from typing import ClassVar + +from .._compat import configclass +from ..primitive_action import PrimitiveAction, PrimitiveActionCfg +from ..scene_data import SceneData + + +@configclass +class VerifyContactCfg(PrimitiveActionCfg): + """Configuration for VerifyContact action. + + Checks a rigid object's ``is_in_contact`` state (populated in SceneData from an + ``IsInContactPhysicsCfg``/``IsInContactManual`` semantic, exposed via an ObsTerm + calling ``mdp.observations.object_is_in_contact``) against an expected value. Does + not move the robot -- the StateMachine preserves whatever action_dict values were + set by the previous action (same mechanism WaitCfg uses). + + Use ``expected_contact=True`` after closing the gripper on an object to verify it + was actually grasped (not just that the gripper closed for long enough). Use + ``expected_contact=False`` after opening the gripper to verify the object actually + released (not stuck to a finger via friction/adhesion) instead of assuming success + from gripper-open duration alone. + + Attributes: + agent_assets: Empty by default (no robots controlled). Leave as default. + object: Name of the rigid object whose is_in_contact state to check. REQUIRED. + expected_contact: True to require contact (grasp verification), False to require + no contact (release verification). Default: True. + settling_time: Time (in seconds) the contact state must hold continuously before + success. Filters single-frame contact-sensor flicker. Default: 0.1s. + + Note: + Requires the named object's scene config to attach a contact semantic (e.g. + ``IsInContactPhysicsCfg`` with ``activate_contact_sensors=True`` on the relevant + assets) AND expose it via an ObsTerm using + ``mdp.observations.object_is_in_contact``. If that plumbing is missing, the + action raises ValueError at check time rather than silently timing out. + + Example:: + + PickObjectCfg(agent_assets="robot", object="beaker", verify_grasp=True) + # is equivalent to appending, after the pick sequence's lift: + VerifyContactCfg(object="beaker", expected_contact=True) + """ + + agent_assets: str | list[str] = field(default_factory=list) + object: str = MISSING + expected_contact: bool = True + settling_time: float = 0.1 + + +class VerifyContact(PrimitiveAction): + """Check a rigid object's physics-derived contact state against an expected value.""" + + cfg_type: ClassVar[type] = VerifyContactCfg + + def __init__( + self, + object: str, + expected_contact: bool = True, + settling_time: float = 0.1, + timeout: float | None = None, + agent_assets: str | list[str] | None = None, + action_space_info=None, + semantics=None, + ): + """ + Args: + object: Name of the rigid object whose is_in_contact state to check. + expected_contact: True to require contact, False to require no contact. + settling_time: Time (in seconds) the state must hold before success. + timeout: Max time (in seconds) before timeout. Uses PrimitiveActionCfg's + default (TIMEOUT_DEFAULT) if None. + agent_assets: Unused (always empty) -- kept for from_cfg() symmetry. + action_space_info: Unused (this action never moves the robot). + semantics: Optional semantic actions to emit on success. + """ + from ..action_constants import TIMEOUT_DEFAULT + + super().__init__( + agent_assets or [], timeout if timeout is not None else TIMEOUT_DEFAULT, action_space_info, semantics + ) + self.object = object + self.expected_contact = expected_contact + self.settling_time = settling_time + + # Initialized in set_execution_params() + self.time_in_state = None + + def set_execution_params(self, num_envs: int, device: str | torch.device, dt: float) -> None: + """Set execution parameters and initialize the settling timer.""" + super().set_execution_params(num_envs, device, dt) + self.time_in_state = torch.zeros(self.num_envs, dtype=torch.float32, device=self.device) + + def _compute_action_impl(self, scene_data: SceneData, env_ids: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Return dummy tensors (robot holds last commanded state). + + Since agent_assets=[], StateMachine never uses these values -- the robot + continues executing its last commanded action unchanged, same as WaitCfg. + """ + action_tensor = torch.zeros(self.num_envs, 1, device=self.device) + action_mask = torch.zeros(1, dtype=torch.bool, device=self.device) + return action_tensor, action_mask + + def _check_completion_impl(self, scene_data: SceneData, env_ids: torch.Tensor) -> None: + """Check if the object's contact state matches expected_contact, settled. + + Args: + scene_data: Complete scene state container. + env_ids: Indices of active environments. + + Raises: + ValueError: If the object is missing from scene_data, or has no + is_in_contact data -- both are scene-authoring gaps this action cannot + recover from, so it fails loudly instead of silently timing out. + """ + if self.object not in scene_data.rigid_objects: + raise ValueError( + f"VerifyContact: object '{self.object}' not found in scene_data.rigid_objects. " + f"Available: {list(scene_data.rigid_objects.keys())}" + ) + + is_in_contact = scene_data.rigid_objects[self.object].is_in_contact + if is_in_contact is None: + raise ValueError( + f"VerifyContact: '{self.object}' has no is_in_contact data. Attach an " + "IsInContactPhysicsCfg (or IsInContactManual) semantic to it -- with " + "activate_contact_sensors=True on the relevant assets for the physics " + "variant -- and expose it via an ObsTerm using " + "mdp.observations.object_is_in_contact so it reaches the state machine." + ) + + in_expected_state = is_in_contact.to(self.device) == self.expected_contact + + # Settling timer: accumulate while in the expected state, reset otherwise -- + # same pattern as MoveToPose's position/orientation settling check. + self.time_in_state[env_ids] = torch.where( + in_expected_state[env_ids], + self.time_in_state[env_ids] + self.dt, + torch.zeros_like(self.time_in_state[env_ids]), + ) + + self._env_success_mask[env_ids] = self.time_in_state[env_ids] >= self.settling_time + self._env_failure_mask[env_ids] = False + + def _reset_impl(self, env_ids: torch.Tensor | None = None) -> None: + """Reset the settling timer when environments are reset.""" + if env_ids is None: + self.time_in_state.zero_() + else: + self.time_in_state[env_ids] = 0.0 + + @classmethod + def from_cfg(cls, cfg: VerifyContactCfg) -> VerifyContact: + """Create VerifyContact from configuration.""" + return cls( + object=cfg.object, + expected_contact=cfg.expected_contact, + settling_time=cfg.settling_time, + timeout=cfg.timeout, + agent_assets=cfg.agent_assets, + action_space_info=cfg.action_space_info, + semantics=cfg.semantics, + ) diff --git a/source/matterix_sm/matterix_sm/scene_data.py b/source/matterix_sm/matterix_sm/scene_data.py index 379bbca..d01ef1e 100644 --- a/source/matterix_sm/matterix_sm/scene_data.py +++ b/source/matterix_sm/matterix_sm/scene_data.py @@ -83,6 +83,18 @@ class RigidObjectData: stored in SceneData. """ + is_in_contact: torch.Tensor | None = None + """Physics-derived contact state, shape (num_envs,), or None if not populated. + + Sourced from an ``IsInContactPhysicsCfg``/``IsInContactManual`` semantic attached to + this object and exposed via an ``ObsTerm`` calling + ``matterix.envs.mdp.observations.object_is_in_contact`` (see + ``managers/semantics/primitive_semantics/is_in_contact.py``). None means the task's + observation/semantics config never populated this field for this object -- actions + that need it (e.g. ``VerifyContactCfg``) should raise a clear error rather than + silently treating it as "not in contact". + """ + @dataclass class SceneData: diff --git a/source/matterix_sm/matterix_sm/state_machine.py b/source/matterix_sm/matterix_sm/state_machine.py index cbc8053..73b3694 100644 --- a/source/matterix_sm/matterix_sm/state_machine.py +++ b/source/matterix_sm/matterix_sm/state_machine.py @@ -935,6 +935,7 @@ def _parse_rigid_object_data(self, data: dict | torch.Tensor, asset_name: str = lin_vel_w=lin_vel_w, ang_vel_w=ang_vel_w, frames=frames_dict, + is_in_contact=data.get("is_in_contact"), ) else: # Flat tensor format - TODO: define standard layout