diff --git a/scripts/environments/teleoperation/teleop_xr_agent.py b/scripts/environments/teleoperation/teleop_xr_agent.py index fb44ed3..deaede2 100644 --- a/scripts/environments/teleoperation/teleop_xr_agent.py +++ b/scripts/environments/teleoperation/teleop_xr_agent.py @@ -45,6 +45,14 @@ parser.add_argument( choices=["left", "right"], help="Which arm/controller to use.", ) +parser.add_argument( + "--base_speed", type=float, default=3.0, + help="Max wheel speed (rad/s) for joystick full forward.", +) +parser.add_argument( + "--base_turn", type=float, default=2.0, + help="Max wheel differential (rad/s) for joystick full left/right.", +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) args_cli = parser.parse_args() @@ -78,6 +86,31 @@ from xr_utils.geometry import R_HEADSET_TO_WORLD # Teleoperation Interface for XR # ===================================================================== +def _quat_wxyz_to_rotation(quat_wxyz: np.ndarray) -> R: + """Convert Isaac-style wxyz quaternion to scipy Rotation.""" + return R.from_quat([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]]) + + +def _rotation_to_quat_wxyz(rot: R) -> np.ndarray: + """Convert scipy Rotation quaternion to Isaac-style wxyz.""" + quat_xyzw = rot.as_quat() + return np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]) + + +def world_pose_to_root_frame( + pos_w: np.ndarray, + quat_wxyz: np.ndarray, + root_pos_w: np.ndarray, + root_quat_wxyz: np.ndarray, +) -> tuple[np.ndarray, np.ndarray]: + """Express a world-frame pose in the robot root frame.""" + root_rot = _quat_wxyz_to_rotation(root_quat_wxyz) + pose_rot = _quat_wxyz_to_rotation(quat_wxyz) + pos_root = root_rot.inv().apply(pos_w - root_pos_w) + quat_root = _rotation_to_quat_wxyz(root_rot.inv() * pose_rot) + return pos_root, quat_root + + class XrTeleopController: """Teleop controller for PICO XR headset.""" @@ -105,6 +138,10 @@ class XrTeleopController: self.grip_active = False self.frame_count = 0 + self.reset_button_latched = False + self.require_grip_reengage = False + self.grip_engage_threshold = 0.8 + self.grip_release_threshold = 0.2 # Callbacks (like reset, etc) self.callbacks = {} @@ -117,6 +154,11 @@ class XrTeleopController: self.prev_xr_quat = None self.grip_active = False self.frame_count = 0 + self.target_eef_pos = None + self.target_eef_quat = None + # Require one grip release after reset so stale controller motion + # cannot immediately drive the robot back toward the previous pose. + self.require_grip_reengage = True def close(self): self.xr_client.close() @@ -144,14 +186,17 @@ class XrTeleopController: Reads the XR controller. Relative bounds return 7D action tensor: [dx, dy, dz, drx, dry, drz, gripper] Absolute bounds return 8D action tensor: [x, y, z, qw, qx, qy, qz, gripper] - Note: current_eef_quat expects wxyz. + Note: in absolute mode current_eef_* and the returned target are in WORLD frame. + The caller is responsible for converting to root frame before sending to IK. """ # XR buttons check (e.g. A or B for reset) try: - if self.xr_client.get_button("B") or self.xr_client.get_button("Y"): + reset_pressed = self.xr_client.get_button("B") or self.xr_client.get_button("Y") + if reset_pressed and not self.reset_button_latched: if "RESET" in self.callbacks: self.callbacks["RESET"]() - except: + self.reset_button_latched = reset_pressed + except Exception: pass try: @@ -165,11 +210,23 @@ class XrTeleopController: if not is_valid_quaternion(raw_pose[3:]): return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) - # Transform XR pose pos directly via Matrix mapping for safety check (though we can map diffs next) - pos_w = self.R_headset_world @ raw_pose[:3] + if self.require_grip_reengage: + if grip <= self.grip_release_threshold: + self.require_grip_reengage = False + else: + if self.is_absolute and current_eef_pos is not None and current_eef_quat is not None: + self.target_eef_pos = current_eef_pos.copy() + self.target_eef_quat = current_eef_quat.copy() + return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) + + # Use hysteresis so noisy analog grip values do not accidentally re-enable teleop. + if self.grip_active: + grip_pressed = grip > self.grip_release_threshold + else: + grip_pressed = grip >= self.grip_engage_threshold # 握持键作为离合器 (Clutch) - if grip < 0.5: + if not grip_pressed: self.prev_xr_pos = None self.prev_xr_quat = None self.grip_active = False @@ -197,11 +254,9 @@ class XrTeleopController: # 2. Extract Delta POS in World frame world_delta_pos = (xr_world_pos - prev_xr_world_pos) * self.pos_sensitivity - pos_norm = np.linalg.norm((raw_pose[:3] - self.prev_xr_pos)) # 3. Extract Delta ROT in World frame world_delta_rot = quat_diff_as_rotvec_xyzw(prev_xr_world_quat_xyzw, xr_world_quat_xyzw) * self.rot_sensitivity - rot_norm = np.linalg.norm(quat_diff_as_rotvec_xyzw(self.prev_xr_quat, raw_pose[3:])) # 4. Gripper gripper_action = 1.0 if trigger > 0.5 else -1.0 @@ -211,16 +266,12 @@ class XrTeleopController: self.target_eef_pos = np.zeros(3) self.target_eef_quat = np.array([1.0, 0.0, 0.0, 0.0]) - # Position update (simple translation in world frame) + # Accumulate in world frame so VR direction always matches sim direction. self.target_eef_pos += world_delta_pos - # Rotation update: apply delta_R (in world frame) to target_R (in world frame) - # R_new = R_delta @ R_target - target_r = R.from_quat([self.target_eef_quat[1], self.target_eef_quat[2], self.target_eef_quat[3], self.target_eef_quat[0]]) + target_r = _quat_wxyz_to_rotation(self.target_eef_quat) delta_r = R.from_rotvec(world_delta_rot) - new_r = delta_r * target_r - new_rot_xyzw = new_r.as_quat() - self.target_eef_quat = np.array([new_rot_xyzw[3], new_rot_xyzw[0], new_rot_xyzw[1], new_rot_xyzw[2]]) + self.target_eef_quat = _rotation_to_quat_wxyz(delta_r * target_r) action = torch.tensor([ self.target_eef_pos[0], self.target_eef_pos[1], self.target_eef_pos[2], @@ -231,14 +282,15 @@ class XrTeleopController: self.prev_xr_quat = raw_pose[3:].copy() else: - # Relative clamping limits (used by relative mode to avoid divergence) - max_pos_delta = 0.04 - if pos_norm > max_pos_delta: - world_delta_pos = (world_delta_pos * max_pos_delta / pos_norm) + max_pos_delta = 0.05 + world_pos_norm = np.linalg.norm(world_delta_pos) + if world_pos_norm > max_pos_delta: + world_delta_pos = world_delta_pos * (max_pos_delta / world_pos_norm) - max_rot_delta = 0.02 - if rot_norm > max_rot_delta: - world_delta_rot = (world_delta_rot * max_rot_delta / rot_norm) + max_rot_delta = 0.15 + world_rot_norm = np.linalg.norm(world_delta_rot) + if world_rot_norm > max_rot_delta: + world_delta_rot = world_delta_rot * (max_rot_delta / world_rot_norm) action = torch.tensor([ world_delta_pos[0], world_delta_pos[1], world_delta_pos[2], @@ -256,11 +308,11 @@ class XrTeleopController: print(f"| Task Mode: {'ABSOLUTE' if self.is_absolute else 'RELATIVE'}") print(f"| Raw VR Pos (OpenXR): {np.array(raw_pose[:3])}") print(f"| Raw VR Quat (xyzw): {np.array(raw_pose[3:])}") - print(f"| XR Delta Pos (world): {world_delta_pos} (norm={pos_norm:.4f})") - print(f"| XR Delta Rot (world): {world_delta_rot} (norm={rot_norm:.4f})") + print(f"| XR Delta Pos (world): {world_delta_pos} (norm={np.linalg.norm(world_delta_pos):.4f})") + print(f"| XR Delta Rot (world): {world_delta_rot} (norm={np.linalg.norm(world_delta_rot):.4f})") if self.is_absolute: - print(f"| Targ Pos (dx,dy,dz): {action[:3].numpy()}") - print(f"| Targ Quat (wxyz): {action[3:7].numpy()}") + print(f"| Targ Pos (world): {action[:3].numpy()}") + print(f"| Targ Quat (world, wxyz): {action[3:7].numpy()}") else: print(f"| Sent Action Pos (dx,dy,dz): {action[:3].numpy()}") print(f"| Sent Action Rot (rx,ry,rz): {action[3:6].numpy()}") @@ -313,30 +365,114 @@ def main() -> None: teleop_interface.add_callback("RESET", request_reset) + def get_arm_action_term(): + return env.action_manager._terms["arm_action"] + + def clear_ik_target_state(): + """Clear the internal IK target so reset does not reuse the previous pose command.""" + if not is_abs_mode: + return + arm_action_term = get_arm_action_term() + ee_pos_b, ee_quat_b = arm_action_term._compute_frame_pose() + arm_action_term._raw_actions.zero_() + arm_action_term._processed_actions.zero_() + arm_action_term._ik_controller._command.zero_() + arm_action_term._ik_controller.ee_pos_des[:] = ee_pos_b + arm_action_term._ik_controller.ee_quat_des[:] = ee_quat_b + + def convert_action_world_to_root(action_tensor: torch.Tensor) -> torch.Tensor: + """Convert an absolute IK action from world frame to robot root frame.""" + robot = env.unwrapped.scene["robot"] + root_pos_w = robot.data.root_pos_w[0].detach().cpu().numpy() + root_quat_w = robot.data.root_quat_w[0].detach().cpu().numpy() # wxyz + target_pos_w = action_tensor[:3].numpy() + target_quat_w = action_tensor[3:7].numpy() + pos_root, quat_root = world_pose_to_root_frame( + target_pos_w, target_quat_w, root_pos_w, root_quat_w, + ) + out = action_tensor.clone() + out[:3] = torch.tensor(pos_root, dtype=torch.float32) + out[3:7] = torch.tensor(quat_root, dtype=torch.float32) + return out + + def get_wheel_action() -> torch.Tensor: + """Read left joystick and return 4-DOF wheel velocity command. + + Skid-steer differential drive. + Joystick: Y-axis (+1 = forward), X-axis (+1 = right turn). + + Joint order from articulation (terminal log): + [right_b, left_b, left_f, right_f] + + Right/left sign convention assumes both sides' joints have the same + axis direction (positive velocity = forward). If the robot drives + backward when pushing forward, negate base_speed in the launch command. + """ + try: + joy = teleop_interface.xr_client.get_joystick("left") + jy = float(joy[1]) # forward / backward + jx = float(joy[0]) # right / left + except Exception: + return torch.zeros(4) + + v = jy * args_cli.base_speed + omega = jx * args_cli.base_turn + + # Positive omega = turn right → left wheels faster, right wheels slower + right_vel = v - omega + left_vel = v + omega + + return torch.tensor( + [right_vel, left_vel, left_vel, right_vel], dtype=torch.float32 + ) + env.reset() + clear_ik_target_state() teleop_interface.reset() print("\n" + "=" * 50) print(" 🚀 Teleoperation Started!") print(" 🎮 Use the TRIGGER to open/close gripper.") - print(" ✊ Hold GRIP button and move the controller to move the arm.") - print(" 🕹️ Press B or Y to reset the environment.") + print(" ✊ Hold GRIP and move the controller to move the arm.") + print(" 🕹️ Left joystick: Y=forward/back, X=turn left/right.") + print(" 🔄 Press B or Y to reset the environment.") print("=" * 50 + "\n") device = env.unwrapped.device sim_frame = 0 obs, _ = env.reset() + clear_ik_target_state() while simulation_app.is_running(): try: with torch.inference_mode(): - # Extract tracking pose to seed absolute IK + if should_reset: + obs, _ = env.reset() + clear_ik_target_state() + teleop_interface.reset() + should_reset = False + sim_frame = 0 + continue + + # Read current EEF in world frame from observations. policy_obs = obs["policy"] eef_pos = policy_obs["eef_pos"][0].cpu().numpy() eef_quat = policy_obs["eef_quat"][0].cpu().numpy() - # Get action from XR Controller - action_np = teleop_interface.advance(current_eef_pos=eef_pos, current_eef_quat=eef_quat) + # Get action from XR Controller (world frame for absolute mode). + action_np = teleop_interface.advance( + current_eef_pos=eef_pos, current_eef_quat=eef_quat, + ) + + # IK expects root-frame commands; convert just before sending. + if is_abs_mode: + action_np = convert_action_world_to_root(action_np) + + # Action manager order: arm_action | wheel_action | gripper_action + # arm=7, wheel=4, gripper=1 → total 12 dims. + wheel_np = get_wheel_action() + action_np = torch.cat([action_np[:7], wheel_np, action_np[7:]]) + actions = action_np.unsqueeze(0).repeat(env.num_envs, 1).to(device) # Step environment @@ -363,30 +499,32 @@ def main() -> None: print(f" [{i:2d}] {name:30s} = {joint_pos[i]:+.4f}") print(f"{'='*70}") # Find arm joint indices dynamically by looking at the first 6-7 joints that aren't fingers or hands - arm_idx = [i for i, n in enumerate(jnames) if "finger" not in n and "hand" not in n][:7] - print(f" Deduced arm indices: {arm_idx}") + arm_idx = [i for i, n in enumerate(jnames) if n.startswith("l_joint")] + print(f" Deduced left arm indices: {arm_idx}") print(f"{'='*70}\n") # Get arm indices (cache-friendly: find once) if not hasattr(env, '_arm_idx_cache'): robot = env.unwrapped.scene["robot"] jnames = robot.joint_names - env._arm_idx_cache = [i for i, n in enumerate(jnames) if "finger" not in n and "hand" not in n][:7] + env._arm_idx_cache = [i for i, n in enumerate(jnames) if n.startswith("l_joint")] arm_idx = env._arm_idx_cache arm_joints = joint_pos[arm_idx] + try: + joy_dbg = teleop_interface.xr_client.get_joystick("left") + joy_str = f"[{joy_dbg[0]:+.2f}, {joy_dbg[1]:+.2f}]" + except Exception: + joy_str = "N/A" + print(f"\n---------------- [ROBOT STATE frame={sim_frame}] ----------------") print(f"| Left Arm Joints (rad): {arm_joints}") print(f"| EEF Pos (world): {eef_pos}") print(f"| EEF Quat (world, wxyz): {eef_quat}") print(f"| Last Action Sent: {last_act}") + print(f"| Joystick (x=turn,y=fwd): {joy_str}") print(f"----------------------------------------------------------------") - if should_reset: - env.reset() - teleop_interface.reset() - should_reset = False - except Exception as e: logger.error(f"Error during simulation step: {e}") break diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py index b956ba0..4ec0acd 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py @@ -1,136 +1,135 @@ -# 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/mindbot/mindbot_cd2.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. - # The pose below keeps q3≠0 and q5≠0. - "l_joint1": 1.5708, # 135° - "l_joint2": -1.5708, # -70° - "l_joint3": -1.5708, # -90° (away from elbow singularity q3=0) - "l_joint4": 1.5708, # 90° - "l_joint5": 1.5708, # 90° (away from wrist singularity q5=0) - "l_joint6": -1.5708, # -70° - # ====== Right arm (RM-65) ====== - "r_joint1": -2.3562, - "r_joint2": -1.2217, - "r_joint3": 1.5708, - "r_joint4": -1.5708, - "r_joint5": -1.5708, - "r_joint6": 1.2217, - # ====== 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_revoluteJoint": 0.0, - }, - pos=(0.0, 0.0, 0.0), - ), - 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_revoluteJoint"], - 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"], - 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.spawn.rigid_props.disable_gravity = True -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.""" +# 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/mindbot/mindbot_cd2.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. + # The pose below keeps q3≠0 and q5≠0. + "l_joint1": 1.5708, # 135° + "l_joint2": -1.5708, # -70° + "l_joint3": -1.5708, # -90° (away from elbow singularity q3=0) + "l_joint4": 1.5708, # 90° + "l_joint5": 1.5708, # 90° (away from wrist singularity q5=0) + "l_joint6": -1.5708, # -70° + # ====== Right arm (RM-65) ====== + "r_joint1": -2.3562, + "r_joint2": -1.2217, + "r_joint3": 1.5708, + "r_joint4": -1.5708, + "r_joint5": -1.5708, + "r_joint6": 1.2217, + # ====== 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_revoluteJoint": 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_revoluteJoint"], + 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"], + 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.""" diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py index d162115..c1614e6 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py @@ -1,317 +1,355 @@ -# Copyright (c) 2024, MindRobot Project -# SPDX-License-Identifier: BSD-3-Clause - -"""MindRobot left arm IK-Rel environment config for teleoperation.""" - -from __future__ import annotations -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR -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.envs import ManagerBasedRLEnvCfg -from isaaclab.envs.mdp.actions.actions_cfg import ( - BinaryJointPositionActionCfg, - DifferentialInverseKinematicsActionCfg, -) -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 TerminationTermCfg as DoneTerm -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg -from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg -from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg -from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg -from isaaclab.utils import configclass -from isaaclab.markers.config import FRAME_MARKER_CFG -from isaaclab.devices.openxr import XrCfg - -# 导入基础配置和MDP函数 -from isaaclab_tasks.manager_based.manipulation.stack import mdp - -# 导入 MindRobot 资产 -from .mindrobot_cfg import MINDBOT_HIGH_PD_CFG - -# 在文件开头添加 -import isaaclab.utils.assets as assets_utils -from mindbot.utils.assets import MINDBOT_ASSETS_DIR - -# # 然后在 scene 配置中使用 -# spawn=sim_utils.UsdFileCfg( -# usd_path=f"{assets_utils.ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" -# ), -# ===================================================================== -# Scene Configuration -# ===================================================================== - - -@configclass -class MindRobotTeleopSceneCfg(InteractiveSceneCfg): - """Minimal scene for MindRobot teleoperation: robot + table + optional cube.""" - - # Ground plane - plane = AssetBaseCfg( - prim_path="/World/GroundPlane", - init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.0, 0.0]), - spawn=GroundPlaneCfg(), - ) - - # # Table - # table = AssetBaseCfg( - # prim_path="{ENV_REGEX_NS}/Table", - # init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), - # spawn=sim_utils.UsdFileCfg( - # usd_path=f"{assets_utils.ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" - # ), - # ) - - # Optional: Single cube for testing (can be removed if not needed) - cube = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - spawn=sim_utils.CuboidCfg( - size=(0.04, 0.04, 0.04), - rigid_props=RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=0.1), - collision_props=sim_utils.CollisionPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 0.45)), - ) - - # room = AssetBaseCfg( - # prim_path="{ENV_REGEX_NS}/Room", - # spawn=sim_utils.UsdFileCfg( - # usd_path=f"{MINDBOT_ASSETS_DIR}/twinlab/Collected_lab2/lab.usd", - # ), - # ) - - # MindRobot - robot: ArticulationCfg = MINDBOT_HIGH_PD_CFG.replace( - prim_path="{ENV_REGEX_NS}/Robot" - ) - - # Lighting - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), - ) - - # End-effector frame transformer - ee_frame: FrameTransformerCfg = None # Will be set in __post_init__ - - -# ===================================================================== -# Actions Configuration -# ===================================================================== - - -@configclass -class MindRobotTeleopActionsCfg: - """Actions for MindRobot left arm IK teleoperation.""" - - # Left arm IK control - arm_action = DifferentialInverseKinematicsActionCfg( - asset_name="robot", - joint_names=["l_joint[1-6]"], - body_name="left_hand_body", - controller=DifferentialIKControllerCfg( - command_type="pose", - use_relative_mode=True, - ik_method="dls", - ), - scale=1, - body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg( - pos=[0.0, 0.0, 0.0], - ), - ) - - # Left gripper control (binary: open/close) - gripper_action = BinaryJointPositionActionCfg( - asset_name="robot", - joint_names=["left_hand_joint_left", "left_hand_joint_right"], - 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, - }, - ) - - -# ===================================================================== -# Observations Configuration -# ===================================================================== - - -@configclass -class MindRobotTeleopObsCfg: - """Minimal observations for MindRobot teleoperation.""" - - @configclass - class PolicyCfg(ObsGroup): - """Observations for policy group - only robot state, no cube dependencies.""" - - actions = ObsTerm(func=mdp.last_action) - joint_pos = ObsTerm(func=mdp.joint_pos_rel) - joint_vel = ObsTerm(func=mdp.joint_vel_rel) - eef_pos = ObsTerm(func=mdp.ee_frame_pos) - eef_quat = ObsTerm(func=mdp.ee_frame_quat) - gripper_pos = ObsTerm(func=mdp.gripper_pos) - - def __post_init__(self): - self.enable_corruption = False - self.concatenate_terms = False - - # observation groups - policy: PolicyCfg = PolicyCfg() - - -# ===================================================================== -# Terminations Configuration -# ===================================================================== - - -@configclass -class MindRobotTeleopTerminationsCfg: - """Minimal terminations for teleoperation - only time_out or none.""" - - # Optional: Keep time_out for safety, or remove entirely - time_out = DoneTerm(func=mdp.time_out, time_out=True) - - -# ===================================================================== -# Events Configuration -# ===================================================================== - -@configclass -class MindRobotTeleopEventsCfg: - """Reset events for teleoperation: R键重置时将场景恢复到初始状态。""" - - reset_scene = EventTerm( - func=mdp.reset_scene_to_default, - mode="reset", - ) - - -# ===================================================================== -# Main Environment Configuration -# ===================================================================== - - -@configclass -class EmptyCfg: - pass - - -@configclass -class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): - """MindRobot 左臂 IK-Rel 遥操作环境配置（最小化版本）。""" - - # Scene settings - scene: MindRobotTeleopSceneCfg = MindRobotTeleopSceneCfg( - num_envs=1, - env_spacing=2.5, - ) - - # Basic settings - observations: MindRobotTeleopObsCfg = MindRobotTeleopObsCfg() - actions: MindRobotTeleopActionsCfg = MindRobotTeleopActionsCfg() - - # MDP settings - terminations: MindRobotTeleopTerminationsCfg = MindRobotTeleopTerminationsCfg() - events: MindRobotTeleopEventsCfg = MindRobotTeleopEventsCfg() - - # Unused managers - commands: EmptyCfg = EmptyCfg() - rewards: EmptyCfg = EmptyCfg() - curriculum: EmptyCfg = EmptyCfg() - - # XR configuration for hand tracking (if needed) - xr: XrCfg = XrCfg( - anchor_pos=(-0.1, -0.5, -1.05), - anchor_rot=(0.866, 0, 0, -0.5), - ) - - def __post_init__(self): - """Post initialization.""" - super().__post_init__() - - # General settings - self.decimation = 2 - self.episode_length_s = 50.0 - - # Simulation settings - self.sim.dt = 0.01 # 100Hz - self.sim.render_interval = 2 - - # Set MindRobot with FIXED root for teleoperation - # The original MINDBOT_HIGH_PD_CFG has fix_root_link=False (mobile base). - # For teleoperation, the base MUST be fixed to prevent the whole robot - # from sliding/tipping when IK applies torques to the arm joints. - robot_cfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - robot_cfg.spawn.articulation_props.fix_root_link = True - self.scene.robot = robot_cfg - - # Configure end-effector frame transformer - marker_cfg = FRAME_MARKER_CFG.copy() - marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) - marker_cfg.prim_path = "/Visuals/FrameTransformer" - self.scene.ee_frame = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/base_link", - debug_vis=False, - visualizer_cfg=marker_cfg, - target_frames=[ - FrameTransformerCfg.FrameCfg( - prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/l_hand_01/left_hand_body", - name="end_effector", - offset=OffsetCfg( - pos=[0.0, 0.0, 0.0], - ), - ), - ], - ) - - - # Configure 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 parameters for status checking - self.gripper_joint_names = ["left_hand_joint_left", "left_hand_joint_right"] - self.gripper_open_val = 0.0 - self.gripper_threshold = 0.005 - - -@configclass -class MindRobotLeftArmIKAbsEnvCfg(MindRobotLeftArmIKEnvCfg): - """MindRobot 左臂 IK-Abs 遥操作环境配置（绝对坐标系，适合 VR 控制器）。""" - - def __post_init__(self): - super().__post_init__() - # Switch controller to absolute mode - self.actions.arm_action.controller.use_relative_mode = False +# Copyright (c) 2024, MindRobot Project +# SPDX-License-Identifier: BSD-3-Clause + +"""MindRobot left arm IK-Rel environment config for teleoperation.""" + +from __future__ import annotations +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +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.envs import ManagerBasedRLEnvCfg +from isaaclab.envs.mdp.actions.actions_cfg import ( + BinaryJointPositionActionCfg, + DifferentialInverseKinematicsActionCfg, + 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 TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg +from isaaclab.utils import configclass +from isaaclab.markers.config import FRAME_MARKER_CFG +from isaaclab.devices.openxr import XrCfg + +# 导入基础配置和MDP函数 +from isaaclab_tasks.manager_based.manipulation.stack import mdp + +# 导入 MindRobot 资产 +from .mindrobot_cfg import MINDBOT_HIGH_PD_CFG + +# 在文件开头添加 +import isaaclab.utils.assets as assets_utils +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + +# # 然后在 scene 配置中使用 +# spawn=sim_utils.UsdFileCfg( +# usd_path=f"{assets_utils.ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" +# ), +# ===================================================================== +# Scene Configuration +# ===================================================================== + + +@configclass +class MindRobotTeleopSceneCfg(InteractiveSceneCfg): + """Minimal scene for MindRobot teleoperation: robot + table + optional cube.""" + + # Ground plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.0, 0.0]), + spawn=GroundPlaneCfg(), + ) + + # # Table + # table = AssetBaseCfg( + # prim_path="{ENV_REGEX_NS}/Table", + # init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), + # spawn=sim_utils.UsdFileCfg( + # usd_path=f"{assets_utils.ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" + # ), + # ) + + # Optional: Single cube for testing (can be removed if not needed) + cube = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.04, 0.04, 0.04), + rigid_props=RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.1), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 0.45)), + ) + + # room = AssetBaseCfg( + # prim_path="{ENV_REGEX_NS}/Room", + # spawn=sim_utils.UsdFileCfg( + # usd_path=f"{MINDBOT_ASSETS_DIR}/twinlab/Collected_lab2/lab.usd", + # ), + # ) + + # MindRobot + robot: ArticulationCfg = MINDBOT_HIGH_PD_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot" + ) + + # Lighting + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + # End-effector frame transformer + ee_frame: FrameTransformerCfg = None # Will be set in __post_init__ + + +# ===================================================================== +# Actions Configuration +# ===================================================================== + + +@configclass +class MindRobotTeleopActionsCfg: + """Actions for MindRobot left arm IK teleoperation.""" + + # Left arm IK control + arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["l_joint[1-6]"], + body_name="left_hand_body", + controller=DifferentialIKControllerCfg( + command_type="pose", + use_relative_mode=True, + ik_method="dls", + ), + scale=1, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg( + pos=[0.0, 0.0, 0.0], + ), + ) + + # Wheel velocity control for differential drive (skid-steer). + # Joint order in articulation: right_b, left_b, left_f, right_f + # (from terminal joint index listing: [2],[3],[4],[5]). + # Action vector: [right_b_vel, left_b_vel, left_f_vel, right_f_vel] in rad/s. + wheel_action = JointVelocityActionCfg( + asset_name="robot", + joint_names=[".*_revolute_Joint"], + scale=1.0, + ) + + # Left gripper control (binary: open/close) + gripper_action = BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["left_hand_joint_left", "left_hand_joint_right"], + 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, + }, + ) + + +# ===================================================================== +# Observations Configuration +# ===================================================================== + + +@configclass +class MindRobotTeleopObsCfg: + """Minimal observations for MindRobot teleoperation.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group - only robot state, no cube dependencies.""" + + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + eef_pos = ObsTerm(func=mdp.ee_frame_pos) + eef_quat = ObsTerm(func=mdp.ee_frame_quat) + gripper_pos = ObsTerm(func=mdp.gripper_pos) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +# ===================================================================== +# Terminations Configuration +# ===================================================================== + + +@configclass +class MindRobotTeleopTerminationsCfg: + """Minimal terminations for teleoperation - only time_out or none.""" + + # Optional: Keep time_out for safety, or remove entirely + 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; chassis/wheels/trunk keep gravity. + + Called once at startup. The @apply_nested decorator on + modify_rigid_body_properties recurses into all rigid-body children under + the given prim path, so every link of the arm (including gripper) is covered. + + Arm prim roots (per env): + Robot/rm_65_fb_left — left RM-65 arm + gripper + Robot/rm_65_fb_right — right RM-65 arm + gripper + """ + 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/rm_65_fb_left", + f"/World/envs/env_{env_id}/Robot/rm_65_b_right", + ]: + schemas.modify_rigid_body_properties(arm_path, arm_cfg) + + +@configclass +class MindRobotTeleopEventsCfg: + """Reset events for teleoperation: R键重置时将场景恢复到初始状态。""" + + disable_arm_gravity = EventTerm( + func=_disable_arm_gravity, + mode="startup", + ) + + reset_scene = EventTerm( + func=mdp.reset_scene_to_default, + mode="reset", + ) + + +# ===================================================================== +# Main Environment Configuration +# ===================================================================== + + +@configclass +class EmptyCfg: + pass + + +@configclass +class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): + """MindRobot 左臂 IK-Rel 遥操作环境配置（最小化版本）。""" + + # Scene settings + scene: MindRobotTeleopSceneCfg = MindRobotTeleopSceneCfg( + num_envs=1, + env_spacing=2.5, + ) + + # Basic settings + observations: MindRobotTeleopObsCfg = MindRobotTeleopObsCfg() + actions: MindRobotTeleopActionsCfg = MindRobotTeleopActionsCfg() + + # MDP settings + terminations: MindRobotTeleopTerminationsCfg = MindRobotTeleopTerminationsCfg() + events: MindRobotTeleopEventsCfg = MindRobotTeleopEventsCfg() + + # Unused managers + commands: EmptyCfg = EmptyCfg() + rewards: EmptyCfg = EmptyCfg() + curriculum: EmptyCfg = EmptyCfg() + + # XR configuration for hand tracking (if needed) + xr: XrCfg = XrCfg( + anchor_pos=(-0.1, -0.5, -1.05), + anchor_rot=(0.866, 0, 0, -0.5), + ) + + def __post_init__(self): + """Post initialization.""" + super().__post_init__() + + # General settings + self.decimation = 2 + self.episode_length_s = 50.0 + + # Simulation settings + self.sim.dt = 0.01 # 100Hz + self.sim.render_interval = 2 + + # Keep the base mobile for teleoperation. + # The absolute arm IK command must therefore be expressed in the robot + # root frame instead of assuming a fixed world-aligned base. + 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 + + # Configure end-effector frame transformer + marker_cfg = FRAME_MARKER_CFG.copy() + marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + marker_cfg.prim_path = "/Visuals/FrameTransformer" + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/base_link", + debug_vis=False, + visualizer_cfg=marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/l_hand_01/left_hand_body", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.0], + ), + ), + ], + ) + + + # Configure 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 parameters for status checking + self.gripper_joint_names = ["left_hand_joint_left", "left_hand_joint_right"] + self.gripper_open_val = 0.0 + self.gripper_threshold = 0.005 + + +@configclass +class MindRobotLeftArmIKAbsEnvCfg(MindRobotLeftArmIKEnvCfg): + """MindRobot 左臂 IK-Abs 遥操作环境配置（绝对坐标系，适合 VR 控制器）。""" + + def __post_init__(self): + super().__post_init__() + # Switch controller to absolute mode + self.actions.arm_action.controller.use_relative_mode = False