test_zed.py

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "deps/zed-isaac-sim"]
+	path = deps/zed-isaac-sim
+	url = https://github.com/stereolabs/zed-isaac-sim.git

scripts/environments/teleoperation/teleop_xr_agent.py

@@ -532,6 +532,74 @@ def main() -> None:
     clear_ik_target_state()
     teleop_interface.reset()
 
+    # Read contact sensor data directly from OmniGraph node USD attributes.
+    # The USD has action graphs that execute each physics step and write outputs to:
+    #   outputs:inContact (bool)  outputs:value (float, Newtons)
+    # /root in USD  →  /World/envs/env_0/Robot  in the loaded scene.
+    _CONTACT_OG_NODES = {
+        "left_r": (
+            "/World/envs/env_0/Robot"
+            "/l_zhuajia___m2_1_cs/isaac_read_contact_sensor_node"
+        ),
+        "left_l": (
+            "/World/envs/env_0/Robot"
+            "/l_zhuajia___m3_1_cs/isaac_read_contact_sensor_node"
+        ),
+    }
+
+    def _og_contact_reading(stage, node_path: str) -> tuple[bool, float]:
+        """Read one OmniGraph contact node output via USD attribute API."""
+        prim = stage.GetPrimAtPath(node_path)
+        if not prim.IsValid():
+            return False, float("nan")
+        in_contact_attr = prim.GetAttribute("outputs:inContact")
+        force_attr = prim.GetAttribute("outputs:value")
+        in_contact = in_contact_attr.Get() if in_contact_attr.IsValid() else False
+        force = force_attr.Get() if force_attr.IsValid() else 0.0
+        return bool(in_contact), float(force or 0.0)
+
+    # One-time diagnostic: run after first env.reset() so all prims exist
+    def _diag_contact_nodes() -> None:
+        import omni.usd
+        stage = omni.usd.get_context().get_stage()
+        print("\n[DIAG] OmniGraph contact node check:")
+        for name, path in _CONTACT_OG_NODES.items():
+            prim = stage.GetPrimAtPath(path)
+            if prim.IsValid():
+                has_in = prim.GetAttribute("outputs:inContact").IsValid()
+                has_val = prim.GetAttribute("outputs:value").IsValid()
+                print(f"  [{name}] ✓ prim valid | outputs:inContact={has_in} outputs:value={has_val}")
+            else:
+                # Try to find similar prims to hint correct path
+                parent_path = "/".join(path.split("/")[:-1])
+                parent = stage.GetPrimAtPath(parent_path)
+                print(f"  [{name}] ✗ NOT FOUND at: {path}")
+                if parent.IsValid():
+                    children = [c.GetName() for c in parent.GetChildren()]
+                    print(f"           parent exists, children: {children}")
+                else:
+                    gp_path = "/".join(path.split("/")[:-2])
+                    gp = stage.GetPrimAtPath(gp_path)
+                    print(f"           grandparent '{gp_path}' valid={gp.IsValid()}")
+
+    def read_gripper_contact() -> str:
+        """Return contact status string by reading OmniGraph node USD attributes."""
+        try:
+            import omni.usd
+            stage = omni.usd.get_context().get_stage()
+            c_r, f_r = _og_contact_reading(stage, _CONTACT_OG_NODES["left_r"])
+            c_l, f_l = _og_contact_reading(stage, _CONTACT_OG_NODES["left_l"])
+            if f_r != f_r:  # nan → prim not found
+                return "prim not found (run with diag)"
+            sym_r = "✓" if c_r else "✗"
+            sym_l = "✓" if c_l else "✗"
+            return f"{sym_r}R={f_r:.1f}N  {sym_l}L={f_l:.1f}N"
+        except Exception as _e:
+            return f"N/A ({_e})"
+
+    # One-time diagnostic: verify OmniGraph contact sensor node paths
+    _diag_contact_nodes()
+
     # Create 4 viewport windows bound to the robot cameras
     create_robot_viewports()
 
@@ -660,11 +728,13 @@ def main() -> None:
                         eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
                         eef_quat_left = policy_obs["eef_quat_left"][0].cpu().numpy()
                         eef_pos_right = policy_obs["eef_pos_right"][0].cpu().numpy()
+
                         print(f"\n---------------- [ROBOT STATE frame={sim_frame}] ----------------")
                         print(f"| Left  Arm Joints (rad):     {arm_joints}")
                         print(f"| Right Arm Joints (rad):     {right_arm_joints}")
                         print(f"| Left  EEF Pos (world, m):   {eef_pos_left}")
                         print(f"| Right EEF Pos (world, m):   {eef_pos_right}")
+                        print(f"| Left Gripper Contact:       {read_gripper_contact()}")
                         print(f"| Cmd left_arm(abs):          {last_act[:7]}")
                         print(f"| Cmd wheel vel (rad/s):      {last_act[7:11]}")
                         print(f"| Cmd left_gripper:           {last_act[11:12]}  (+1=open -1=close)")

source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py

@@ -20,7 +20,9 @@ from mindbot.utils.assets import MINDBOT_ASSETS_DIR
 
 MINDBOT_CFG = ArticulationCfg(
     spawn=sim_utils.UsdFileCfg(
-        usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindrobot_2i/mindrobot_2i.usd",
+        # usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindrobot_2i/mindrobot_2i.usd",
+        usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindrobot_2i/contact_sensor.usd",
+        activate_contact_sensors=True,
         rigid_props=sim_utils.RigidBodyPropertiesCfg(
             disable_gravity=False,
             max_depenetration_velocity=5.0,

test_zed.py

@@ -0,0 +1,40 @@
+import pyzed.sl as sl
+import time
+import cv2
+
+zed = sl.Camera()
+init = sl.InitParameters()
+init.set_from_stream("127.0.0.1", 30000)
+init.depth_mode = sl.DEPTH_MODE.NEURAL
+
+print("尝试连接...")
+err = zed.open(init)
+print(f"结果: {err}")
+
+if err == sl.ERROR_CODE.SUCCESS:
+    print("连接成功！等待流就绪...")
+    image_left = sl.Mat()
+    image_depth = sl.Mat()
+
+    for i in range(30):
+        if zed.grab() == sl.ERROR_CODE.SUCCESS:
+            zed.retrieve_image(image_left, sl.VIEW.LEFT)
+            zed.retrieve_image(image_depth, sl.VIEW.DEPTH)
+
+            left = image_left.get_data()
+            depth = image_depth.get_data()
+
+            cv2.imwrite("left_2i.png", left)
+            cv2.imwrite("depth_2i.png", depth)
+            print(f"已保存 left.png 和 depth.png，分辨率: {left.shape[1]}x{left.shape[0]}")
+            break
+        else:
+            print(f"  等待中... ({i+1}/30)")
+            time.sleep(0.5)
+    else:
+        print("超时：无法获取图像帧")
+
+    image_left.free()
+    image_depth.free()
+    time.sleep(0.5)
+    zed.close()