From 7be48b3964f59084836571c9a95eb9e9c555f69b Mon Sep 17 00:00:00 2001 From: yt lee Date: Sat, 14 Mar 2026 13:31:26 +0800 Subject: [PATCH] dual arm XR teleoperation: env cfg, shared client, joint locking - Add MindRobotDualArmIKAbsEnvCfg: standalone dual-arm env inheriting ManagerBasedRLEnvCfg directly (no single-arm dependency), 20D action space (left_arm7 | wheel4 | left_gripper1 | right_arm7 | right_gripper1) - Add local mdp/ with parameterized gripper_pos(joint_names) to support independent left/right gripper observations without modifying IsaacLab - Update teleop_xr_agent.py for dual-arm mode: shared XrClient to avoid SDK double-init crash, root-frame IK command caching so arm joints are locked when grip not pressed (EEF world pos still tracks chassis) - Tune mindrobot_cfg.py initial poses with singularity-avoiding offsets - Add CLAUDE.md project instructions and debug_action_assembly.py Co-Authored-By: Claude Sonnet 4.6 --- CLAUDE.md | 130 +++++ scripts/debug_action_assembly.py | 113 ++++ .../teleoperation/teleop_xr_agent.py | 501 ++++++++++-------- .../manager_based/il/open_drybox/__init__.py | 10 + .../il/open_drybox/mdp/__init__.py | 9 + .../il/open_drybox/mdp/observations.py | 48 ++ .../il/open_drybox/mindrobot_cfg.py | 56 +- .../mindrobot_dual_arm_ik_env_cfg.py | 343 ++++++++++++ .../mindrobot_left_arm_ik_env_cfg.py | 40 +- source/mindbot/mindbot/utils/assets.py | 4 +- 10 files changed, 986 insertions(+), 268 deletions(-) create mode 100644 CLAUDE.md create mode 100644 scripts/debug_action_assembly.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mdp/__init__.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mdp/observations.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_dual_arm_ik_env_cfg.py diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..483a62e --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,130 @@ +# MindBot Project + +基于 Isaac Lab 的移动操作机器人（MindRobot）仿真学习框架，支持强化学习（RL）和模仿学习（IL）。 + +## 环境要求 + +- **Isaac Sim**: 5.1.0 +- **Python**: 3.11 +- **依赖**: isaaclab, isaaclab_assets, isaaclab_mimic, isaaclab_rl, isaaclab_tasks +- **Conda 环境**： + - `env_isaaclab` — 主开发环境 + - `xr` — 另一个同样包含 isaaclab/isaacsim 的环境，两者均可运行 Isaac Lab 脚本 + +**IMPORTANT**: 所有 Isaac Lab 脚本必须通过 `~/IsaacLab/isaaclab.sh -p` 启动，直接用 `python` 会报 `KeyError: 'EXP_PATH'`（该变量只有 `isaaclab.sh` 会在启动时设置）。 + +```bash +# 安装包 +~/IsaacLab/isaaclab.sh -p -m pip install -e source/mindbot +``` + +## 关键环境变量 + +```bash +export MINDBOT_ASSETS_DIR=/home/tangger/LYT/maic_usd_assets_moudle # USD 资源路径（默认值） +``` + +## 常用命令 + +```bash +# 列出所有注册环境 +python scripts/list_envs.py + +# 测试环境（零动作 / 随机动作） +python scripts/zero_agent.py --task Template-Mindbot-v0 --num_envs 4 +python scripts/random_agent.py --task Template-Mindbot-v0 --num_envs 4 + +# RL 训练（RSL-RL PPO） +python scripts/rsl_rl/train.py --task Template-Mindbot-v0 --num_envs 4096 + +# RL 推理 +python scripts/rsl_rl/play.py --task Template-Mindbot-v0 + +# 遥操作（Spacemouse / 键盘） +python scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-MindRobot-LeftArm-IK-Rel-v0 + +# 遥操作（XR 设备） +python scripts/environments/teleoperation/teleop_xr_agent.py --task Isaac-MindRobot-LeftArm-IK-Rel-v0 + +# 代码格式化 +pre-commit run --all-files +``` + +## 项目架构 + +``` +source/mindbot/mindbot/ +├── robot/mindbot.py # MindRobot 关节/执行器配置（MINDBOT_CFG） +├── utils/assets.py # USD 资源路径（MINDBOT_ASSETS_DIR） +└── tasks/manager_based/ + ├── rl/ # 强化学习任务 + │ ├── mindbot/ # Template-Mindbot-v0（抓取） + │ ├── centrifuge/ # Template-centrifuge-lidup-v0 + │ ├── pull/ # Template-Pull-v0 + │ └── pullUltrasoundLidUp/ # Pull-Ultrasound-Lid-Up-v0 + └── il/ # 模仿学习任务 + ├── demo/ # Demo 任务（H1、Franka） + └── open_drybox/ # Isaac-MindRobot-LeftArm-IK-{Rel,Abs}-v0 +``` + +## 注册任务列表 + +| 任务 ID | 类型 | 说明 | +|---|---|---| +| `Template-Mindbot-v0` | RL | MindRobot 抓取 | +| `Template-centrifuge-lidup-v0` | RL | 离心机旋转 | +| `Template-Pull-v0` | RL | 物体拉取 | +| `Pull-Ultrasound-Lid-Up-v0` | RL | 超声机盖拉取 | +| `Isaac-MindRobot-LeftArm-IK-Rel-v0` | IL | 左臂 IK 相对控制 | +| `Isaac-MindRobot-LeftArm-IK-Abs-v0` | IL | 左臂 IK 绝对控制 | + +## 新增任务的规范 + +### RL 任务结构 +``` +tasks/manager_based/rl// +├── __init__.py # gym.register(id="-v0", ...) +├── _env_cfg.py # ManagerBasedRLEnvCfg 子类 +├── agents/ +│ └── rsl_rl_ppo_cfg.py # RslRlOnPolicyRunnerCfg 子类 +└── mdp/ + ├── rewards.py + ├── terminations.py + └── __init__.py +``` + +### IL 任务结构 +``` +tasks/manager_based/il// +├── __init__.py # gym.register with robomimic_bc_cfg_entry_point +├── _env_cfg.py # ManagerBasedRLEnvCfg 子类（使用 DifferentialIKController） +└── agents/robomimic/ + └── bc_rnn_low_dim.json # RoboMimic BC 配置 +``` + +新任务的 `__init__.py` 必须在 `mindbot/tasks/manager_based/__init__.py` 中 import 才能注册到 Gym。 + +## 代码风格 + +- 文件头使用 SPDX 许可证注释：`# SPDX-FileCopyrightText: Copyright (c) 2022-2025, The Isaac Lab Project Developers.` +- 关节角度配置用弧度，注释中可标注对应度数 +- 执行器刚度/阻尼参数调整需在注释中说明物理含义 + +## MindRobot 关节组 + +| 关节组 | 关节名 | +|---|---| +| 左臂（6 自由度） | l_joint1 ~ l_joint6 | +| 右臂（6 自由度） | r_joint1 ~ r_joint6 | +| 左夹爪 | left_hand_joint_left/right | +| 右夹爪 | right_hand_joint_left/right | +| 躯干升降 | PrismaticJoint | +| 头部 | head_revoluteJoint | +| 底盘轮子 | front/back revolute joints | + +## 遥操作数据收集 + +- Isaac Lab 内遥操作脚本使用 `env_isaaclab` 环境 +- XR 设备接入依赖 `deps/XRoboToolkit-Teleop-Sample-Python/`，需切换到 `xr` 环境 +- 示教数据格式兼容 LeRobot / RoboMimic +- 多相机配置：cam_head, cam_chest, cam_left_hand, cam_right_hand, cam_top, cam_side diff --git a/scripts/debug_action_assembly.py b/scripts/debug_action_assembly.py new file mode 100644 index 0000000..981b720 --- /dev/null +++ b/scripts/debug_action_assembly.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +""" +验证 teleop_xr_agent.py 中相对模式下动作拼接顺序的正确性。 +无需 Isaac Sim / XR 设备，纯 CPU 运行。 + +运行方式： + python scripts/debug_action_assembly.py +""" + +import torch + +# ============================================================ +# 模拟 Action Manager 的切分逻辑 +# ============================================================ +# 来自 mindrobot_left_arm_ik_env_cfg.py 的 Action 配置： +# arm_action : DifferentialInverseKinematicsActionCfg，相对模式 6D，绝对模式 7D +# wheel_action: JointVelocityActionCfg，4 个轮子，4D +# gripper_action: BinaryJointPositionActionCfg，1D + +def action_manager_split(action: torch.Tensor, arm_size: int): + """模拟 Isaac Lab ManagerBasedRLEnv 的动作切分。""" + assert action.shape[-1] == arm_size + 4 + 1, \ + f"期望总维度 {arm_size + 4 + 1}，实际 {action.shape[-1]}" + arm = action[..., :arm_size] + wheel = action[..., arm_size:arm_size + 4] + gripper = action[..., arm_size + 4:] + return arm, wheel, gripper + + +# ============================================================ +# 复现 teleop_xr_agent.py:474 的拼接逻辑 +# ============================================================ + +def current_assembly(action_np: torch.Tensor, wheel_np: torch.Tensor) -> torch.Tensor: + """现有代码的拼接逻辑（逐字复制自 teleop_xr_agent.py:474）。""" + return torch.cat([action_np[:7], wheel_np, action_np[7:]]) + + +def fixed_assembly(action_np: torch.Tensor, wheel_np: torch.Tensor, is_abs_mode: bool) -> torch.Tensor: + """修正后的拼接逻辑。""" + arm_size = 7 if is_abs_mode else 6 + arm_action = action_np[:arm_size] + gripper_val = action_np[arm_size:] # 最后一个元素 + return torch.cat([arm_action, wheel_np, gripper_val]) + + +# ============================================================ +# 测试 +# ============================================================ + +def run_test(mode: str): + is_abs = (mode == "absolute") + arm_size = 7 if is_abs else 6 + + # 构造可辨识的测试值 + if is_abs: + # 绝对模式：[x,y,z,qw,qx,qy,qz, gripper] + fake_action = torch.tensor([0.1, 0.2, 0.3, # pos + 1.0, 0.0, 0.0, 0.0, # quat + 0.9]) # gripper (trigger=0.9) + else: + # 相对模式：[dx,dy,dz, drx,dry,drz, gripper] + fake_action = torch.tensor([0.01, 0.02, 0.03, # delta pos + 0.05, 0.06, 0.07, # delta rot + 0.9]) # gripper (trigger=0.9) + + fake_wheel = torch.tensor([1.5, 1.5, 1.5, 1.5]) # 向前行驶 + + print(f"\n{'='*60}") + print(f" 测试模式: {mode.upper()}") + print(f"{'='*60}") + print(f" 原始 action_np : {fake_action.tolist()}") + print(f" 原始 wheel_np : {fake_wheel.tolist()}") + print(f" 期望 gripper 值 : {fake_action[-1].item()} (应为 0.9)") + print(f" 期望 wheel 值 : {fake_wheel.tolist()} (应为 [1.5,1.5,1.5,1.5])") + + # ---- 现有代码 ---- + assembled_current = current_assembly(fake_action, fake_wheel) + try: + arm_c, wheel_c, grip_c = action_manager_split(assembled_current, arm_size) + print(f"\n [现有代码]") + print(f" 拼接结果({assembled_current.shape[0]}D) : {assembled_current.tolist()}") + print(f" → arm ({arm_c.shape[0]}D) : {arm_c.tolist()}") + print(f" → wheel ({wheel_c.shape[0]}D) : {wheel_c.tolist()}") + print(f" → gripper ({grip_c.shape[0]}D) : {grip_c.tolist()}") + arm_ok = True + wheel_ok = torch.allclose(wheel_c, fake_wheel) + grip_ok = torch.allclose(grip_c, fake_action[-1:]) + print(f" arm 正确? {'✅' if arm_ok else '❌'} | wheel 正确? {'✅' if wheel_ok else '❌ ← BUG'} | gripper 正确? {'✅' if grip_ok else '❌ ← BUG'}") + except AssertionError as e: + print(f" [现有代码] 维度断言失败: {e}") + + # ---- 修正后代码 ---- + assembled_fixed = fixed_assembly(fake_action, fake_wheel, is_abs) + try: + arm_f, wheel_f, grip_f = action_manager_split(assembled_fixed, arm_size) + print(f"\n [修正后代码]") + print(f" 拼接结果({assembled_fixed.shape[0]}D) : {assembled_fixed.tolist()}") + print(f" → arm ({arm_f.shape[0]}D) : {arm_f.tolist()}") + print(f" → wheel ({wheel_f.shape[0]}D) : {wheel_f.tolist()}") + print(f" → gripper ({grip_f.shape[0]}D) : {grip_f.tolist()}") + arm_ok = True + wheel_ok = torch.allclose(wheel_f, fake_wheel) + grip_ok = torch.allclose(grip_f, fake_action[-1:]) + print(f" arm 正确? {'✅' if arm_ok else '❌'} | wheel 正确? {'✅' if wheel_ok else '❌'} | gripper 正确? {'✅' if grip_ok else '❌'}") + except AssertionError as e: + print(f" [修正后代码] 维度断言失败: {e}") + + +if __name__ == "__main__": + run_test("relative") + run_test("absolute") + print() diff --git a/scripts/environments/teleoperation/teleop_xr_agent.py b/scripts/environments/teleoperation/teleop_xr_agent.py index deaede2..870fd01 100644 --- a/scripts/environments/teleoperation/teleop_xr_agent.py +++ b/scripts/environments/teleoperation/teleop_xr_agent.py @@ -6,7 +6,11 @@ """ Script to run teleoperation with Isaac Lab manipulation environments using PICO XR Controllers. -This script uses XRoboToolkit to fetch XR controller poses and maps them to differential IK actions. + +Only absolute IK mode is supported (Isaac-MindRobot-LeftArm-IK-Abs-v0). +The controller computes delta pose in Isaac Sim world frame and accumulates +an absolute EEF target, which is then converted to robot root frame before +being sent to the DifferentialIK action manager. """ import argparse @@ -24,44 +28,30 @@ logger = logging.getLogger(__name__) # add argparse arguments parser = argparse.ArgumentParser( - description="Teleoperation for Isaac Lab environments with PICO XR Controller." -) -parser.add_argument( - "--num_envs", type=int, default=1, help="Number of environments to simulate." + description="Teleoperation for Isaac Lab environments with PICO XR Controller (absolute IK mode only)." ) +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") parser.add_argument( "--task", type=str, - default="Isaac-MindRobot-LeftArm-IK-Rel-v0", - help="Name of the task.", + default="Isaac-MindRobot-LeftArm-IK-Abs-v0", + help="Name of the task (must be an Abs IK task).", ) parser.add_argument( - "--sensitivity", type=float, default=5.0, help="Sensitivity factor for pos/rot." + "--sensitivity", type=float, default=None, + help="Set both pos and rot sensitivity (overridden by --pos_sensitivity/--rot_sensitivity).", ) -parser.add_argument( - "--arm", - type=str, - default="left", - 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 +parser.add_argument("--pos_sensitivity", type=float, default=None, help="Position sensitivity. Default: 1.0.") +parser.add_argument("--rot_sensitivity", type=float, default=None, help="Rotation sensitivity. Default: 0.3.") +parser.add_argument("--arm", type=str, default="left", 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.") + AppLauncher.add_app_launcher_args(parser) args_cli = parser.parse_args() app_launcher_args = vars(args_cli) - -# Disable some rendering settings to speed up app_launcher_args["xr"] = False -# launch omniverse app app_launcher = AppLauncher(app_launcher_args) simulation_app = app_launcher.app @@ -83,7 +73,7 @@ from xr_utils.geometry import R_HEADSET_TO_WORLD # ===================================================================== -# Teleoperation Interface for XR +# Helpers # ===================================================================== def _quat_wxyz_to_rotation(quat_wxyz: np.ndarray) -> R: @@ -92,9 +82,9 @@ def _quat_wxyz_to_rotation(quat_wxyz: np.ndarray) -> R: 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]]) + """Convert scipy Rotation to Isaac-style wxyz quaternion.""" + q = rot.as_quat() + return np.array([q[3], q[0], q[1], q[2]]) def world_pose_to_root_frame( @@ -111,30 +101,39 @@ def world_pose_to_root_frame( return pos_root, quat_root -class XrTeleopController: - """Teleop controller for PICO XR headset.""" +# ===================================================================== +# Teleoperation Controller +# ===================================================================== - def __init__(self, arm="left", pos_sensitivity=1.0, rot_sensitivity=0.3, is_absolute=False): - self.xr_client = XrClient() +class XrTeleopController: + """Teleop controller for PICO XR headset (absolute IK mode). + + Accumulates an absolute EEF target in Isaac Sim world frame. + Grip button acts as clutch; B/Y buttons trigger environment reset. + """ + + def __init__(self, arm="left", pos_sensitivity=1.0, rot_sensitivity=0.3, + max_pos_per_step=0.05, max_rot_per_step=0.08, xr_client=None): + self.xr_client = xr_client if xr_client is not None else XrClient() + self._owns_client = xr_client is None # only close if we created it self.pos_sensitivity = pos_sensitivity self.rot_sensitivity = rot_sensitivity + # Hard caps per physics step to prevent IK divergence. + # max_rot_per_step ~4.6°, max_pos_per_step 5 cm. + self.max_pos_per_step = max_pos_per_step + self.max_rot_per_step = max_rot_per_step self.arm = arm - self.is_absolute = is_absolute - + self.controller_name = "left_controller" if arm == "left" else "right_controller" self.grip_name = "left_grip" if arm == "left" else "right_grip" self.trigger_name = "left_trigger" if arm == "left" else "right_trigger" - # Coordinate transform matrix self.R_headset_world = R_HEADSET_TO_WORLD - # Raw XR tracking space poses self.prev_xr_pos = None self.prev_xr_quat = None - - # Absolute target states - self.target_eef_pos = None - self.target_eef_quat = None # wxyz + self.target_eef_pos = None # world frame + self.target_eef_quat = None # world frame, wxyz self.grip_active = False self.frame_count = 0 @@ -142,8 +141,6 @@ class XrTeleopController: self.require_grip_reengage = False self.grip_engage_threshold = 0.8 self.grip_release_threshold = 0.2 - - # Callbacks (like reset, etc) self.callbacks = {} def add_callback(self, name: str, func: Callable): @@ -156,40 +153,35 @@ class XrTeleopController: 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. + # Require grip release after reset to avoid driving to stale pose. self.require_grip_reengage = True def close(self): - self.xr_client.close() + if self._owns_client: + self.xr_client.close() - def get_zero_action(self, trigger, current_eef_pos=None, current_eef_quat=None): - if self.is_absolute: - action = torch.zeros(8) - # Stay at the current valid pose, or fallback to the cached target - if current_eef_pos is not None and current_eef_quat is not None: - action[:3] = torch.tensor(current_eef_pos.copy()) - action[3:7] = torch.tensor(current_eef_quat.copy()) - elif self.target_eef_pos is not None and self.target_eef_quat is not None: - action[:3] = torch.tensor(self.target_eef_pos.copy()) - action[3:7] = torch.tensor(self.target_eef_quat.copy()) - else: - action[3] = 1.0 # default w=1 for quat - action[7] = 1.0 if trigger > 0.5 else -1.0 + def get_zero_action(self, trigger, current_eef_pos=None, current_eef_quat=None) -> torch.Tensor: + """Return 8D hold action [x, y, z, qw, qx, qy, qz, gripper] at frozen target pose.""" + action = torch.zeros(8) + if self.target_eef_pos is not None and self.target_eef_quat is not None: + # Prefer frozen world-frame target so IK holds a fixed world position + # even if the robot chassis drifts slightly under gravity. + action[:3] = torch.tensor(self.target_eef_pos.copy()) + action[3:7] = torch.tensor(self.target_eef_quat.copy()) + elif current_eef_pos is not None and current_eef_quat is not None: + action[:3] = torch.tensor(current_eef_pos.copy()) + action[3:7] = torch.tensor(current_eef_quat.copy()) else: - action = torch.zeros(7) - action[6] = 1.0 if trigger > 0.5 else -1.0 + action[3] = 1.0 # identity quaternion + action[7] = 1.0 if trigger > 0.5 else -1.0 return action def advance(self, current_eef_pos=None, current_eef_quat=None) -> torch.Tensor: + """Read XR controller and return 8D absolute action [x, y, z, qw, qx, qy, qz, gripper]. + + Position and quaternion are in Isaac Sim world frame. + Caller must convert to robot root frame before sending to IK. """ - 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: 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: reset_pressed = self.xr_client.get_button("B") or self.xr_client.get_button("Y") if reset_pressed and not self.reset_button_latched: @@ -203,141 +195,120 @@ class XrTeleopController: raw_pose = self.xr_client.get_pose(self.controller_name) grip = self.xr_client.get_key_value(self.grip_name) trigger = self.xr_client.get_key_value(self.trigger_name) - except Exception as e: + except Exception: return self.get_zero_action(0.0, current_eef_pos, current_eef_quat) - # Skip transformation if quaternion is invalid if not is_valid_quaternion(raw_pose[3:]): return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) + # Wait for grip release after reset 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: + if 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) + # Grip button as clutch with hysteresis + grip_pressed = grip > self.grip_release_threshold if self.grip_active else grip >= self.grip_engage_threshold + if not grip_pressed: self.prev_xr_pos = None self.prev_xr_quat = None + # Only snapshot target on the transition frame (grip just released) or first ever frame. + # After that, keep it frozen so IK holds a fixed world-frame position. + if self.grip_active or self.target_eef_pos is None: + if 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() self.grip_active = False - # Ensure target tracks the current pose while we are not grabbing - 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() # wxyz return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) if not self.grip_active: self.prev_xr_pos = raw_pose[:3].copy() self.prev_xr_quat = raw_pose[3:].copy() - if self.is_absolute and current_eef_pos is not None and current_eef_quat is not None: + if 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() # wxyz + self.target_eef_quat = current_eef_quat.copy() self.grip_active = True return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) - # Since OpenXR headset zero is not guaranteed to match robot zero, we compute the - # raw transformation in World Frame, but apply it relatively to the stored target. - - # 1. First, find current XR pose projected into World frame + # Project current and previous XR poses into Isaac Sim world frame xr_world_pos, xr_world_quat_xyzw = transform_xr_pose(raw_pose[:3], raw_pose[3:]) prev_xr_world_pos, prev_xr_world_quat_xyzw = transform_xr_pose(self.prev_xr_pos, self.prev_xr_quat) - # 2. Extract Delta POS in World frame + # Delta position (world frame), clamped per step world_delta_pos = (xr_world_pos - prev_xr_world_pos) * self.pos_sensitivity + pos_norm = np.linalg.norm(world_delta_pos) + if pos_norm > self.max_pos_per_step: + world_delta_pos *= self.max_pos_per_step / pos_norm - # 3. Extract Delta ROT in World frame + # Delta rotation (world frame), clamped per step 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(world_delta_rot) + if rot_norm > self.max_rot_per_step: + world_delta_rot *= self.max_rot_per_step / rot_norm - # 4. Gripper gripper_action = 1.0 if trigger > 0.5 else -1.0 - if self.is_absolute: - if self.target_eef_pos is None: - self.target_eef_pos = np.zeros(3) - self.target_eef_quat = np.array([1.0, 0.0, 0.0, 0.0]) + # Accumulate absolute target + if self.target_eef_pos is None: + self.target_eef_pos = np.zeros(3) + self.target_eef_quat = np.array([1.0, 0.0, 0.0, 0.0]) - # Accumulate in world frame so VR direction always matches sim direction. - self.target_eef_pos += world_delta_pos + self.target_eef_pos += world_delta_pos - target_r = _quat_wxyz_to_rotation(self.target_eef_quat) - delta_r = R.from_rotvec(world_delta_rot) - 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], - self.target_eef_quat[0], self.target_eef_quat[1], self.target_eef_quat[2], self.target_eef_quat[3], - gripper_action], dtype=torch.float32) - - self.prev_xr_pos = raw_pose[:3].copy() - self.prev_xr_quat = raw_pose[3:].copy() - - else: - 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.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], - world_delta_rot[0], world_delta_rot[1], world_delta_rot[2], - gripper_action], dtype=torch.float32) - - self.prev_xr_pos = raw_pose[:3].copy() - self.prev_xr_quat = raw_pose[3:].copy() + # Apply rotation delta in world frame so controller direction = EEF world direction + target_r = _quat_wxyz_to_rotation(self.target_eef_quat) + self.target_eef_quat = _rotation_to_quat_wxyz(R.from_rotvec(world_delta_rot) * target_r) + self.prev_xr_pos = raw_pose[:3].copy() + self.prev_xr_quat = raw_pose[3:].copy() self.frame_count += 1 + action = torch.tensor([ + *self.target_eef_pos, + *self.target_eef_quat, + gripper_action, + ], dtype=torch.float32) + if self.frame_count % 30 == 0: np.set_printoptions(precision=4, suppress=True, floatmode='fixed') print("\n====================== [VR TELEOP DEBUG] ======================") - 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={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 (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()}") + print(f"| Targ Pos (world): {action[:3].numpy()}") + print(f"| Targ Quat (world, wxyz): {action[3:7].numpy()}") print(f"| Gripper & Trigger: Grip={grip:.2f}, Trig={trigger:.2f}") print("===============================================================") return action + # ===================================================================== # Main Execution Loop # ===================================================================== def main() -> None: """Run teleoperation with PICO XR Controller against Isaac Lab environment.""" - - # 1. Configuration parsing + env_cfg = parse_env_cfg(args_cli.task, num_envs=args_cli.num_envs) env_cfg.env_name = args_cli.task if not isinstance(env_cfg, ManagerBasedRLEnvCfg): raise ValueError(f"Teleoperation requires ManagerBasedRLEnvCfg. Got: {type(env_cfg)}") + if "Abs" not in args_cli.task: + raise ValueError( + f"Task '{args_cli.task}' is not an absolute IK task. " + "Only Abs tasks are supported (e.g. Isaac-MindRobot-LeftArm-IK-Abs-v0)." + ) env_cfg.terminations.time_out = None - # 2. Environment creation try: env = gym.make(args_cli.task, cfg=env_cfg).unwrapped except Exception as e: @@ -345,19 +316,41 @@ def main() -> None: simulation_app.close() return - # 3. Teleoperation Interface Initialization - is_abs_mode = "Abs" in args_cli.task - - print(f"\n[INFO] Connecting to PICO XR Headset using {args_cli.arm} controller...") - print(f"[INFO] Using IK Mode: {'ABSOLUTE' if is_abs_mode else 'RELATIVE'}") - teleop_interface = XrTeleopController( - arm=args_cli.arm, - pos_sensitivity=args_cli.sensitivity, - rot_sensitivity=args_cli.sensitivity * (1.0 if is_abs_mode else 0.3), # Absolute rotation handles 1:1 better than relative - is_absolute=is_abs_mode - ) - + # Detect single-arm vs dual-arm based on action manager term names + is_dual_arm = "left_arm_action" in env.action_manager._terms + + # Sensitivity: explicit args override --sensitivity, which overrides defaults + pos_sens = 1.0 + rot_sens = 0.3 + if args_cli.sensitivity is not None: + pos_sens = args_cli.sensitivity + rot_sens = args_cli.sensitivity + if args_cli.pos_sensitivity is not None: + pos_sens = args_cli.pos_sensitivity + if args_cli.rot_sensitivity is not None: + rot_sens = args_cli.rot_sensitivity + + if is_dual_arm: + print(f"\n[INFO] Dual-arm mode detected. Using both controllers.") + print(f"[INFO] IK Mode: ABSOLUTE") + print(f"[INFO] Sensitivity: pos={pos_sens:.3f} rot={rot_sens:.3f}") + shared_client = XrClient() + teleop_left = XrTeleopController(arm="left", pos_sensitivity=pos_sens, rot_sensitivity=rot_sens, xr_client=shared_client) + teleop_right = XrTeleopController(arm="right", pos_sensitivity=pos_sens, rot_sensitivity=rot_sens, xr_client=shared_client) + teleop_interface = teleop_left # primary interface for callbacks/reset + teleop_right_ref = teleop_right + else: + print(f"\n[INFO] Connecting to PICO XR Headset using {args_cli.arm} controller...") + print(f"[INFO] IK Mode: ABSOLUTE") + print(f"[INFO] Sensitivity: pos={pos_sens:.3f} rot={rot_sens:.3f}") + teleop_interface = XrTeleopController( + arm=args_cli.arm, + pos_sensitivity=pos_sens, + rot_sensitivity=rot_sens, + ) + should_reset = False + def request_reset(): nonlocal should_reset should_reset = True @@ -365,30 +358,34 @@ 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 + """Sync IK controller's internal target to current EEF pose to avoid jumps after reset.""" + if is_dual_arm: + for term_key in ["left_arm_action", "right_arm_action"]: + arm_action_term = env.action_manager._terms[term_key] + 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 + else: + arm_action_term = env.action_manager._terms["arm_action"] + 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.""" + """Convert 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() + root_quat_w = robot.data.root_quat_w[0].detach().cpu().numpy() pos_root, quat_root = world_pose_to_root_frame( - target_pos_w, target_quat_w, root_pos_w, root_quat_w, + action_tensor[:3].numpy(), action_tensor[3:7].numpy(), + root_pos_w, root_quat_w, ) out = action_tensor.clone() out[:3] = torch.tensor(pos_root, dtype=torch.float32) @@ -396,53 +393,43 @@ def main() -> None: 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. - """ + """Read left joystick → 4-DOF wheel velocity [right_b, left_b, left_f, right_f].""" try: joy = teleop_interface.xr_client.get_joystick("left") - jy = float(joy[1]) # forward / backward - jx = float(joy[0]) # right / left + jy = float(joy[1]) + jx = float(joy[0]) 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) - return torch.tensor( - [right_vel, left_vel, left_vel, right_vel], dtype=torch.float32 - ) - - env.reset() + obs, _ = env.reset() clear_ik_target_state() teleop_interface.reset() + if is_dual_arm: + teleop_right_ref.reset() print("\n" + "=" * 50) - print(" 🚀 Teleoperation Started!") - print(" 🎮 Use the TRIGGER to open/close gripper.") - 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(" Teleoperation Started!") + if is_dual_arm: + print(" LEFT controller → left arm") + print(" RIGHT controller → right arm") + print(" TRIGGER: open/close gripper") + print(" GRIP (hold): move the arm") + print(" Left joystick: Y=forward/back, X=turn") + print(" B / Y: reset environment") print("=" * 50 + "\n") device = env.unwrapped.device sim_frame = 0 obs, _ = env.reset() clear_ik_target_state() - + last_root_left = None + last_root_right = None + while simulation_app.is_running(): try: with torch.inference_mode(): @@ -450,66 +437,78 @@ def main() -> None: obs, _ = env.reset() clear_ik_target_state() teleop_interface.reset() + if is_dual_arm: + teleop_right_ref.reset() should_reset = False sim_frame = 0 + last_root_left = None + last_root_right = None 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 (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:]]) + + 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() + eef_pos_right = policy_obs["eef_pos_right"][0].cpu().numpy() + eef_quat_right = policy_obs["eef_quat_right"][0].cpu().numpy() + + left_action = teleop_left.advance(current_eef_pos=eef_pos_left, current_eef_quat=eef_quat_left) + right_action = teleop_right_ref.advance(current_eef_pos=eef_pos_right, current_eef_quat=eef_quat_right) + + # Only recompute root-frame IK target when grip is active; otherwise freeze joints + if teleop_left.grip_active or last_root_left is None: + last_root_left = convert_action_world_to_root(left_action)[:7].clone() + 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_np = torch.cat([ + last_root_left, wheel_np, left_action[7:8], + last_root_right, right_action[7:8], + ]) + else: + eef_pos = policy_obs["eef_pos"][0].cpu().numpy() + eef_quat = policy_obs["eef_quat"][0].cpu().numpy() + + action_np = teleop_interface.advance(current_eef_pos=eef_pos, current_eef_quat=eef_quat) + + if teleop_interface.grip_active or last_root_left is None: + last_root_left = convert_action_world_to_root(action_np)[:7].clone() + + # Action layout: arm(7) | wheel(4) | gripper(1) + action_np = torch.cat([last_root_left, wheel_np, action_np[7:8]]) actions = action_np.unsqueeze(0).repeat(env.num_envs, 1).to(device) - - # Step environment obs, _, _, _, _ = env.step(actions) - # Print robot state every 30 frames sim_frame += 1 if sim_frame % 30 == 0: np.set_printoptions(precision=4, suppress=True, floatmode='fixed') policy_obs = obs["policy"] joint_pos = policy_obs["joint_pos"][0].cpu().numpy() - eef_pos = policy_obs["eef_pos"][0].cpu().numpy() - eef_quat = policy_obs["eef_quat"][0].cpu().numpy() last_act = policy_obs["actions"][0].cpu().numpy() - # On first print, dump ALL joint names + positions to identify indices if sim_frame == 30: robot = env.unwrapped.scene["robot"] jnames = robot.joint_names print(f"\n{'='*70}") - print(f" ALL {len(jnames)} JOINT NAMES AND POSITIONS (relative)") + print(f" ALL {len(jnames)} JOINT NAMES AND POSITIONS") print(f"{'='*70}") for i, name in enumerate(jnames): 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 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 n.startswith("l_joint")] - arm_idx = env._arm_idx_cache - arm_joints = joint_pos[arm_idx] + env._right_arm_idx_cache = [i for i, n in enumerate(jnames) if n.startswith("r_joint")] + arm_joints = joint_pos[env._arm_idx_cache] + right_arm_joints = joint_pos[env._right_arm_idx_cache] try: joy_dbg = teleop_interface.xr_client.get_joystick("left") @@ -517,21 +516,59 @@ def main() -> None: 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 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() + 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"| 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)") + print(f"| Cmd right_arm(abs): {last_act[12:19]}") + print(f"| Cmd right_gripper: {last_act[19:]} (+1=open -1=close)") + print(f"| Joystick (x=turn,y=fwd): {joy_str}") + print(f"----------------------------------------------------------------") + else: + eef_pos = policy_obs["eef_pos"][0].cpu().numpy() + eef_quat = policy_obs["eef_quat"][0].cpu().numpy() + + if not hasattr(env, '_prev_eef_quat'): + env._prev_eef_quat = eef_quat.copy() + prev_q = env._prev_eef_quat + r_prev = R.from_quat([prev_q[1], prev_q[2], prev_q[3], prev_q[0]]) + r_curr = R.from_quat([eef_quat[1], eef_quat[2], eef_quat[3], eef_quat[0]]) + delta_rotvec = (r_curr * r_prev.inv()).as_rotvec() + delta_angle_deg = np.degrees(np.linalg.norm(delta_rotvec)) + delta_axis = delta_rotvec / (np.linalg.norm(delta_rotvec) + 1e-9) + env._prev_eef_quat = eef_quat.copy() + approach_vec = r_curr.apply(np.array([1.0, 0.0, 0.0])) + + print(f"\n---------------- [ROBOT STATE frame={sim_frame}] ----------------") + print(f"| Left Arm Joints (rad): {arm_joints}") + print(f"| EEF Pos (world, m): {eef_pos}") + print(f"| EEF approach vec (world): [{approach_vec[0]:+.3f}, {approach_vec[1]:+.3f}, {approach_vec[2]:+.3f}] (夹爪朝向)") + print(f"| EEF rot since last print: {delta_angle_deg:+.2f}° around [{delta_axis[0]:+.3f},{delta_axis[1]:+.3f},{delta_axis[2]:+.3f}] (world)") + print(f"| Cmd arm(abs pos+quat): {last_act[:7]}") + print(f"| Cmd wheel vel (rad/s): {last_act[7:11]}") + print(f"| Cmd gripper: {last_act[11:]} (+1=open -1=close)") + print(f"| Joystick (x=turn,y=fwd): {joy_str}") + print(f"----------------------------------------------------------------") except Exception as e: logger.error(f"Error during simulation step: {e}") break teleop_interface.close() + if is_dual_arm: + teleop_right_ref.close() + shared_client.close() env.close() + if __name__ == "__main__": main() simulation_app.close() 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 2d2a6f9..e91070b 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 @@ -46,4 +46,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-DualArm-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.mindrobot_dual_arm_ik_env_cfg:MindRobotDualArmIKAbsEnvCfg", + "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/mdp/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mdp/__init__.py new file mode 100644 index 0000000..be7ca22 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mdp/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2024, MindRobot Project +# SPDX-License-Identifier: BSD-3-Clause + +"""MDP functions specific to the open_drybox teleoperation environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 +from isaaclab_tasks.manager_based.manipulation.stack.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mdp/observations.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mdp/observations.py new file mode 100644 index 0000000..e5f46b1 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mdp/observations.py @@ -0,0 +1,48 @@ +# Copyright (c) 2024, MindRobot Project +# SPDX-License-Identifier: BSD-3-Clause + +"""Custom observation functions for open_drybox teleoperation environments.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def gripper_pos( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + joint_names: list[str] = None, +) -> torch.Tensor: + """Return the opening state of a parallel gripper specified by joint names. + + Unlike the built-in ``gripper_pos`` which reads from ``env.cfg.gripper_joint_names``, + this version accepts ``joint_names`` directly so the same function can be used + for both left and right grippers in a dual-arm setup. + + Args: + robot_cfg: Scene entity config for the robot articulation. + joint_names: List of exactly 2 joint names ``[joint_open_positive, joint_open_negative]``. + The first joint increases with opening, the second decreases. + + Returns: + Tensor of shape ``(num_envs, 2)`` with the joint positions (sign-corrected). + """ + if joint_names is None: + raise ValueError("joint_names must be specified") + + robot = env.scene[robot_cfg.name] + ids, _ = robot.find_joints(joint_names) + + if len(ids) != 2: + raise ValueError(f"Expected exactly 2 gripper joints, got {len(ids)} for {joint_names}") + + j1 = robot.data.joint_pos[:, ids[0]].clone().unsqueeze(1) + j2 = -1.0 * robot.data.joint_pos[:, ids[1]].clone().unsqueeze(1) + return torch.cat((j1, j2), dim=1) 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 398efe7..74f7899 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 @@ -36,20 +36,20 @@ MINDBOT_CFG = ArticulationCfg( 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, + # 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, @@ -139,3 +139,31 @@ 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", +] + +# 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_dual_arm_ik_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_dual_arm_ik_env_cfg.py new file mode 100644 index 0000000..d41eae3 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_dual_arm_ik_env_cfg.py @@ -0,0 +1,343 @@ +# 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, + 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.utils import configclass + +from . import mdp + +from .mindrobot_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_LEFT_ARM_PRIM_ROOT, + MINDBOT_RIGHT_ARM_PRIM_ROOT, + MINDBOT_LEFT_EEF_PRIM, + MINDBOT_RIGHT_EEF_PRIM, +) + + +# ===================================================================== +# 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(), + ) + + 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)), + ) + + 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 20D): + 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 + """ + + 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}, + ) + + +# ===================================================================== +# 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 MindRobotDualArmIKAbsEnvCfg(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.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 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 fc809b5..b3e2f88 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 @@ -37,7 +37,16 @@ from isaaclab.devices.openxr import XrCfg from isaaclab_tasks.manager_based.manipulation.stack import mdp # 导入 MindRobot 资产 -from .mindrobot_cfg import MINDBOT_HIGH_PD_CFG +from .mindrobot_cfg import ( + MINDBOT_HIGH_PD_CFG, + MINDBOT_LEFT_ARM_JOINTS, + MINDBOT_LEFT_EEF_BODY, + MINDBOT_LEFT_GRIPPER_JOINTS, + MINDBOT_WHEEL_JOINTS, + MINDBOT_LEFT_ARM_PRIM_ROOT, + MINDBOT_LEFT_EEF_PRIM, + MINDBOT_RIGHT_ARM_PRIM_ROOT, +) # 在文件开头添加 import isaaclab.utils.assets as assets_utils @@ -119,8 +128,8 @@ class MindRobotTeleopActionsCfg: # Left arm IK control arm_action = DifferentialInverseKinematicsActionCfg( asset_name="robot", - joint_names=["l_joint[1-6]"], - body_name="left_hand_body", + joint_names=[MINDBOT_LEFT_ARM_JOINTS], + body_name=MINDBOT_LEFT_EEF_BODY, controller=DifferentialIKControllerCfg( command_type="pose", use_relative_mode=True, @@ -134,24 +143,17 @@ class MindRobotTeleopActionsCfg: # 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"], - joint_names=[ - "right_b_revolute_Joint", - "left_b_revolute_Joint", - "left_f_revolute_Joint", - "right_f_revolute_Joint", - ], + joint_names=MINDBOT_WHEEL_JOINTS, 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"], + joint_names=MINDBOT_LEFT_GRIPPER_JOINTS, open_command_expr={ "left_hand_joint_left": 0.0, "left_hand_joint_right": 0.0, @@ -225,8 +227,8 @@ def _disable_arm_gravity(env, env_ids: torch.Tensor): 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", + 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) @@ -309,16 +311,14 @@ class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): 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", + 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="{ENV_REGEX_NS}/Robot/rm_65_fb_left/l_hand_01/left_hand_body", + prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_LEFT_EEF_PRIM}", name="end_effector", - offset=OffsetCfg( - pos=[0.0, 0.0, 0.0], - ), + offset=OffsetCfg(pos=[0.0, 0.0, 0.0]), ), ], ) @@ -346,7 +346,7 @@ class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): ) # Gripper parameters for status checking - self.gripper_joint_names = ["left_hand_joint_left", "left_hand_joint_right"] + self.gripper_joint_names = MINDBOT_LEFT_GRIPPER_JOINTS self.gripper_open_val = 0.0 self.gripper_threshold = 0.005 diff --git a/source/mindbot/mindbot/utils/assets.py b/source/mindbot/mindbot/utils/assets.py index ad019cd..d88d8f0 100644 --- a/source/mindbot/mindbot/utils/assets.py +++ b/source/mindbot/mindbot/utils/assets.py @@ -20,6 +20,6 @@ import os ## MINDBOT_ASSETS_DIR: str = os.environ.get( "MINDBOT_ASSETS_DIR", - # "/home/tangger/DataDisk/maic_usd_assets_moudle", - "/home/maic/LYT/maic_usd_assets_moudle", + "/home/tangger/LYT/maic_usd_assets_moudle", + # "/home/maic/LYT/maic_usd_assets_moudle", )