双臂烘箱调整

2026-03-14 20:45:41 +08:00
parent 1afdf9c7f7
commit 763e555862
8 changed files with 275 additions and 56 deletions
--- a/scripts/environments/teleoperation/teleop_xr_agent.py
+++ b/scripts/environments/teleoperation/teleop_xr_agent.py
@@ -44,8 +44,10 @@ parser.add_argument(
 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_speed", type=float, default=5.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.")
+parser.add_argument("--drive_speed", type=float, default=0.5, help="Max robot linear speed (m/s) for direct base control. Default: 0.5")
+parser.add_argument("--drive_turn", type=float, default=1.5, help="Max robot yaw rate (rad/s) for direct base control. Default: 1.5")

 AppLauncher.add_app_launcher_args(parser)
 args_cli = parser.parse_args()
@@ -392,19 +394,54 @@ def main() -> None:
        out[3:7] = torch.tensor(quat_root, dtype=torch.float32)
        return out

-    def get_wheel_action() -> torch.Tensor:
-        """Read left joystick → 4-DOF wheel velocity [right_b, left_b, left_f, right_f]."""
+    def get_chassis_commands() -> tuple[torch.Tensor, float, float]:
+        """Read left joystick → (wheel_cmd 4D, v_fwd m/s, omega rad/s).
+
+        wheel_cmd  : differential wheel velocities for the action vector (training data).
+        v_fwd      : desired robot forward speed in m/s (for direct base control).
+        omega      : desired robot yaw rate in rad/s  (for direct base control).
+        """
        try:
            joy = teleop_interface.xr_client.get_joystick("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
-        right_vel = v - omega
-        left_vel  = v + omega
-        return torch.tensor([right_vel, left_vel, left_vel, right_vel], dtype=torch.float32)
+            return torch.zeros(4), 0.0, 0.0
+        # Wheel velocity commands for action vector (rad/s)
+        v_w = jy * args_cli.base_speed
+        omega_w = jx * args_cli.base_turn
+        right_vel = v_w - omega_w
+        left_vel  = v_w + omega_w
+        wheel_cmd = torch.tensor([right_vel, left_vel, left_vel, right_vel], dtype=torch.float32)
+        if abs(v_w) > 0.1 or abs(omega_w) > 0.1:
+            print(f"[WHEEL] joy=({jx:+.2f},{jy:+.2f}) v_wheel={v_w:.2f} ω_wheel={omega_w:.2f} → {wheel_cmd.tolist()}")
+        # Direct base velocity commands (m/s, rad/s)
+        v_fwd  =  jy * args_cli.drive_speed
+        omega  = -jx * args_cli.drive_turn   # jx<0 (left push) → positive yaw = left turn
+        return wheel_cmd, v_fwd, omega
+
+    def apply_base_velocity(v_fwd: float, omega: float) -> None:
+        """Directly set robot root linear+angular velocity to bypass isotropic friction.
+
+        PhysX uses isotropic friction, so skid-steer lateral slip is impossible through
+        wheel torques alone. This function overrides the base velocity each step to give
+        clean translational + rotational motion regardless of friction.
+        The write is buffered and applied at the start of the next env.step().
+        """
+        if abs(v_fwd) < 1e-4 and abs(omega) < 1e-4:
+            return
+        robot = env.unwrapped.scene["robot"]
+        rq = robot.data.root_quat_w          # [N, 4]  wxyz
+        w_q, x_q, y_q, z_q = rq[:, 0], rq[:, 1], rq[:, 2], rq[:, 3]
+        # Robot forward direction in world frame (rotate [1,0,0] by root quaternion)
+        fwd_x = 1.0 - 2.0 * (y_q * y_q + z_q * z_q)
+        fwd_y = 2.0 * (x_q * y_q + w_q * z_q)
+        vel = torch.zeros(env.num_envs, 6, device=device)
+        vel[:, 0] = v_fwd * fwd_x   # world x linear
+        vel[:, 1] = v_fwd * fwd_y   # world y linear
+        # vel[:, 2] = 0              # z: let physics handle gravity/contact
+        vel[:, 5] = omega            # world z angular (yaw)
+        robot.write_root_velocity_to_sim(vel)

    obs, _ = env.reset()
    clear_ik_target_state()
@@ -446,7 +483,7 @@ def main() -> None:
                    continue

                policy_obs = obs["policy"]
-                wheel_np = get_wheel_action()
+                wheel_np, v_fwd, omega_base = get_chassis_commands()

                if is_dual_arm:
                    eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
@@ -482,6 +519,9 @@ def main() -> None:

                actions = action_np.unsqueeze(0).repeat(env.num_envs, 1).to(device)
                obs, _, _, _, _ = env.step(actions)
+                # Direct base velocity override: bypasses isotropic friction limitation
+                # so skid-steer turning works correctly.
+                apply_base_velocity(v_fwd, omega_base)

                sim_frame += 1
                if sim_frame % 30 == 0:
--- a/source/mindbot/mindbot/assets/dryingbox.py
+++ b/source/mindbot/mindbot/assets/dryingbox.py
@@ -11,11 +11,13 @@ from mindbot.utils.assets import MINDBOT_ASSETS_DIR

 DRYINGBOX_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
-        usd_path=r"c:\Users\PC\workpalce\maic_usd_assets_moudle\devices\DryingBox\Equipment_BB_13.usd",
+        usd_path=f"{MINDBOT_ASSETS_DIR}/devices/DryingBox/Equipment_BB_13.usd",
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
-            disable_gravity=True, max_depenetration_velocity=1.0
+            disable_gravity=False,
+            max_depenetration_velocity=1.0
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
+            fix_root_link=False,
            enabled_self_collisions=False,
            solver_position_iteration_count=32,
            solver_velocity_iteration_count=16,
--- a/source/mindbot/mindbot/assets/lab.py
+++ b/source/mindbot/mindbot/assets/lab.py
@@ -12,7 +12,7 @@ from mindbot.utils.assets import MINDBOT_ASSETS_DIR
 ROOM_CFG = AssetBaseCfg(
    # prim_path="{ENV_REGEX_NS}/Room",
    spawn=UsdFileCfg(
-        usd_path=f"{MINDBOT_ASSETS_DIR}\\sences\\Lab\\lab.usd",
+        usd_path=f"{MINDBOT_ASSETS_DIR}/sences/Lab/lab.usd",
    ),
    
 )
--- a/source/mindbot/mindbot/assets/table.py
+++ b/source/mindbot/mindbot/assets/table.py
@@ -1,14 +1,8 @@
-import isaaclab.sim as sim_utils
-from isaaclab.actuators import ImplicitActuatorCfg
-from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
-from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR
+from isaaclab.assets import AssetBaseCfg
 from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
-from isaaclab.sim.schemas.schemas_cfg import (
-    RigidBodyPropertiesCfg,
-    CollisionPropertiesCfg,
-)
 from mindbot.utils.assets import MINDBOT_ASSETS_DIR

+
 TABLE_CFG = AssetBaseCfg(
    # prim_path="{ENV_REGEX_NS}/Table",
    init_state=AssetBaseCfg.InitialStateCfg(
@@ -16,17 +10,6 @@ TABLE_CFG = AssetBaseCfg(
        rot=[0.7071, 0.0, 0.0, -0.7071],
    ),
    spawn=UsdFileCfg(
-        usd_path=f"{MINDBOT_ASSETS_DIR}\\sences\\Table_C\\Table_C.usd",
-        rigid_props=RigidBodyPropertiesCfg(
-            rigid_body_enabled=True,
-            solver_position_iteration_count=32,
-            solver_velocity_iteration_count=16,
-            disable_gravity=False,
-        ),
-        collision_props=CollisionPropertiesCfg(
-            collision_enabled=True,
-            contact_offset=0.0005,  # original 0.02
-            rest_offset=0,
-        ),
+        usd_path=f"{MINDBOT_ASSETS_DIR}/sences/Table_C_coll/Table_C.usd",
    ),
 )
--- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/CLAUDE.md
+++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/CLAUDE.md
@@ -0,0 +1,86 @@
+# open_drybox 子目录 — Claude 编码指引
+
+本文件是 `source/mindbot/mindbot/tasks/manager_based/il/open_drybox/` 的局部 Claude 指引，
+补充根目录 `CLAUDE.md` 中未覆盖的细节。
+
+---
+
+## 文件职责速查
+
+| 文件 | 职责 | 修改频率 |
+|---|---|---|
+| `mindrobot_cfg.py` | 机器人物理参数、初始位姿、关节名称常量 | 低（调参时改） |
+| `mindrobot_left_arm_ik_env_cfg.py` | 单臂 IK 环境（Rel/Abs）配置 | 中 |
+| `mindrobot_dual_arm_ik_env_cfg.py` | 双臂 IK 环境配置，**直接继承** `ManagerBasedRLEnvCfg` | 中 |
+| `mdp/__init__.py` | 聚合 MDP 函数，**import 顺序决定覆盖优先级**（后者覆盖前者） | 低 |
+| `mdp/observations.py` | 自定义观测函数，覆盖 IsaacLab 同名函数 | 低 |
+| `__init__.py` | Gym 注册，新增任务必须在这里 `gym.register` | 加任务时改 |
+
+---
+
+## 关键设计决策
+
+### 1. 不修改 IsaacLab 源码
+所有自定义函数放在本地 `mdp/` 中，通过 import 覆盖机制生效：
+```python
+# mdp/__init__.py 的 import 顺序
+from isaaclab.envs.mdp import *             # 基础函数
+from isaaclab_tasks...stack.mdp import *    # stack 任务函数（含 ee_frame_pos 等）
+from .observations import *                 # 本地覆盖（gripper_pos 等）
+```
+
+### 2. 双臂 env cfg 独立，不继承单臂
+`mindrobot_dual_arm_ik_env_cfg.py` 直接继承 `ManagerBasedRLEnvCfg`，
+避免单臂配置的耦合问题。
+
+### 3. gripper_pos 必须指定 joint_names
+```python
+# 正确用法
+ObsTerm(func=mdp.gripper_pos, params={"joint_names": MINDBOT_LEFT_GRIPPER_JOINTS})
+# 错误：不传 joint_names 会抛 ValueError
+ObsTerm(func=mdp.gripper_pos)
+```
+
+### 4. XR 双臂共享 XrClient
+两个 `XrTeleopController` 实例必须共享同一个 `XrClient`，否则 SDK 崩溃：
+```python
+shared_client = XrClient()
+teleop_left  = XrTeleopController(arm="left",  xr_client=shared_client)
+teleop_right = XrTeleopController(arm="right", xr_client=shared_client)
+```
+
+### 5. 关节锁定（grip 松开时）
+teleop 主循环缓存 root-frame IK 命令（`last_root_left/right`），
+grip 未按下时发送冻结的缓存值，防止底盘漂移带动关节运动。
+
+---
+
+## 修改检查清单
+
+**新增关节组或重命名关节**
+- [ ] 更新 `mindrobot_cfg.py` 中的常量（`MINDBOT_*`）
+- [ ] 同步更新 `mindrobot_dual_arm_ik_env_cfg.py` 的 Actions/Observations
+
+**新增观测函数**
+- [ ] 在 `mdp/observations.py` 中添加
+- [ ] 确认 `mdp/__init__.py` 的 import 顺序不会被覆盖
+- [ ] 在 env cfg 的 `PolicyCfg` 中添加 `ObsTerm`
+
+**新增任务**
+- [ ] 在 `__init__.py` 中 `gym.register`
+- [ ] 确认 `mindbot/tasks/manager_based/__init__.py` 已 import 本包
+
+**修改初始位姿**
+- 注意避免奇异点：肘关节 `q3≠0`，腕关节 `q5≠0`
+- 修改后在仿真中目视验证
+
+---
+
+## 常见错误
+
+| 错误 | 原因 | 修复 |
+|---|---|---|
+| `AttributeError: module 'mdp' has no attribute 'ee_frame_pos'` | `mdp/__init__.py` 缺少 `stack.mdp` import | 确认三行 import 都在 |
+| `KeyError: 'arm_action'` in teleop | 双臂任务动作名是 `left_arm_action`，不是 `arm_action` | 检查 `clear_ik_target_state` 的 is_dual_arm 分支 |
+| `terminate called without an active exception` (XR SDK) | 两个 `XrClient()` 实例并存 | 共享 `shared_client` 传入两个控制器 |
+| gripper obs assertion error | `gripper_pos` 未传 `joint_names` | 加 `params={"joint_names": ...}` |
--- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/README.md
+++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/README.md
@@ -0,0 +1,103 @@
+# open_drybox — MindRobot IL 遥操作任务
+
+基于 Isaac Lab `ManagerBasedRLEnv` 的模仿学习（IL）数据采集任务，支持 MindRobot 单臂/双臂 IK 绝对/相对控制，配合 XR 手柄或键盘进行遥操作。
+
+---
+
+## 注册的任务 ID
+
+| 任务 ID | 控制模式 | 说明 |
+|---|---|---|
+| `Isaac-MindRobot-LeftArm-IK-Rel-v0` | 左臂 IK 相对 | EEF 增量控制 |
+| `Isaac-MindRobot-LeftArm-IK-Abs-v0` | 左臂 IK 绝对 | EEF 绝对位姿控制 |
+| `Isaac-MindRobot-DualArm-IK-Abs-v0` | 双臂 IK 绝对 | 双臂 + 底盘，20D 动作空间 |
+
+---
+
+## 目录结构
+
+```
+open_drybox/
+├── __init__.py                        # Gym 任务注册
+├── mindrobot_cfg.py                   # 机器人关节/执行器/初始位姿配置（MINDBOT_CFG）
+│                                      # 及所有关节名称常量（MINDBOT_*）
+├── mindrobot_left_arm_ik_env_cfg.py   # 单臂任务环境配置
+├── mindrobot_dual_arm_ik_env_cfg.py   # 双臂任务环境配置（继承 ManagerBasedRLEnvCfg）
+├── mdp/
+│   ├── __init__.py                    # 聚合 isaaclab.envs.mdp + stack.mdp + 本地 obs
+│   └── observations.py               # gripper_pos(joint_names) — 支持双爪独立观测
+└── agents/
+    └── robomimic/
+        ├── bc_rnn_low_dim.json        # RoboMimic BC-RNN 低维观测配置
+        ├── bc_rnn_image_200.json      # BC-RNN 图像观测配置
+        └── bc_rnn_image_cosmos.json   # BC-RNN Cosmos 图像配置
+```
+
+---
+
+## 双臂动作空间（20D）
+
+```
+[ left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1) ]
+  EEF 绝对位姿   轮速rad/s  +1=开/-1=关       EEF 绝对位姿   +1=开/-1=关
+  [x,y,z,qw,qx,qy,qz]                        [x,y,z,qw,qx,qy,qz]
+```
+
+轮子顺序（与 `MINDBOT_WHEEL_JOINTS` 一致）：`[right_b, left_b, left_f, right_f]`
+
+---
+
+## 关键常量（mindrobot_cfg.py）
+
+```python
+MINDBOT_LEFT_ARM_JOINTS   = "l_joint[1-6]"
+MINDBOT_RIGHT_ARM_JOINTS  = "r_joint[1-6]"
+MINDBOT_LEFT_GRIPPER_JOINTS  = ["left_hand_joint_left",  "left_hand_joint_right"]
+MINDBOT_RIGHT_GRIPPER_JOINTS = ["right_hand_joint_left", "right_hand_joint_right"]
+MINDBOT_WHEEL_JOINTS = ["right_b_revolute_Joint", "left_b_revolute_Joint",
+                         "left_f_revolute_Joint",  "right_f_revolute_Joint"]
+MINDBOT_LEFT_EEF_BODY  = "left_hand_body"
+MINDBOT_RIGHT_EEF_BODY = "right_hand_body"
+```
+
+---
+
+## 遥操作启动命令
+
+```bash
+# XR 双臂
+~/IsaacLab/isaaclab.sh -p scripts/environments/teleoperation/teleop_xr_agent.py \
+    --task Isaac-MindRobot-DualArm-IK-Abs-v0
+
+# XR 单臂
+~/IsaacLab/isaaclab.sh -p scripts/environments/teleoperation/teleop_xr_agent.py \
+    --task Isaac-MindRobot-LeftArm-IK-Abs-v0
+
+# 键盘 + 差速底盘
+~/IsaacLab/isaaclab.sh -p scripts/environments/teleoperation/tele_se3_with_wheel_agent.py \
+    --task Isaac-MindRobot-LeftArm-IK-Rel-v0
+```
+
+---
+
+## 数据采集 / 回放
+
+```bash
+# 录制 demo（输出 HDF5）
+~/IsaacLab/isaaclab.sh -p scripts/tools/record_demos.py \
+    --task Isaac-MindRobot-DualArm-IK-Abs-v0 --num_demos 10
+
+# 回放 demo 验证
+~/IsaacLab/isaaclab.sh -p scripts/tools/replay_demos.py \
+    --task Isaac-MindRobot-DualArm-IK-Abs-v0 --dataset path/to/demo.hdf5
+```
+
+---
+
+## 注意事项
+
+- 所有 Isaac Lab 脚本必须通过 `~/IsaacLab/isaaclab.sh -p` 启动
+- USD 资源路径通过环境变量设置：`export MINDBOT_ASSETS_DIR=/your/path`
+- `mdp/observations.py` 的 `gripper_pos` 覆盖了 `stack.mdp` 的同名函数，
+  支持通过 `joint_names` 参数区分左右夹爪，**不需要修改 IsaacLab 源码**
+- 双臂环境的 `FrameTransformer` 根节点是各臂的 `base_link`，目标是 EEF prim
--- 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
@@ -31,6 +31,7 @@ from isaaclab.scene import InteractiveSceneCfg
 from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg, OffsetCfg
 from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg
 from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg
+from isaaclab.sim.spawners.materials import RigidBodyMaterialCfg
 from isaaclab.utils import configclass

 from . import mdp
@@ -50,6 +51,9 @@ from .mindrobot_cfg import (
    MINDBOT_RIGHT_EEF_PRIM,
 )

+from mindbot.assets.lab import ROOM_CFG
+from mindbot.assets.table import TABLE_CFG
+from mindbot.assets.dryingbox import DRYINGBOX_CFG

 # =====================================================================
 # Scene Configuration
@@ -62,20 +66,21 @@ class MindRobotDualArmSceneCfg(InteractiveSceneCfg):
    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)),
+        spawn=GroundPlaneCfg(
+            # Moderate friction: enough traction to move forward, low enough to
+            # allow skid-steer lateral wheel slip for turning.
+            # Default (1.0/1.0) moves but can't turn; 0.4/0.3 spins in place.
+            physics_material=RigidBodyMaterialCfg(
+                static_friction=0.7,
+                dynamic_friction=0.5,
+                restitution=0.0,
+            )
        ),
-        init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 0.45)),
    )
+    
+    room = ROOM_CFG.replace(prim_path="{ENV_REGEX_NS}/Room")
+    table = TABLE_CFG.replace(prim_path="{ENV_REGEX_NS}/Table")
+    drying_box = DRYINGBOX_CFG.replace(prim_path="{ENV_REGEX_NS}/DryingBox")

    robot: ArticulationCfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")

--- 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
@@ -84,17 +84,17 @@ class MindRobotTeleopSceneCfg(InteractiveSceneCfg):
    # )

    # 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)),
-    )
+    # 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 = ROOM_CFG.replace(prim_path="{ENV_REGEX_NS}/Room")
    table = TABLE_CFG.replace(prim_path="{ENV_REGEX_NS}/Table")