test_zed.py

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

.gitignore

@@ -74,3 +74,6 @@ tests/
 
 # Docker history
 .isaac-lab-docker-history
+
+# Local dependencies (not uploaded to repo)
+deps/XRoboToolkit-Teleop-Sample-Python

.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

@@ -74,6 +74,54 @@ from xr_utils import XrClient, transform_xr_pose, quat_diff_as_rotvec_xyzw, is_v
 from xr_utils.geometry import R_HEADSET_TO_WORLD
 
 
+# =====================================================================
+# Robot Camera Viewport Utilities
+# =====================================================================
+
+# Resolved prim paths for env_0 — must match CameraCfg prim_path with
+# {ENV_REGEX_NS} → /World/envs/env_0
+_ROBOT_CAM_PRIMS: dict[str, str] = {
+    "Left Hand":  "/World/envs/env_0/Robot/rm_65_fb_left/Link_6/robot___left_8_02/cam_left_hand",
+    "Right Hand": "/World/envs/env_0/Robot/rm_65_b_right/Link6/robot___right_8_2/cam_right_hand",
+    "Head":       "/World/envs/env_0/Robot/robot_head/ZED_X/base_link/ZED_X/CameraLeft",
+    "Chest":      "/World/envs/env_0/Robot/robot_trunk/cam_chest",
+}
+
+# Stores created viewport window objects
+_robot_viewports: dict[str, object] = {}
+
+
+def create_robot_viewports() -> None:
+    """Create 4 viewport windows and bind each to a robot camera prim.
+
+    Must be called after env.reset() so all prims exist on the USD stage.
+    """
+    try:
+        import omni.kit.viewport.utility as vp_util
+    except ImportError:
+        logger.warning("[Viewport] omni.kit.viewport.utility not available — skipping viewport creation.")
+        return
+
+    for name, cam_path in _ROBOT_CAM_PRIMS.items():
+        vp_win = vp_util.create_viewport_window(
+            f"Robot {name} View", width=640, height=360
+        )
+        vp_win.viewport_api.camera_path = cam_path
+        _robot_viewports[name] = vp_win
+        print(f"[INFO] Viewport 'Robot {name} View' bound to: {cam_path}")
+
+
+def rebind_robot_viewports() -> None:
+    """Re-bind all robot viewports to their camera paths.
+
+    Call this after every env reset so viewports stay locked to the cameras.
+    """
+    for name, vp_win in _robot_viewports.items():
+        cam_path = _ROBOT_CAM_PRIMS[name]
+        vp_win.viewport_api.camera_path = cam_path
+
+
+
 # =====================================================================
 # Helpers
 # =====================================================================
@@ -291,6 +339,43 @@ class XrTeleopController:
         return action
 
 
+# =====================================================================
+# Head Tracking Utility
+# =====================================================================
+
+def get_head_joint_targets(
+    xr_client, neutral_rot: R | None
+) -> tuple[np.ndarray, R | None]:
+    """Extract head joint targets [yaw, pitch] from HMD pose.
+
+    At first call (neutral_rot is None) the current headset orientation
+    is recorded as the neutral reference and zeros are returned.
+    Subsequent calls return yaw/pitch relative to that neutral pose.
+
+    Returns:
+        (joint_targets, neutral_rot) where joint_targets is shape (2,)
+        float32 [head_yaw_Joint_rad, head_pitch_Joint_rad].
+    """
+    try:
+        raw_pose = xr_client.get_pose("headset")
+        if not is_valid_quaternion(raw_pose[3:]):
+            return np.zeros(2, dtype=np.float32), neutral_rot
+        _, head_world_quat_xyzw = transform_xr_pose(raw_pose[:3], raw_pose[3:])
+        head_rot = R.from_quat(head_world_quat_xyzw)
+        if neutral_rot is None:
+            # First call — record neutral and hold zero
+            return np.zeros(2, dtype=np.float32), head_rot
+        # Relative rotation from neutral heading
+        rel_rot = head_rot * neutral_rot.inv()
+        # ZYX Euler: [0]=yaw (Z), [1]=pitch (Y)
+        euler_zyx = rel_rot.as_euler("ZYX")
+        yaw   = float(np.clip(euler_zyx[0], -1.57, 1.57))   # ±90°
+        pitch = float(np.clip(euler_zyx[1], -0.52, 0.52))   # ±30°
+        return np.array([yaw, pitch], dtype=np.float32), neutral_rot
+    except Exception:
+        return np.zeros(2, dtype=np.float32), neutral_rot
+
+
 # =====================================================================
 # Main Execution Loop
 # =====================================================================
@@ -446,6 +531,79 @@ def main() -> None:
     obs, _ = env.reset()
     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()
+
+
     if is_dual_arm:
         teleop_right_ref.reset()
 
@@ -466,6 +624,7 @@ def main() -> None:
     clear_ik_target_state()
     last_root_left = None
     last_root_right = None
+    neutral_head_rot = None  # calibrated on first headset sample after reset
 
     while simulation_app.is_running():
         try:
@@ -476,15 +635,23 @@ def main() -> None:
                     teleop_interface.reset()
                     if is_dual_arm:
                         teleop_right_ref.reset()
+                    rebind_robot_viewports()
                     should_reset = False
                     sim_frame = 0
                     last_root_left = None
                     last_root_right = None
+                    neutral_head_rot = None  # re-calibrate on next headset sample
                     continue
 
                 policy_obs = obs["policy"]
                 wheel_np, v_fwd, omega_base = get_chassis_commands()
 
+                # Head tracking: HMD yaw/pitch relative to neutral pose
+                head_cmd_np, neutral_head_rot = get_head_joint_targets(
+                    teleop_interface.xr_client, neutral_head_rot
+                )
+                head_cmd = torch.tensor(head_cmd_np, dtype=torch.float32)
+
                 if is_dual_arm:
                     eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
                     eef_quat_left = policy_obs["eef_quat_left"][0].cpu().numpy()
@@ -500,10 +667,11 @@ def main() -> None:
                     if teleop_right_ref.grip_active or last_root_right is None:
                         last_root_right = convert_action_world_to_root(right_action)[:7].clone()
 
-                    # Action layout: left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1)
+                    # Action layout: left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1) | head(2)
                     action_np = torch.cat([
                         last_root_left, wheel_np, left_action[7:8],
                         last_root_right, right_action[7:8],
+                        head_cmd,
                     ])
                 else:
                     eef_pos = policy_obs["eef_pos"][0].cpu().numpy()
@@ -560,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/config/extension.toml

@@ -20,6 +20,7 @@ keywords = ["extension", "template", "isaaclab"]
 "isaaclab_mimic" = {}
 "isaaclab_rl" = {}
 "isaaclab_tasks" = {}
+"sl.sensor.camera" = {}
 # NOTE: Add additional dependencies here
 
 [[python.module]]

source/mindbot/mindbot/assets/table.py

@@ -1,5 +1,6 @@
 from isaaclab.assets import AssetBaseCfg
 from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
+from isaaclab.sim.schemas.schemas_cfg import CollisionPropertiesCfg
 from mindbot.utils.assets import MINDBOT_ASSETS_DIR
 
 
@@ -11,5 +12,6 @@ TABLE_CFG = AssetBaseCfg(
     ),
     spawn=UsdFileCfg(
         usd_path=f"{MINDBOT_ASSETS_DIR}/sences/Table_C_coll/Table_C.usd",
+        collision_props=CollisionPropertiesCfg(collision_enabled=True),
     ),
 )

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

@@ -56,4 +56,14 @@ gym.register(
         "robomimic_bc_cfg_entry_point": f"{__name__}.agents:robomimic/bc_rnn_low_dim.json",
     },
     disable_env_checker=True,
+)
+
+gym.register(
+    id="Isaac-MindRobot-2i-DualArm-IK-Abs-v0",
+    entry_point="isaaclab.envs:ManagerBasedRLEnv",
+    kwargs={
+        "env_cfg_entry_point": f"{__name__}.mindrobot_2i_dual_arm_ik_env_cfg:MindRobot2iDualArmIKAbsEnvCfg",
+        "robomimic_bc_cfg_entry_point": f"{__name__}.agents:robomimic/bc_rnn_low_dim.json",
+    },
+    disable_env_checker=True,
 )

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

@@ -0,0 +1,179 @@
+# Copyright (c) 2024, MindRobot Project
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Configuration for MindRobot dual-arm robot.
+
+Arms are RealMan RM-65 (6-DOF, 5kg payload, 7.2kg self-weight, 610mm reach).
+Reference: https://develop.realman-robotics.com/robot4th/robotParameter/RM65OntologyParameters/
+
+The following configurations are available:
+
+* :obj:`MINDBOT_CFG`: Base configuration with gravity enabled (general purpose).
+* :obj:`MINDBOT_HIGH_PD_CFG`: Gravity disabled, stiffer PD for IK task-space control.
+"""
+
+import isaaclab.sim as sim_utils
+from isaaclab.actuators import ImplicitActuatorCfg
+from isaaclab.assets import ArticulationCfg
+
+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/contact_sensor.usd",
+        activate_contact_sensors=True,
+        rigid_props=sim_utils.RigidBodyPropertiesCfg(
+            disable_gravity=False,
+            max_depenetration_velocity=5.0,
+        ),
+        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
+            fix_root_link=False,
+            enabled_self_collisions=True,
+            solver_position_iteration_count=8,
+            solver_velocity_iteration_count=0,
+        ),
+    ),
+    init_state=ArticulationCfg.InitialStateCfg(
+        joint_pos={
+            # ====== Left arm (RM-65) — away from singularities ======
+            # Elbow singularity: q3=0; Wrist singularity: q5=0.
+            # Small offsets on q2/q4/q6 to avoid exact 90° Jacobian symmetry.
+            "l_joint1":  1.5708,   #  90°
+            "l_joint2": -1.2217,   # -70°  (offset from -90° for better elbow workspace)
+            "l_joint3": -1.5708,   # -90°  (away from elbow singularity q3=0)
+            "l_joint4":  1.3963,   #  80°  (offset from 90° to break wrist symmetry)
+            "l_joint5":  1.5708,   #  90°  (away from wrist singularity q5=0)
+            "l_joint6": -1.3963,   # -80°  (offset from -90° to break wrist symmetry)
+            # ====== Right arm (RM-65) — same joint values as left (verified visually) ======
+            "r_joint1":  1.5708,   #  90°
+            "r_joint2": -1.2217,   # -70°
+            "r_joint3": -1.5708,   # -90°  (away from elbow singularity q3=0)
+            "r_joint4":  1.3963,   #  80°
+            "r_joint5":  1.5708,   #  90°  (away from wrist singularity q5=0)
+            "r_joint6": 1.3963,   # -80°
+            # ====== Grippers (0=open) ======
+            "left_hand_joint_left": 0.0,
+            "left_hand_joint_right": 0.0,
+            "right_hand_joint_left": 0.0,
+            "right_hand_joint_right": 0.0,
+            # ====== Trunk & Head ======
+            "PrismaticJoint": 0.26,
+            "head_pitch_Joint": 0.0,
+            "head_yaw_Joint": 0.0,
+        },
+        pos=(0.0, 0.0, 0.7),
+    ),
+    actuators={
+        # RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s
+        "left_arm_shoulder": ImplicitActuatorCfg(
+            joint_names_expr=["l_joint[1-3]"],
+            effort_limit_sim=50.0,
+            velocity_limit_sim=3.14,
+            stiffness=80.0,
+            damping=4.0,
+        ),
+        # RM-65 J4-J6: wrist — max 225°/s ≈ 3.93 rad/s
+        "left_arm_wrist": ImplicitActuatorCfg(
+            joint_names_expr=["l_joint[4-6]"],
+            effort_limit_sim=20.0,
+            velocity_limit_sim=3.93,
+            stiffness=80.0,
+            damping=4.0,
+        ),
+        "right_arm_shoulder": ImplicitActuatorCfg(
+            joint_names_expr=["r_joint[1-3]"],
+            effort_limit_sim=50.0,
+            velocity_limit_sim=3.14,
+            stiffness=80.0,
+            damping=4.0,
+        ),
+        "right_arm_wrist": ImplicitActuatorCfg(
+            joint_names_expr=["r_joint[4-6]"],
+            effort_limit_sim=20.0,
+            velocity_limit_sim=3.93,
+            stiffness=80.0,
+            damping=4.0,
+        ),
+        "head": ImplicitActuatorCfg(
+            joint_names_expr=[
+                "head_yaw_Joint",
+                "head_pitch_Joint"
+                ],
+            effort_limit_sim=12.0,
+            stiffness=80.0,
+            damping=4.0,
+        ),
+        "trunk": ImplicitActuatorCfg(
+            joint_names_expr=["PrismaticJoint"],
+            effort_limit_sim=200.0,
+            velocity_limit_sim=0.2,
+            stiffness=1000.0,
+            damping=100.0,
+        ),
+        "wheels": ImplicitActuatorCfg(
+            # joint_names_expr=[".*_revolute_Joint"],
+            joint_names_expr=[
+                "right_b_revolute_Joint",
+                "left_b_revolute_Joint",
+                "left_f_revolute_Joint",
+                "right_f_revolute_Joint",
+            ],
+            effort_limit_sim=200.0,
+            stiffness=0.0,
+            damping=100.0,
+        ),
+        "grippers": ImplicitActuatorCfg(
+            joint_names_expr=[".*_hand_joint.*"],
+            effort_limit_sim=200.0,
+            stiffness=2e3,
+            damping=1e2,
+        ),
+    },
+    soft_joint_pos_limit_factor=1.0,
+)
+"""Base configuration for MindRobot. Gravity enabled, low PD gains."""
+
+
+MINDBOT_HIGH_PD_CFG = MINDBOT_CFG.copy()
+MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].stiffness = 400.0
+MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].damping = 80.0
+MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].stiffness = 400.0
+MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].damping = 80.0
+MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].stiffness = 400.0
+MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].damping = 80.0
+MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].stiffness = 400.0
+MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].damping = 80.0
+"""IK task-space control configuration. Gravity disabled, stiffer PD for tracking."""
+
+
+# =====================================================================
+# Robot Body / Joint Name Constants
+# Keep all robot-specific strings here so env cfgs stay robot-agnostic.
+# =====================================================================
+
+# Joint name patterns (regex, for use in action/actuator configs)
+MINDBOT_LEFT_ARM_JOINTS = "l_joint[1-6]"
+MINDBOT_RIGHT_ARM_JOINTS = "r_joint[1-6]"
+MINDBOT_LEFT_GRIPPER_JOINTS = ["left_hand_joint_left", "left_hand_joint_right"]
+MINDBOT_RIGHT_GRIPPER_JOINTS = ["right_hand_joint_left", "right_hand_joint_right"]
+MINDBOT_WHEEL_JOINTS = [
+    "right_b_revolute_Joint",
+    "left_b_revolute_Joint",
+    "left_f_revolute_Joint",
+    "right_f_revolute_Joint",
+]
+MINDBOT_HEAD_JOIONTS =[
+    "head_yaw_Joint",    #
+    "head_pitch_Joint"   
+]
+
+# EEF body names (as defined in the USD asset)
+MINDBOT_LEFT_EEF_BODY = "left_hand_body"
+MINDBOT_RIGHT_EEF_BODY = "right_hand_body"
+
+# Prim paths relative to the Robot prim (i.e. after "{ENV_REGEX_NS}/Robot/")
+MINDBOT_LEFT_ARM_PRIM_ROOT = "rm_65_fb_left"
+MINDBOT_RIGHT_ARM_PRIM_ROOT = "rm_65_b_right"
+MINDBOT_LEFT_EEF_PRIM = "rm_65_fb_left/l_hand_01/left_hand_body"
+MINDBOT_RIGHT_EEF_PRIM = "rm_65_b_right/r_hand/right_hand_body"

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

@@ -0,0 +1,363 @@
+# Copyright (c) 2024, MindRobot Project
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""MindRobot dual-arm IK absolute environment config for XR teleoperation."""
+
+from __future__ import annotations
+
+import torch
+
+import isaaclab.sim as sim_utils
+from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
+from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
+from isaaclab.devices import DevicesCfg
+from isaaclab.devices.keyboard import Se3KeyboardCfg
+from isaaclab.devices.spacemouse import Se3SpaceMouseCfg
+from isaaclab.devices.gamepad import Se3GamepadCfg
+from isaaclab.devices.openxr import XrCfg
+from isaaclab.envs import ManagerBasedRLEnvCfg
+from isaaclab.envs.mdp.actions.actions_cfg import (
+    BinaryJointPositionActionCfg,
+    DifferentialInverseKinematicsActionCfg,
+    JointPositionActionCfg,
+    JointVelocityActionCfg,
+)
+from isaaclab.managers import EventTermCfg as EventTerm
+from isaaclab.managers import ObservationGroupCfg as ObsGroup
+from isaaclab.managers import ObservationTermCfg as ObsTerm
+from isaaclab.managers import SceneEntityCfg
+from isaaclab.managers import TerminationTermCfg as DoneTerm
+from isaaclab.markers.config import FRAME_MARKER_CFG
+from isaaclab.scene import InteractiveSceneCfg
+from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg, OffsetCfg
+from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg
+from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg
+from isaaclab.sim.spawners.materials import RigidBodyMaterialCfg
+from isaaclab.utils import configclass
+from isaaclab.envs import ViewerCfg
+
+from . import mdp
+
+from .mindrobot_2i_cfg import (
+    MINDBOT_HIGH_PD_CFG,
+    MINDBOT_LEFT_ARM_JOINTS,
+    MINDBOT_RIGHT_ARM_JOINTS,
+    MINDBOT_LEFT_EEF_BODY,
+    MINDBOT_RIGHT_EEF_BODY,
+    MINDBOT_LEFT_GRIPPER_JOINTS,
+    MINDBOT_RIGHT_GRIPPER_JOINTS,
+    MINDBOT_WHEEL_JOINTS,
+    MINDBOT_HEAD_JOIONTS,
+    MINDBOT_LEFT_ARM_PRIM_ROOT,
+    MINDBOT_RIGHT_ARM_PRIM_ROOT,
+    MINDBOT_LEFT_EEF_PRIM,
+    MINDBOT_RIGHT_EEF_PRIM,
+)
+
+from mindbot.assets.lab import ROOM_CFG
+from mindbot.assets.table import TABLE_CFG
+from mindbot.assets.dryingbox import DRYINGBOX_CFG
+
+# =====================================================================
+# Scene Configuration
+# =====================================================================
+
+@configclass
+class MindRobotDualArmSceneCfg(InteractiveSceneCfg):
+    """Scene for MindRobot dual-arm teleoperation."""
+
+    plane = AssetBaseCfg(
+        prim_path="/World/GroundPlane",
+        init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.0, 0.0]),
+        spawn=GroundPlaneCfg(
+            # Moderate friction: enough traction to move forward, low enough to
+            # allow skid-steer lateral wheel slip for turning.
+            # Default (1.0/1.0) moves but can't turn; 0.4/0.3 spins in place.
+            physics_material=RigidBodyMaterialCfg(
+                static_friction=0.7,
+                dynamic_friction=0.5,
+                restitution=0.0,
+            )
+        ),
+    )
+    
+    room = ROOM_CFG.replace(prim_path="{ENV_REGEX_NS}/Room")
+    table = TABLE_CFG.replace(prim_path="{ENV_REGEX_NS}/Table")
+    drying_box = DRYINGBOX_CFG.replace(prim_path="{ENV_REGEX_NS}/DryingBox")
+
+    robot: ArticulationCfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
+
+    light = AssetBaseCfg(
+        prim_path="/World/light",
+        spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0),
+    )
+
+    # EEF frame transformers — set in __post_init__
+    ee_frame: FrameTransformerCfg = None
+    ee_frame_right: FrameTransformerCfg = None
+
+
+# =====================================================================
+# Actions Configuration
+# =====================================================================
+
+@configclass
+class MindRobotDualArmActionsCfg:
+    """Actions for MindRobot dual-arm IK teleoperation (absolute mode).
+
+    Action vector layout (total 22D):
+        left_arm_action     (7D): [x, y, z, qw, qx, qy, qz]  absolute EEF pose, root frame
+        wheel_action        (4D): [right_b, left_b, left_f, right_f] wheel vel (rad/s)
+        left_gripper_action (1D): +1=open, -1=close
+        right_arm_action    (7D): [x, y, z, qw, qx, qy, qz]  absolute EEF pose, root frame
+        right_gripper_action(1D): +1=open, -1=close
+        head_action         (2D): [yaw_rad, pitch_rad]  absolute joint position
+    """
+
+    left_arm_action = DifferentialInverseKinematicsActionCfg(
+        asset_name="robot",
+        joint_names=[MINDBOT_LEFT_ARM_JOINTS],
+        body_name=MINDBOT_LEFT_EEF_BODY,
+        controller=DifferentialIKControllerCfg(
+            command_type="pose",
+            use_relative_mode=False,
+            ik_method="dls",
+        ),
+        scale=1,
+        body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.0]),
+    )
+
+    wheel_action = JointVelocityActionCfg(
+        asset_name="robot",
+        joint_names=MINDBOT_WHEEL_JOINTS,
+        scale=1.0,
+    )
+
+    left_gripper_action = BinaryJointPositionActionCfg(
+        asset_name="robot",
+        joint_names=MINDBOT_LEFT_GRIPPER_JOINTS,
+        open_command_expr={"left_hand_joint_left": 0.0, "left_hand_joint_right": 0.0},
+        close_command_expr={"left_hand_joint_left": -0.03, "left_hand_joint_right": 0.03},
+    )
+
+    right_arm_action = DifferentialInverseKinematicsActionCfg(
+        asset_name="robot",
+        joint_names=[MINDBOT_RIGHT_ARM_JOINTS],
+        body_name=MINDBOT_RIGHT_EEF_BODY,
+        controller=DifferentialIKControllerCfg(
+            command_type="pose",
+            use_relative_mode=False,
+            ik_method="dls",
+        ),
+        scale=1,
+        body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.0]),
+    )
+
+    right_gripper_action = BinaryJointPositionActionCfg(
+        asset_name="robot",
+        joint_names=MINDBOT_RIGHT_GRIPPER_JOINTS,
+        open_command_expr={"right_hand_joint_left": 0.0, "right_hand_joint_right": 0.0},
+        close_command_expr={"right_hand_joint_left": -0.03, "right_hand_joint_right": 0.03},
+    )
+
+    head_action = JointPositionActionCfg(
+        asset_name="robot",
+        joint_names=MINDBOT_HEAD_JOIONTS,
+        scale=1.0,
+    )
+
+
+# =====================================================================
+# Observations Configuration
+# =====================================================================
+
+@configclass
+class MindRobotDualArmObsCfg:
+    """Observations for dual-arm teleoperation."""
+
+    @configclass
+    class PolicyCfg(ObsGroup):
+        actions = ObsTerm(func=mdp.last_action)
+        joint_pos = ObsTerm(func=mdp.joint_pos_rel)
+        joint_vel = ObsTerm(func=mdp.joint_vel_rel)
+        # Left arm EEF
+        eef_pos_left = ObsTerm(func=mdp.ee_frame_pos)
+        eef_quat_left = ObsTerm(func=mdp.ee_frame_quat)
+        # Left gripper (2 joints)
+        left_gripper_pos = ObsTerm(
+            func=mdp.gripper_pos,
+            params={"joint_names": MINDBOT_LEFT_GRIPPER_JOINTS},
+        )
+        # Right arm EEF
+        eef_pos_right = ObsTerm(
+            func=mdp.ee_frame_pos,
+            params={"ee_frame_cfg": SceneEntityCfg("ee_frame_right")},
+        )
+        eef_quat_right = ObsTerm(
+            func=mdp.ee_frame_quat,
+            params={"ee_frame_cfg": SceneEntityCfg("ee_frame_right")},
+        )
+        # Right gripper (2 joints)
+        right_gripper_pos = ObsTerm(
+            func=mdp.gripper_pos,
+            params={"joint_names": MINDBOT_RIGHT_GRIPPER_JOINTS},
+        )
+
+        def __post_init__(self):
+            self.enable_corruption = False
+            self.concatenate_terms = False
+
+    policy: PolicyCfg = PolicyCfg()
+
+
+# =====================================================================
+# Terminations Configuration
+# =====================================================================
+
+@configclass
+class MindRobotDualArmTerminationsCfg:
+    time_out = DoneTerm(func=mdp.time_out, time_out=True)
+
+
+# =====================================================================
+# Events Configuration
+# =====================================================================
+
+def _disable_arm_gravity(env, env_ids: torch.Tensor):
+    """Disable gravity for both arm subtrees at startup."""
+    import isaaclab.sim.schemas as schemas
+
+    arm_cfg = RigidBodyPropertiesCfg(disable_gravity=True)
+    for env_id in range(env.num_envs):
+        for arm_path in [
+            f"/World/envs/env_{env_id}/Robot/{MINDBOT_LEFT_ARM_PRIM_ROOT}",
+            f"/World/envs/env_{env_id}/Robot/{MINDBOT_RIGHT_ARM_PRIM_ROOT}",
+        ]:
+            schemas.modify_rigid_body_properties(arm_path, arm_cfg)
+
+
+@configclass
+class MindRobotDualArmEventsCfg:
+    disable_arm_gravity = EventTerm(
+        func=_disable_arm_gravity,
+        mode="startup",
+    )
+    reset_scene = EventTerm(
+        func=mdp.reset_scene_to_default,
+        mode="reset",
+    )
+
+
+# =====================================================================
+# Empty placeholder configs
+# =====================================================================
+
+@configclass
+class EmptyCfg:
+    pass
+
+
+# =====================================================================
+# Main Environment Configuration
+# =====================================================================
+
+@configclass
+class MindRobot2iDualArmIKAbsEnvCfg(ManagerBasedRLEnvCfg):
+    """MindRobot 双臂 IK-Abs 遥操作环境配置（XR 控制器）。"""
+
+    scene: MindRobotDualArmSceneCfg = MindRobotDualArmSceneCfg(
+        num_envs=1,
+        env_spacing=2.5,
+    )
+
+    observations: MindRobotDualArmObsCfg = MindRobotDualArmObsCfg()
+    actions: MindRobotDualArmActionsCfg = MindRobotDualArmActionsCfg()
+    terminations: MindRobotDualArmTerminationsCfg = MindRobotDualArmTerminationsCfg()
+    events: MindRobotDualArmEventsCfg = MindRobotDualArmEventsCfg()
+
+    commands: EmptyCfg = EmptyCfg()
+    rewards: EmptyCfg = EmptyCfg()
+    curriculum: EmptyCfg = EmptyCfg()
+
+    xr: XrCfg = XrCfg(
+        anchor_pos=(-0.1, -0.5, -1.05),
+        anchor_rot=(0.866, 0, 0, -0.5),
+    )
+
+    def __post_init__(self):
+        super().__post_init__()
+
+        self.viewer = ViewerCfg(
+            eye=(2.0, -1.5, 1.8),      # 相机位置
+            lookat=(0.0, 0.0, 0.8),    # 瞄准点（机器人腰部高度附近）
+            origin_type="world",
+        )
+        self.decimation = 2
+        self.episode_length_s = 50.0
+        self.sim.dt = 0.01  # 100 Hz
+        self.sim.render_interval = 2
+
+        # Mobile base — root link not fixed
+        robot_cfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
+        robot_cfg.spawn.articulation_props.fix_root_link = False
+        self.scene.robot = robot_cfg
+
+        # Left arm EEF frame transformer
+        marker_cfg = FRAME_MARKER_CFG.copy()
+        marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
+        marker_cfg.prim_path = "/Visuals/FrameTransformerLeft"
+        self.scene.ee_frame = FrameTransformerCfg(
+            prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_LEFT_ARM_PRIM_ROOT}/base_link",
+            debug_vis=False,
+            visualizer_cfg=marker_cfg,
+            target_frames=[
+                FrameTransformerCfg.FrameCfg(
+                    prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_LEFT_EEF_PRIM}",
+                    name="end_effector",
+                    offset=OffsetCfg(pos=[0.0, 0.0, 0.0]),
+                ),
+            ],
+        )
+
+        # Right arm EEF frame transformer
+        marker_cfg_right = FRAME_MARKER_CFG.copy()
+        marker_cfg_right.markers["frame"].scale = (0.1, 0.1, 0.1)
+        marker_cfg_right.prim_path = "/Visuals/FrameTransformerRight"
+        self.scene.ee_frame_right = FrameTransformerCfg(
+            prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_RIGHT_ARM_PRIM_ROOT}/base_link",
+            debug_vis=False,
+            visualizer_cfg=marker_cfg_right,
+            target_frames=[
+                FrameTransformerCfg.FrameCfg(
+                    prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_RIGHT_EEF_PRIM}",
+                    name="end_effector",
+                    offset=OffsetCfg(pos=[0.0, 0.0, 0.0]),
+                ),
+            ],
+        )
+
+        # Teleoperation devices
+        self.teleop_devices = DevicesCfg(
+            devices={
+                "keyboard": Se3KeyboardCfg(
+                    pos_sensitivity=0.05,
+                    rot_sensitivity=0.05,
+                    sim_device=self.sim.device,
+                ),
+                "spacemouse": Se3SpaceMouseCfg(
+                    pos_sensitivity=0.05,
+                    rot_sensitivity=0.05,
+                    sim_device=self.sim.device,
+                ),
+                "gamepad": Se3GamepadCfg(
+                    pos_sensitivity=0.1,
+                    rot_sensitivity=0.1,
+                    sim_device=self.sim.device,
+                ),
+            }
+        )
+
+        # Gripper params (left gripper for single-arm compat; dual-arm uses obs directly)
+        self.gripper_joint_names = MINDBOT_LEFT_GRIPPER_JOINTS
+        self.gripper_open_val = 0.0
+        self.gripper_threshold = 0.005

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

@@ -33,6 +33,7 @@ from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg
 from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg
 from isaaclab.sim.spawners.materials import RigidBodyMaterialCfg
 from isaaclab.utils import configclass
+from isaaclab.envs import ViewerCfg
 
 from . import mdp
 
@@ -277,6 +278,11 @@ class MindRobotDualArmIKAbsEnvCfg(ManagerBasedRLEnvCfg):
     def __post_init__(self):
         super().__post_init__()
 
+        self.viewer = ViewerCfg(
+            eye=(2.0, -1.5, 1.8),      # 相机位置
+            lookat=(0.0, 0.0, 0.8),    # 瞄准点（机器人腰部高度附近）
+            origin_type="world",
+        )
         self.decimation = 2
         self.episode_length_s = 50.0
         self.sim.dt = 0.01  # 100 Hz

source/mindbot/mindbot/utils/assets.py

@@ -20,6 +20,7 @@ import os
 ##
 MINDBOT_ASSETS_DIR: str = os.environ.get(
     "MINDBOT_ASSETS_DIR",
-    "/home/tangger/LYT/maic_usd_assets_moudle",
+    # "/home/tangger/LYT/maic_usd_assets_moudle",
+    "/home/maic/xh/maic_usd_assets_moudle"
     # "/home/maic/LYT/maic_usd_assets_moudle",
 )

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()