From d23898c3cb0d733797fcaa1eb1cb2cbd62ff6ea6 Mon Sep 17 00:00:00 2001 From: hangX Date: Thu, 19 Mar 2026 21:33:39 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=8C=E8=87=82=E5=A4=B4=E9=83=A8=E9=81=A5?= =?UTF-8?q?=E6=93=8D=E4=BD=9C=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../teleoperation/teleop_xr_agent.py | 50 ++- .../manager_based/il/open_drybox/__init__.py | 10 + .../il/open_drybox/mindrobot_2i_cfg.py | 177 +++++++++ .../mindrobot_2i_dual_arm_ik_env_cfg.py | 363 ++++++++++++++++++ 4 files changed, 598 insertions(+), 2 deletions(-) create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_dual_arm_ik_env_cfg.py diff --git a/scripts/environments/teleoperation/teleop_xr_agent.py b/scripts/environments/teleoperation/teleop_xr_agent.py index a2527b0..c4af675 100644 --- a/scripts/environments/teleoperation/teleop_xr_agent.py +++ b/scripts/environments/teleoperation/teleop_xr_agent.py @@ -83,7 +83,7 @@ from xr_utils.geometry import R_HEADSET_TO_WORLD _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/cam_head", + "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", } @@ -339,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 # ===================================================================== @@ -519,6 +556,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: @@ -534,11 +572,18 @@ def main() -> None: 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() @@ -554,10 +599,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() diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py index e91070b..6f57b30 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py +++ b/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, ) \ No newline at end of file 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 new file mode 100644 index 0000000..18bf439 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py @@ -0,0 +1,177 @@ +# 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", + 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" diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_dual_arm_ik_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_dual_arm_ik_env_cfg.py new file mode 100644 index 0000000..ef4e93a --- /dev/null +++ b/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