diff --git a/scripts/environments/teleoperation/teleop_xr_agent.py b/scripts/environments/teleoperation/teleop_xr_agent.py index c4af675..e59f0c2 100644 --- a/scripts/environments/teleoperation/teleop_xr_agent.py +++ b/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)") diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py index 18bf439..2c1cad6 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py +++ b/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,