Add physics-derived grasp/release verification to Pick/Place

[lanafi00]

Description

PickObjectCfg/PlaceObjectCfg reported success from end-effector pose thresholds and gripper-hold duration alone, never checking whether the object was actually grasped or released. This fix 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 off, so existing workflows are unaffected.

Verified live: ran a full pick-and-place episode in Isaac Sim (Matterix-Test-Beaker-Lift-Franka-v1) with a contact sensor on the beaker filtered against the gripper fingers, verify_grasp=True and verify_release=True. Both VerifyContact steps succeeded against real ContactSensor force data.

Type of change

• New feature (non-breaking change which adds functionality)

Checklist

• [ X] I have run the pre-commit checks with <FULL_PATH_TO_ISAACLAB>/isaaclab.sh --format
• I have made corresponding changes to the documentation
• [ X] My changes generate no new warnings
• [ X] I have added tests that prove my fix is effective or that my feature works
• I have updated the changelog and the corresponding version in the extension's config/extension.toml file
• I have added my name to the CONTRIBUTORS.md or my name already exists there

[SissiFeng]

Thank you for adding opt-in grasp/release verification. The direction is useful, and keeping it disabled by default preserves the existing workflow behavior. However, I reproduced a multi-environment failure and found a gap in the contact contract, so I am requesting changes before merge.

What I reviewed and tested

• Reviewed PR commit: c12ffe36bdbe491ea4639f17db19612b626fd91f.
• Reviewed all seven changed files, the existing contact observation function, and the physics contact predicate.
• Applied the PR diff to a separate source snapshot of current main (5d86bd6e4fc7dd6ea83dead1d076c0176440be9e) and ran the existing CPU suite: 8 passed.
• Ran eight additional CPU contract cases against the original PR head: 4 passed, 4 failed. These exercised the actual StateMachine observation-parsing and action-execution paths with synthetic contact observations. The PR production code was not modified.
• Checked the existing CI results: CPU tests and pre-commit are green, but the PR adds no test files for the new feature.

I have not independently run the Isaac pick-and-place scenario reported in the description. The findings below distinguish the reproduced CPU failure from risks identified by source inspection.

Findings

1. Standard contact observations crash verification with multiple environments.

object_is_in_contact() returns a boolean tensor shaped (num_envs, 1). The new StateMachine parser passes this through unchanged, while VerifyContact.time_in_state is shaped (num_envs,).

In VerifyContact._check_completion_impl(), torch.where() broadcasts those inputs to (num_envs, num_envs), which cannot be assigned back to the one-dimensional timer.

Results using the standard observation shape:

Environments	expected_contact=True	expected_contact=False
1	Passed	Passed
2	RuntimeError	RuntimeError
8	RuntimeError	RuntimeError

The two-environment error is:

RuntimeError: shape mismatch: value tensor of shape [2, 2]
cannot be broadcast to indexing result of shape [2]

A minimal reproduction from the PR checkout is:

PYTHONPATH=source/matterix_sm python - <<'PY'
import torch
from matterix_sm import StateMachine, VerifyContactCfg

sm = StateMachine(num_envs=2, dt=0.05, device="cpu")
sm.set_action_sequence([VerifyContactCfg(object="beaker")])
sm.reset()
sm.step({
    "rigid_objects": {
        "beaker__is_in_contact": torch.ones((2, 1), dtype=torch.bool)
    }
})
PY

The other two passing contract cases checked settling/interruption and partial reset with flat (N,) input, and rejection of missing contact data. These controls do not remove the failure with the standard (N, 1) observation interface.

2. Generic contact is not sufficient to establish gripper grasp/release.

The new verification reads only the object's generic is_in_contact boolean. It does not identify the contacting gripper or validate that the value comes from an appropriately filtered physics sensor. The documented input also permits manual contact semantics.

The existing heat-transfer task provides a concrete example: its beaker contact predicate is filtered against ika_plate. If that contact observation is wired into the new verification, contact with the plate can satisfy expected_contact=True even without a gripper grasp. Conversely, a correctly released object resting on the plate can remain in contact and fail expected_contact=False.

These are configuration risks established by source inspection; I have not reproduced those physical scenarios in Isaac during this review. The reported successful run with finger-only filters does not establish correct behavior for other accepted configurations.

The documentation also states that the object actually moved with the gripper. A boolean contact check alone does not measure that motion.

Required before merge

1. Fix and validate the observation shape contract.
   Normalize the supported contact input to one value per environment and reject incompatible shapes clearly. Add tests for 1, 2, and 8 environments with both expected contact values, using the actual observation dictionary interface. Ensure timers and completion remain independent across environments, including interrupted contact and partial resets.

2. Make the gripper-contact requirements explicit and enforceable.
   Provide and validate the required physics-sensor filtering/configuration for grasp/release verification so support-surface contact or manually assigned semantics cannot silently serve as physical grasp evidence. A general-purpose contact primitive can remain useful, but the Pick/Place verification options need a clear contract for their stronger claims.

3. Commit feature-specific tests and a reproducible scene/workflow example.
   The current green CI covers existing tests, while the new multi-environment failure is not covered. Include the sensor configuration and observation wiring used by the reported live test, and cover missing data and unsuccessful verification as well as success.

4. Provide runtime evidence after the fixes.
   Run multi-environment Isaac validation covering successful grasp/release, a missed grasp, an object that remains in gripper contact after attempted release, and release onto a surface that still contacts the object. Include the tested code and asset revisions, dependency versions, commands, and result logs.

5. Align the documentation with the evidence actually checked.
   If the implementation verifies only sustained gripper contact, describe that limit. If it claims to verify that the object follows the lift, add the corresponding object-motion evidence.

The reproduced tensor-shape failure and the grasp/release contact contract should be resolved before approval. The opt-in design is reasonable, but the current implementation is not yet ready to merge.