From 2c9db335179f380e63cf0e42140edd4d0511e3b5 Mon Sep 17 00:00:00 2001 From: yt lee Date: Wed, 4 Mar 2026 18:39:56 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=85=A8=E5=B1=80=E5=8F=98?= =?UTF-8?q?=E9=87=8F=E8=B7=AF=E5=BE=84=EF=BC=9B=20=E6=B7=BB=E5=8A=A0IL?= =?UTF-8?q?=E7=9A=84demo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scripts/environments/random_agent.py | 1 + source/mindbot/mindbot/robot/mindbot.py | 7 +- .../tasks/manager_based/il/demo/README.md | 43 ++ .../tasks/manager_based/il/demo/__init__.py | 6 + .../il/demo/franka_stack/__init__.py | 34 ++ .../il/demo/franka_stack/mdp/__init__.py | 11 + .../franka_stack/mdp/franka_stack_events.py | 315 +++++++++++ .../il/demo/franka_stack/mdp/observations.py | 534 ++++++++++++++++++ .../il/demo/franka_stack/mdp/terminations.py | 93 +++ .../demo/franka_stack/stack_ik_rel_env_cfg.py | 69 +++ .../demo/franka_stack/stack_task__init__.py | 9 + .../il/demo/h1_locomotion/__init__.py | 55 ++ .../il/demo/h1_locomotion/flat_env_cfg.py | 41 ++ .../il/demo/h1_locomotion/mdp/__init__.py | 12 + .../il/demo/h1_locomotion/mdp/curriculums.py | 56 ++ .../il/demo/h1_locomotion/mdp/rewards.py | 119 ++++ .../h1_locomotion/mdp/symmetry/__init__.py | 10 + .../demo/h1_locomotion/mdp/symmetry/anymal.py | 261 +++++++++ .../il/demo/h1_locomotion/mdp/terminations.py | 54 ++ .../il/demo/h1_locomotion/rough_env_cfg.py | 142 +++++ .../h1_locomotion/velocity_task__init__.py | 12 + .../il/open_drybox/mindrobot_cfg.py | 7 +- .../mindrobot_left_arm_ik_env_cfg.py | 32 +- .../rl/centrifuge/centrifuge_env_cfg.py | 10 +- .../rl/mindbot/mindbot_env_cfg.py | 8 +- .../manager_based/rl/pull/pull_env_cfg.py | 10 +- .../rl/pullUltrasoundLidUp/mindbot_env_cfg.py | 10 +- source/mindbot/mindbot/utils/__init__.py | 6 + source/mindbot/mindbot/utils/assets.py | 24 + 29 files changed, 1961 insertions(+), 30 deletions(-) create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/README.md create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/__init__.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/__init__.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/__init__.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/franka_stack_events.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/observations.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/terminations.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/stack_ik_rel_env_cfg.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/stack_task__init__.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/__init__.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/flat_env_cfg.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/__init__.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/curriculums.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/rewards.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/symmetry/__init__.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/symmetry/anymal.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/terminations.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/rough_env_cfg.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/velocity_task__init__.py create mode 100644 source/mindbot/mindbot/utils/__init__.py create mode 100644 source/mindbot/mindbot/utils/assets.py diff --git a/scripts/environments/random_agent.py b/scripts/environments/random_agent.py index 6a40060..3df6aa7 100644 --- a/scripts/environments/random_agent.py +++ b/scripts/environments/random_agent.py @@ -33,6 +33,7 @@ import gymnasium as gym import torch import isaaclab_tasks # noqa: F401 +import mindbot.tasks # noqa: F401 from isaaclab_tasks.utils import parse_env_cfg # PLACEHOLDER: Extension template (do not remove this comment) diff --git a/source/mindbot/mindbot/robot/mindbot.py b/source/mindbot/mindbot/robot/mindbot.py index fa2685c..e8ca376 100644 --- a/source/mindbot/mindbot/robot/mindbot.py +++ b/source/mindbot/mindbot/robot/mindbot.py @@ -11,16 +11,15 @@ from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + ## # Configuration ## MINDBOT_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - # 1. UPDATE THE USD PATH to your local file - # C:\Users\PC\workpalce\maic_usd_assets\twinlab\MIC_sim-3.0\pupet_arm_sim_product\sim_data_acquisition\usds\Collected_robot_2_3\robot_2_4.usd - usd_path="/home/maic/LYT/maic_usd_assets/robots/mindrobot-1105-lab/mindrobot_cd2.usd", - # usd_path="/home/maic/LYT/maic_usd_assets/twinlab/MIC_sim-3.0/pupet_arm_sim_product/sim_data_acquisition/usds/Collected_robot_2_3/robot_2_6.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindrobot-1105-lab/mindrobot_cd2.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=1.0, diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/README.md b/source/mindbot/mindbot/tasks/manager_based/il/demo/README.md new file mode 100644 index 0000000..5c8e171 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/README.md @@ -0,0 +1,43 @@ +# IsaacLab 官方 Demo 代码 + +> ⚠️ 本目录提供**可运行的官方 Demo**，不仅可以作为只读参考，更可以直接用来运行和验证，加深对 IsaacLab 各种机制的理解。 +> 所有的环境配置均直接复用了 `/home/tangger/IsaacLab/source/isaaclab_tasks` 里的模块。我们给这些任务加上了 `Mindbot-Demo-` 前缀重新注册，以便直接在此运行。 + +## 目录说明 + +### `franka_stack/` — Franka 机械臂 Stack 任务 (IK-Rel) +- **注册名称**: `Mindbot-Demo-Stack-Cube-Franka-IK-Rel-v0` +- **参考价值**: + - 完整的 ManagerBased IL/RL env cfg 结构（scene / actions / observations / events / rewards / terminations） + - `events/` 里的 `reset_scene_to_default` + `reset_robot_joints` 写法 → **R 键重置参考** + - 这是所有机械臂操作任务（比如你正在写的 open_drybox）的最佳对标参考。 +- **如何运行它**: + ```bash + python scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Mindbot-Demo-Stack-Cube-Franka-IK-Rel-v0 \ + --num_envs 1 --teleop_device keyboard + ``` + +### `h1_locomotion/` — 宇树 H1 人形机器人运动控制 +- **注册名称**: + - `Mindbot-Demo-Velocity-Flat-H1-v0` + - `Mindbot-Demo-Velocity-Rough-H1-v0` +- **参考价值**: + - 人形双足机器人（或移动底盘）的关节配置、观测空间设计 + - 运动控制的 rewards / curriculum / terminations 完整示例 +- **如何运行它**: + ```bash + # 跑一个随机策略看样子 + python scripts/environments/random_agent.py \ + --task Mindbot-Demo-Velocity-Flat-H1-v0 \ + --num_envs 8 + + # 或者测试基于 RL 的训练 (如果有装 rsl_rl) + python scripts/reinforcement_learning/rsl_rl/train.py \ + --task Mindbot-Demo-Velocity-Flat-H1-v0 \ + --num_envs 4096 --headless + ``` + +## 对 AI 辅助的意义 + +当你进行新任务的开发时，AI 会将这些 Demo 当作**强有力的本地示例和知识库**，提供准确、符合 IsaacLab 最优实践的代码修改意见，避免凭空猜设 API。 diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/__init__.py new file mode 100644 index 0000000..3671047 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2024, MindRobot Project +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym # noqa: F401 + +from . import franka_stack, h1_locomotion diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/__init__.py new file mode 100644 index 0000000..264cb33 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2024, MindRobot Project +# SPDX-License-Identifier: BSD-3-Clause +# +# Demo: Franka Stack-Cube IK-Rel +# 直接复用 IsaacLab 官方的 Franka Stack 任务，注册为 Mindbot-Demo 前缀。 +# 参考原始注册: Isaac-Stack-Cube-Franka-IK-Rel-v0 +# +# 运行指令: +# python scripts/environments/teleoperation/teleop_se3_agent.py \ +# --task Mindbot-Demo-Stack-Cube-Franka-IK-Rel-v0 \ +# --num_envs 1 --teleop_device keyboard + +import gymnasium as gym + +## +# Register Gym environments. +## + +gym.register( + id="Mindbot-Demo-Stack-Cube-Franka-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + # 直接引用 isaaclab_tasks 中的原始 cfg，无需 copy 依赖链 + "env_cfg_entry_point": ( + "isaaclab_tasks.manager_based.manipulation.stack.config.franka" + ".stack_ik_rel_env_cfg:FrankaCubeStackEnvCfg" + ), + "robomimic_bc_cfg_entry_point": ( + "isaaclab_tasks.manager_based.manipulation.stack.config.franka" + ".agents:robomimic/bc_rnn_low_dim.json" + ), + }, + disable_env_checker=True, +) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/__init__.py new file mode 100644 index 0000000..ea04fcc --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the lift environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/franka_stack_events.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/franka_stack_events.py new file mode 100644 index 0000000..3d9e1db --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/franka_stack_events.py @@ -0,0 +1,315 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +import math +import random +from typing import TYPE_CHECKING + +import torch + +from isaacsim.core.utils.extensions import enable_extension + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, AssetBase +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def set_default_joint_pose( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + default_pose: torch.Tensor, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + # Set the default pose for robots in all envs + asset = env.scene[asset_cfg.name] + asset.data.default_joint_pos = torch.tensor(default_pose, device=env.device).repeat(env.num_envs, 1) + + +def randomize_joint_by_gaussian_offset( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + mean: float, + std: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + asset: Articulation = env.scene[asset_cfg.name] + + # Add gaussian noise to joint states + joint_pos = asset.data.default_joint_pos[env_ids].clone() + joint_vel = asset.data.default_joint_vel[env_ids].clone() + joint_pos += math_utils.sample_gaussian(mean, std, joint_pos.shape, joint_pos.device) + + # Clamp joint pos to limits + joint_pos_limits = asset.data.soft_joint_pos_limits[env_ids] + joint_pos = joint_pos.clamp_(joint_pos_limits[..., 0], joint_pos_limits[..., 1]) + + # Don't noise the gripper poses + joint_pos[:, -2:] = asset.data.default_joint_pos[env_ids, -2:] + + # Set into the physics simulation + asset.set_joint_position_target(joint_pos, env_ids=env_ids) + asset.set_joint_velocity_target(joint_vel, env_ids=env_ids) + asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + + +def sample_random_color(base=(0.75, 0.75, 0.75), variation=0.1): + """ + Generates a randomized color that stays close to the base color while preserving overall brightness. + The relative balance between the R, G, and B components is maintained by ensuring that + the sum of random offsets is zero. + + Parameters: + base (tuple): The base RGB color with each component between 0 and 1. + variation (float): Maximum deviation to sample for each channel before balancing. + + Returns: + tuple: A new RGB color with balanced random variation. + """ + # Generate random offsets for each channel in the range [-variation, variation] + offsets = [random.uniform(-variation, variation) for _ in range(3)] + # Compute the average offset + avg_offset = sum(offsets) / 3 + # Adjust offsets so their sum is zero (maintaining brightness) + balanced_offsets = [offset - avg_offset for offset in offsets] + + # Apply the balanced offsets to the base color and clamp each channel between 0 and 1 + new_color = tuple(max(0, min(1, base_component + offset)) for base_component, offset in zip(base, balanced_offsets)) + + return new_color + + +def randomize_scene_lighting_domelight( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + intensity_range: tuple[float, float], + color_variation: float, + textures: list[str], + default_intensity: float = 3000.0, + default_color: tuple[float, float, float] = (0.75, 0.75, 0.75), + default_texture: str = "", + asset_cfg: SceneEntityCfg = SceneEntityCfg("light"), +): + asset: AssetBase = env.scene[asset_cfg.name] + light_prim = asset.prims[0] + + intensity_attr = light_prim.GetAttribute("inputs:intensity") + intensity_attr.Set(default_intensity) + + color_attr = light_prim.GetAttribute("inputs:color") + color_attr.Set(default_color) + + texture_file_attr = light_prim.GetAttribute("inputs:texture:file") + texture_file_attr.Set(default_texture) + + if not hasattr(env.cfg, "eval_mode") or not env.cfg.eval_mode: + return + + if env.cfg.eval_type in ["light_intensity", "all"]: + # Sample new light intensity + new_intensity = random.uniform(intensity_range[0], intensity_range[1]) + # Set light intensity to light prim + intensity_attr.Set(new_intensity) + + if env.cfg.eval_type in ["light_color", "all"]: + # Sample new light color + new_color = sample_random_color(base=default_color, variation=color_variation) + # Set light color to light prim + color_attr.Set(new_color) + + if env.cfg.eval_type in ["light_texture", "all"]: + # Sample new light texture (background) + new_texture = random.sample(textures, 1)[0] + # Set light texture to light prim + texture_file_attr.Set(new_texture) + + +def sample_object_poses( + num_objects: int, + min_separation: float = 0.0, + pose_range: dict[str, tuple[float, float]] = {}, + max_sample_tries: int = 5000, +): + range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + pose_list = [] + + for i in range(num_objects): + for j in range(max_sample_tries): + sample = [random.uniform(range[0], range[1]) for range in range_list] + + # Accept pose if it is the first one, or if reached max num tries + if len(pose_list) == 0 or j == max_sample_tries - 1: + pose_list.append(sample) + break + + # Check if pose of object is sufficiently far away from all other objects + separation_check = [math.dist(sample[:3], pose[:3]) > min_separation for pose in pose_list] + if False not in separation_check: + pose_list.append(sample) + break + + return pose_list + + +def randomize_object_pose( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + asset_cfgs: list[SceneEntityCfg], + min_separation: float = 0.0, + pose_range: dict[str, tuple[float, float]] = {}, + max_sample_tries: int = 5000, +): + if env_ids is None: + return + + # Randomize poses in each environment independently + for cur_env in env_ids.tolist(): + pose_list = sample_object_poses( + num_objects=len(asset_cfgs), + min_separation=min_separation, + pose_range=pose_range, + max_sample_tries=max_sample_tries, + ) + + # Randomize pose for each object + for i in range(len(asset_cfgs)): + asset_cfg = asset_cfgs[i] + asset = env.scene[asset_cfg.name] + + # Write pose to simulation + pose_tensor = torch.tensor([pose_list[i]], device=env.device) + positions = pose_tensor[:, 0:3] + env.scene.env_origins[cur_env, 0:3] + orientations = math_utils.quat_from_euler_xyz(pose_tensor[:, 3], pose_tensor[:, 4], pose_tensor[:, 5]) + asset.write_root_pose_to_sim( + torch.cat([positions, orientations], dim=-1), env_ids=torch.tensor([cur_env], device=env.device) + ) + asset.write_root_velocity_to_sim( + torch.zeros(1, 6, device=env.device), env_ids=torch.tensor([cur_env], device=env.device) + ) + + +def randomize_rigid_objects_in_focus( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + asset_cfgs: list[SceneEntityCfg], + out_focus_state: torch.Tensor, + min_separation: float = 0.0, + pose_range: dict[str, tuple[float, float]] = {}, + max_sample_tries: int = 5000, +): + if env_ids is None: + return + + # List of rigid objects in focus for each env (dim = [num_envs, num_rigid_objects]) + env.rigid_objects_in_focus = [] + + for cur_env in env_ids.tolist(): + # Sample in focus object poses + pose_list = sample_object_poses( + num_objects=len(asset_cfgs), + min_separation=min_separation, + pose_range=pose_range, + max_sample_tries=max_sample_tries, + ) + + selected_ids = [] + for asset_idx in range(len(asset_cfgs)): + asset_cfg = asset_cfgs[asset_idx] + asset = env.scene[asset_cfg.name] + + # Randomly select an object to bring into focus + object_id = random.randint(0, asset.num_objects - 1) + selected_ids.append(object_id) + + # Create object state tensor + object_states = torch.stack([out_focus_state] * asset.num_objects).to(device=env.device) + pose_tensor = torch.tensor([pose_list[asset_idx]], device=env.device) + positions = pose_tensor[:, 0:3] + env.scene.env_origins[cur_env, 0:3] + orientations = math_utils.quat_from_euler_xyz(pose_tensor[:, 3], pose_tensor[:, 4], pose_tensor[:, 5]) + object_states[object_id, 0:3] = positions + object_states[object_id, 3:7] = orientations + + asset.write_object_state_to_sim( + object_state=object_states, env_ids=torch.tensor([cur_env], device=env.device) + ) + + env.rigid_objects_in_focus.append(selected_ids) + + +def randomize_visual_texture_material( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + asset_cfg: SceneEntityCfg, + textures: list[str], + default_texture: str = "", + texture_rotation: tuple[float, float] = (0.0, 0.0), +): + """Randomize the visual texture of bodies on an asset using Replicator API. + + This function randomizes the visual texture of the bodies of the asset using the Replicator API. + The function samples random textures from the given texture paths and applies them to the bodies + of the asset. The textures are projected onto the bodies and rotated by the given angles. + + .. note:: + The function assumes that the asset follows the prim naming convention as: + "{asset_prim_path}/{body_name}/visuals" where the body name is the name of the body to + which the texture is applied. This is the default prim ordering when importing assets + from the asset converters in Isaac Lab. + + .. note:: + When randomizing the texture of individual assets, please make sure to set + :attr:`isaaclab.scene.InteractiveSceneCfg.replicate_physics` to False. This ensures that physics + parser will parse the individual asset properties separately. + """ + if hasattr(env.cfg, "eval_mode") and ( + not env.cfg.eval_mode or env.cfg.eval_type not in [f"{asset_cfg.name}_texture", "all"] + ): + return + # textures = [default_texture] + + # enable replicator extension if not already enabled + enable_extension("omni.replicator.core") + # we import the module here since we may not always need the replicator + import omni.replicator.core as rep + + # check to make sure replicate_physics is set to False, else raise error + # note: We add an explicit check here since texture randomization can happen outside of 'prestartup' mode + # and the event manager doesn't check in that case. + if env.cfg.scene.replicate_physics: + raise RuntimeError( + "Unable to randomize visual texture material with scene replication enabled." + " For stable USD-level randomization, please disable scene replication" + " by setting 'replicate_physics' to False in 'InteractiveSceneCfg'." + ) + + # convert from radians to degrees + texture_rotation = tuple(math.degrees(angle) for angle in texture_rotation) + + # obtain the asset entity + asset = env.scene[asset_cfg.name] + + # join all bodies in the asset + body_names = asset_cfg.body_names + if isinstance(body_names, str): + body_names_regex = body_names + elif isinstance(body_names, list): + body_names_regex = "|".join(body_names) + else: + body_names_regex = ".*" + + if not hasattr(asset, "cfg"): + prims_group = rep.get.prims(path_pattern=f"{asset.prim_paths[0]}/visuals") + else: + prims_group = rep.get.prims(path_pattern=f"{asset.cfg.prim_path}/{body_names_regex}/visuals") + + with prims_group: + rep.randomizer.texture( + textures=textures, project_uvw=True, texture_rotate=rep.distribution.uniform(*texture_rotation) + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/observations.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/observations.py new file mode 100644 index 0000000..31123e7 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/observations.py @@ -0,0 +1,534 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING, Literal + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import FrameTransformer + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def cube_positions_in_world_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), +) -> torch.Tensor: + """The position of the cubes in the world frame.""" + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + + return torch.cat((cube_1.data.root_pos_w, cube_2.data.root_pos_w, cube_3.data.root_pos_w), dim=1) + + +def instance_randomize_cube_positions_in_world_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), +) -> torch.Tensor: + """The position of the cubes in the world frame.""" + if not hasattr(env, "rigid_objects_in_focus"): + return torch.full((env.num_envs, 9), fill_value=-1) + + cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name] + cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name] + cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name] + + cube_1_pos_w = [] + cube_2_pos_w = [] + cube_3_pos_w = [] + for env_id in range(env.num_envs): + cube_1_pos_w.append(cube_1.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][0], :3]) + cube_2_pos_w.append(cube_2.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][1], :3]) + cube_3_pos_w.append(cube_3.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][2], :3]) + cube_1_pos_w = torch.stack(cube_1_pos_w) + cube_2_pos_w = torch.stack(cube_2_pos_w) + cube_3_pos_w = torch.stack(cube_3_pos_w) + + return torch.cat((cube_1_pos_w, cube_2_pos_w, cube_3_pos_w), dim=1) + + +def cube_orientations_in_world_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), +): + """The orientation of the cubes in the world frame.""" + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + + return torch.cat((cube_1.data.root_quat_w, cube_2.data.root_quat_w, cube_3.data.root_quat_w), dim=1) + + +def instance_randomize_cube_orientations_in_world_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), +) -> torch.Tensor: + """The orientation of the cubes in the world frame.""" + if not hasattr(env, "rigid_objects_in_focus"): + return torch.full((env.num_envs, 9), fill_value=-1) + + cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name] + cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name] + cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name] + + cube_1_quat_w = [] + cube_2_quat_w = [] + cube_3_quat_w = [] + for env_id in range(env.num_envs): + cube_1_quat_w.append(cube_1.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][0], :4]) + cube_2_quat_w.append(cube_2.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][1], :4]) + cube_3_quat_w.append(cube_3.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][2], :4]) + cube_1_quat_w = torch.stack(cube_1_quat_w) + cube_2_quat_w = torch.stack(cube_2_quat_w) + cube_3_quat_w = torch.stack(cube_3_quat_w) + + return torch.cat((cube_1_quat_w, cube_2_quat_w, cube_3_quat_w), dim=1) + + +def object_obs( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), +): + """ + Object observations (in world frame): + cube_1 pos, + cube_1 quat, + cube_2 pos, + cube_2 quat, + cube_3 pos, + cube_3 quat, + gripper to cube_1, + gripper to cube_2, + gripper to cube_3, + cube_1 to cube_2, + cube_2 to cube_3, + cube_1 to cube_3, + """ + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + + cube_1_pos_w = cube_1.data.root_pos_w + cube_1_quat_w = cube_1.data.root_quat_w + + cube_2_pos_w = cube_2.data.root_pos_w + cube_2_quat_w = cube_2.data.root_quat_w + + cube_3_pos_w = cube_3.data.root_pos_w + cube_3_quat_w = cube_3.data.root_quat_w + + ee_pos_w = ee_frame.data.target_pos_w[:, 0, :] + gripper_to_cube_1 = cube_1_pos_w - ee_pos_w + gripper_to_cube_2 = cube_2_pos_w - ee_pos_w + gripper_to_cube_3 = cube_3_pos_w - ee_pos_w + + cube_1_to_2 = cube_1_pos_w - cube_2_pos_w + cube_2_to_3 = cube_2_pos_w - cube_3_pos_w + cube_1_to_3 = cube_1_pos_w - cube_3_pos_w + + return torch.cat( + ( + cube_1_pos_w - env.scene.env_origins, + cube_1_quat_w, + cube_2_pos_w - env.scene.env_origins, + cube_2_quat_w, + cube_3_pos_w - env.scene.env_origins, + cube_3_quat_w, + gripper_to_cube_1, + gripper_to_cube_2, + gripper_to_cube_3, + cube_1_to_2, + cube_2_to_3, + cube_1_to_3, + ), + dim=1, + ) + + +def instance_randomize_object_obs( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), +): + """ + Object observations (in world frame): + cube_1 pos, + cube_1 quat, + cube_2 pos, + cube_2 quat, + cube_3 pos, + cube_3 quat, + gripper to cube_1, + gripper to cube_2, + gripper to cube_3, + cube_1 to cube_2, + cube_2 to cube_3, + cube_1 to cube_3, + """ + if not hasattr(env, "rigid_objects_in_focus"): + return torch.full((env.num_envs, 9), fill_value=-1) + + cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name] + cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name] + cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + + cube_1_pos_w = [] + cube_2_pos_w = [] + cube_3_pos_w = [] + cube_1_quat_w = [] + cube_2_quat_w = [] + cube_3_quat_w = [] + for env_id in range(env.num_envs): + cube_1_pos_w.append(cube_1.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][0], :3]) + cube_2_pos_w.append(cube_2.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][1], :3]) + cube_3_pos_w.append(cube_3.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][2], :3]) + cube_1_quat_w.append(cube_1.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][0], :4]) + cube_2_quat_w.append(cube_2.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][1], :4]) + cube_3_quat_w.append(cube_3.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][2], :4]) + cube_1_pos_w = torch.stack(cube_1_pos_w) + cube_2_pos_w = torch.stack(cube_2_pos_w) + cube_3_pos_w = torch.stack(cube_3_pos_w) + cube_1_quat_w = torch.stack(cube_1_quat_w) + cube_2_quat_w = torch.stack(cube_2_quat_w) + cube_3_quat_w = torch.stack(cube_3_quat_w) + + ee_pos_w = ee_frame.data.target_pos_w[:, 0, :] + gripper_to_cube_1 = cube_1_pos_w - ee_pos_w + gripper_to_cube_2 = cube_2_pos_w - ee_pos_w + gripper_to_cube_3 = cube_3_pos_w - ee_pos_w + + cube_1_to_2 = cube_1_pos_w - cube_2_pos_w + cube_2_to_3 = cube_2_pos_w - cube_3_pos_w + cube_1_to_3 = cube_1_pos_w - cube_3_pos_w + + return torch.cat( + ( + cube_1_pos_w - env.scene.env_origins, + cube_1_quat_w, + cube_2_pos_w - env.scene.env_origins, + cube_2_quat_w, + cube_3_pos_w - env.scene.env_origins, + cube_3_quat_w, + gripper_to_cube_1, + gripper_to_cube_2, + gripper_to_cube_3, + cube_1_to_2, + cube_2_to_3, + cube_1_to_3, + ), + dim=1, + ) + + +def ee_frame_pos(env: ManagerBasedRLEnv, ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame")) -> torch.Tensor: + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + ee_frame_pos = ee_frame.data.target_pos_w[:, 0, :] - env.scene.env_origins[:, 0:3] + + return ee_frame_pos + + +def ee_frame_quat(env: ManagerBasedRLEnv, ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame")) -> torch.Tensor: + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + ee_frame_quat = ee_frame.data.target_quat_w[:, 0, :] + + return ee_frame_quat + + +def gripper_pos( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """ + Obtain the versatile gripper position of both Gripper and Suction Cup. + """ + robot: Articulation = env.scene[robot_cfg.name] + + if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0: + # Handle multiple surface grippers by concatenating their states + gripper_states = [] + for gripper_name, surface_gripper in env.scene.surface_grippers.items(): + gripper_states.append(surface_gripper.state.view(-1, 1)) + + if len(gripper_states) == 1: + return gripper_states[0] + else: + return torch.cat(gripper_states, dim=1) + + else: + if hasattr(env.cfg, "gripper_joint_names"): + gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names) + assert len(gripper_joint_ids) == 2, "Observation gripper_pos only support parallel gripper for now" + finger_joint_1 = robot.data.joint_pos[:, gripper_joint_ids[0]].clone().unsqueeze(1) + finger_joint_2 = -1 * robot.data.joint_pos[:, gripper_joint_ids[1]].clone().unsqueeze(1) + return torch.cat((finger_joint_1, finger_joint_2), dim=1) + else: + raise NotImplementedError("[Error] Cannot find gripper_joint_names in the environment config") + + +def object_grasped( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg, + ee_frame_cfg: SceneEntityCfg, + object_cfg: SceneEntityCfg, + diff_threshold: float = 0.06, +) -> torch.Tensor: + """Check if an object is grasped by the specified robot.""" + + robot: Articulation = env.scene[robot_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + + object_pos = object.data.root_pos_w + end_effector_pos = ee_frame.data.target_pos_w[:, 0, :] + pose_diff = torch.linalg.vector_norm(object_pos - end_effector_pos, dim=1) + + if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0: + surface_gripper = env.scene.surface_grippers["surface_gripper"] + suction_cup_status = surface_gripper.state.view(-1, 1) # 1: closed, 0: closing, -1: open + suction_cup_is_closed = (suction_cup_status == 1).to(torch.float32) + grasped = torch.logical_and(suction_cup_is_closed, pose_diff < diff_threshold) + + else: + if hasattr(env.cfg, "gripper_joint_names"): + gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names) + assert len(gripper_joint_ids) == 2, "Observations only support parallel gripper for now" + + grasped = torch.logical_and( + pose_diff < diff_threshold, + torch.abs( + robot.data.joint_pos[:, gripper_joint_ids[0]] + - torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device) + ) + > env.cfg.gripper_threshold, + ) + grasped = torch.logical_and( + grasped, + torch.abs( + robot.data.joint_pos[:, gripper_joint_ids[1]] + - torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device) + ) + > env.cfg.gripper_threshold, + ) + + return grasped + + +def object_stacked( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg, + upper_object_cfg: SceneEntityCfg, + lower_object_cfg: SceneEntityCfg, + xy_threshold: float = 0.05, + height_threshold: float = 0.005, + height_diff: float = 0.0468, +) -> torch.Tensor: + """Check if an object is stacked by the specified robot.""" + + robot: Articulation = env.scene[robot_cfg.name] + upper_object: RigidObject = env.scene[upper_object_cfg.name] + lower_object: RigidObject = env.scene[lower_object_cfg.name] + + pos_diff = upper_object.data.root_pos_w - lower_object.data.root_pos_w + height_dist = torch.linalg.vector_norm(pos_diff[:, 2:], dim=1) + xy_dist = torch.linalg.vector_norm(pos_diff[:, :2], dim=1) + + stacked = torch.logical_and(xy_dist < xy_threshold, (height_dist - height_diff) < height_threshold) + + if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0: + surface_gripper = env.scene.surface_grippers["surface_gripper"] + suction_cup_status = surface_gripper.state.view(-1, 1) # 1: closed, 0: closing, -1: open + suction_cup_is_open = (suction_cup_status == -1).to(torch.float32) + stacked = torch.logical_and(suction_cup_is_open, stacked) + + else: + if hasattr(env.cfg, "gripper_joint_names"): + gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names) + assert len(gripper_joint_ids) == 2, "Observations only support parallel gripper for now" + stacked = torch.logical_and( + torch.isclose( + robot.data.joint_pos[:, gripper_joint_ids[0]], + torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device), + atol=1e-4, + rtol=1e-4, + ), + stacked, + ) + stacked = torch.logical_and( + torch.isclose( + robot.data.joint_pos[:, gripper_joint_ids[1]], + torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device), + atol=1e-4, + rtol=1e-4, + ), + stacked, + ) + else: + raise ValueError("No gripper_joint_names found in environment config") + + return stacked + + +def cube_poses_in_base_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + return_key: Literal["pos", "quat", None] = None, +) -> torch.Tensor: + """The position and orientation of the cubes in the robot base frame.""" + + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + + pos_cube_1_world = cube_1.data.root_pos_w + pos_cube_2_world = cube_2.data.root_pos_w + pos_cube_3_world = cube_3.data.root_pos_w + + quat_cube_1_world = cube_1.data.root_quat_w + quat_cube_2_world = cube_2.data.root_quat_w + quat_cube_3_world = cube_3.data.root_quat_w + + robot: Articulation = env.scene[robot_cfg.name] + root_pos_w = robot.data.root_pos_w + root_quat_w = robot.data.root_quat_w + + pos_cube_1_base, quat_cube_1_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, pos_cube_1_world, quat_cube_1_world + ) + pos_cube_2_base, quat_cube_2_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, pos_cube_2_world, quat_cube_2_world + ) + pos_cube_3_base, quat_cube_3_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, pos_cube_3_world, quat_cube_3_world + ) + + pos_cubes_base = torch.cat((pos_cube_1_base, pos_cube_2_base, pos_cube_3_base), dim=1) + quat_cubes_base = torch.cat((quat_cube_1_base, quat_cube_2_base, quat_cube_3_base), dim=1) + + if return_key == "pos": + return pos_cubes_base + elif return_key == "quat": + return quat_cubes_base + else: + return torch.cat((pos_cubes_base, quat_cubes_base), dim=1) + + +def object_abs_obs_in_base_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """ + Object Abs observations (in base frame): remove the relative observations, + and add abs gripper pos and quat in robot base frame + cube_1 pos, + cube_1 quat, + cube_2 pos, + cube_2 quat, + cube_3 pos, + cube_3 quat, + gripper pos, + gripper quat, + """ + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + robot: Articulation = env.scene[robot_cfg.name] + + root_pos_w = robot.data.root_pos_w + root_quat_w = robot.data.root_quat_w + + cube_1_pos_w = cube_1.data.root_pos_w + cube_1_quat_w = cube_1.data.root_quat_w + + cube_2_pos_w = cube_2.data.root_pos_w + cube_2_quat_w = cube_2.data.root_quat_w + + cube_3_pos_w = cube_3.data.root_pos_w + cube_3_quat_w = cube_3.data.root_quat_w + + pos_cube_1_base, quat_cube_1_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, cube_1_pos_w, cube_1_quat_w + ) + pos_cube_2_base, quat_cube_2_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, cube_2_pos_w, cube_2_quat_w + ) + pos_cube_3_base, quat_cube_3_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, cube_3_pos_w, cube_3_quat_w + ) + + ee_pos_w = ee_frame.data.target_pos_w[:, 0, :] + ee_quat_w = ee_frame.data.target_quat_w[:, 0, :] + ee_pos_base, ee_quat_base = math_utils.subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w) + + return torch.cat( + ( + pos_cube_1_base, + quat_cube_1_base, + pos_cube_2_base, + quat_cube_2_base, + pos_cube_3_base, + quat_cube_3_base, + ee_pos_base, + ee_quat_base, + ), + dim=1, + ) + + +def ee_frame_pose_in_base_frame( + env: ManagerBasedRLEnv, + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + return_key: Literal["pos", "quat", None] = None, +) -> torch.Tensor: + """ + The end effector pose in the robot base frame. + """ + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + ee_frame_pos_w = ee_frame.data.target_pos_w[:, 0, :] + ee_frame_quat_w = ee_frame.data.target_quat_w[:, 0, :] + + robot: Articulation = env.scene[robot_cfg.name] + root_pos_w = robot.data.root_pos_w + root_quat_w = robot.data.root_quat_w + ee_pos_in_base, ee_quat_in_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, ee_frame_pos_w, ee_frame_quat_w + ) + + if return_key == "pos": + return ee_pos_in_base + elif return_key == "quat": + return ee_quat_in_base + else: + return torch.cat((ee_pos_in_base, ee_quat_in_base), dim=1) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/terminations.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/terminations.py new file mode 100644 index 0000000..d486803 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/mdp/terminations.py @@ -0,0 +1,93 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations for the lift task. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def cubes_stacked( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + xy_threshold: float = 0.04, + height_threshold: float = 0.005, + height_diff: float = 0.0468, + atol=0.0001, + rtol=0.0001, +): + robot: Articulation = env.scene[robot_cfg.name] + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + + pos_diff_c12 = cube_1.data.root_pos_w - cube_2.data.root_pos_w + pos_diff_c23 = cube_2.data.root_pos_w - cube_3.data.root_pos_w + + # Compute cube position difference in x-y plane + xy_dist_c12 = torch.norm(pos_diff_c12[:, :2], dim=1) + xy_dist_c23 = torch.norm(pos_diff_c23[:, :2], dim=1) + + # Compute cube height difference + h_dist_c12 = torch.norm(pos_diff_c12[:, 2:], dim=1) + h_dist_c23 = torch.norm(pos_diff_c23[:, 2:], dim=1) + + # Check cube positions + stacked = torch.logical_and(xy_dist_c12 < xy_threshold, xy_dist_c23 < xy_threshold) + stacked = torch.logical_and(h_dist_c12 - height_diff < height_threshold, stacked) + stacked = torch.logical_and(pos_diff_c12[:, 2] < 0.0, stacked) + stacked = torch.logical_and(h_dist_c23 - height_diff < height_threshold, stacked) + stacked = torch.logical_and(pos_diff_c23[:, 2] < 0.0, stacked) + + # Check gripper positions + if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0: + surface_gripper = env.scene.surface_grippers["surface_gripper"] + suction_cup_status = surface_gripper.state.view(-1) # 1: closed, 0: closing, -1: open + suction_cup_is_open = (suction_cup_status == -1).to(torch.float32) + stacked = torch.logical_and(suction_cup_is_open, stacked) + + else: + if hasattr(env.cfg, "gripper_joint_names"): + gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names) + assert len(gripper_joint_ids) == 2, "Terminations only support parallel gripper for now" + + stacked = torch.logical_and( + torch.isclose( + robot.data.joint_pos[:, gripper_joint_ids[0]], + torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device), + atol=atol, + rtol=rtol, + ), + stacked, + ) + stacked = torch.logical_and( + torch.isclose( + robot.data.joint_pos[:, gripper_joint_ids[1]], + torch.tensor(env.cfg.gripper_open_val, dtype=torch.float32).to(env.device), + atol=atol, + rtol=rtol, + ), + stacked, + ) + else: + raise ValueError("No gripper_joint_names found in environment config") + + return stacked diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/stack_ik_rel_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/stack_ik_rel_env_cfg.py new file mode 100644 index 0000000..16eb9b9 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/stack_ik_rel_env_cfg.py @@ -0,0 +1,69 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg +from isaaclab.devices.openxr.retargeters.manipulator.se3_rel_retargeter import Se3RelRetargeterCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import stack_joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaCubeStackEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3RelRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + delta_pos_scale_factor=10.0, + delta_rot_scale_factor=10.0, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + } + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/stack_task__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/stack_task__init__.py new file mode 100644 index 0000000..9e0ebd8 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/franka_stack/stack_task__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the object stack environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/__init__.py new file mode 100644 index 0000000..a9a1a78 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/__init__.py @@ -0,0 +1,55 @@ +# Copyright (c) 2024, MindRobot Project +# SPDX-License-Identifier: BSD-3-Clause +# +# Demo: Unitree H1 Humanoid Locomotion (Flat terrain) +# 直接复用 IsaacLab 官方的 H1 运动控制任务，注册为 Mindbot-Demo 前缀。 +# 参考原始注册: Isaac-Velocity-Flat-H1-v0 +# +# 运行指令 (查看): +# python scripts/environments/random_agent.py \ +# --task Mindbot-Demo-Velocity-Flat-H1-v0 \ +# --num_envs 32 +# +# 训练指令 (RSL-RL): +# python scripts/reinforcement_learning/rsl_rl/train.py \ +# --task Mindbot-Demo-Velocity-Flat-H1-v0 \ +# --num_envs 4096 --headless + +import gymnasium as gym + +## +# Register Gym environments. +## + +gym.register( + id="Mindbot-Demo-Velocity-Flat-H1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + # 直接引用 isaaclab_tasks 中的原始 cfg + "env_cfg_entry_point": ( + "isaaclab_tasks.manager_based.locomotion.velocity.config.h1" + ".flat_env_cfg:H1FlatEnvCfg" + ), + "rsl_rl_cfg_entry_point": ( + "isaaclab_tasks.manager_based.locomotion.velocity.config.h1" + ".agents.rsl_rl_ppo_cfg:H1FlatPPORunnerCfg" + ), + }, +) + +gym.register( + id="Mindbot-Demo-Velocity-Rough-H1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": ( + "isaaclab_tasks.manager_based.locomotion.velocity.config.h1" + ".rough_env_cfg:H1RoughEnvCfg" + ), + "rsl_rl_cfg_entry_point": ( + "isaaclab_tasks.manager_based.locomotion.velocity.config.h1" + ".agents.rsl_rl_ppo_cfg:H1RoughPPORunnerCfg" + ), + }, +) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/flat_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/flat_env_cfg.py new file mode 100644 index 0000000..e9b9e2a --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/flat_env_cfg.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import H1RoughEnvCfg + + +@configclass +class H1FlatEnvCfg(H1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + self.rewards.feet_air_time.weight = 1.0 + self.rewards.feet_air_time.params["threshold"] = 0.6 + + +class H1FlatEnvCfg_PLAY(H1FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/__init__.py new file mode 100644 index 0000000..6f6cad0 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the locomotion environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .curriculums import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/curriculums.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/curriculums.py new file mode 100644 index 0000000..88187a6 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/curriculums.py @@ -0,0 +1,56 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to create curriculum for the learning environment. + +The functions can be passed to the :class:`isaaclab.managers.CurriculumTermCfg` object to enable +the curriculum introduced by the function. +""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import Articulation +from isaaclab.managers import SceneEntityCfg +from isaaclab.terrains import TerrainImporter + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def terrain_levels_vel( + env: ManagerBasedRLEnv, env_ids: Sequence[int], asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Curriculum based on the distance the robot walked when commanded to move at a desired velocity. + + This term is used to increase the difficulty of the terrain when the robot walks far enough and decrease the + difficulty when the robot walks less than half of the distance required by the commanded velocity. + + .. note:: + It is only possible to use this term with the terrain type ``generator``. For further information + on different terrain types, check the :class:`isaaclab.terrains.TerrainImporter` class. + + Returns: + The mean terrain level for the given environment ids. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + terrain: TerrainImporter = env.scene.terrain + command = env.command_manager.get_command("base_velocity") + # compute the distance the robot walked + distance = torch.norm(asset.data.root_pos_w[env_ids, :2] - env.scene.env_origins[env_ids, :2], dim=1) + # robots that walked far enough progress to harder terrains + move_up = distance > terrain.cfg.terrain_generator.size[0] / 2 + # robots that walked less than half of their required distance go to simpler terrains + move_down = distance < torch.norm(command[env_ids, :2], dim=1) * env.max_episode_length_s * 0.5 + move_down *= ~move_up + # update terrain levels + terrain.update_env_origins(env_ids, move_up, move_down) + # return the mean terrain level + return torch.mean(terrain.terrain_levels.float()) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/rewards.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/rewards.py new file mode 100644 index 0000000..f804aa6 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/rewards.py @@ -0,0 +1,119 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to define rewards for the learning environment. + +The functions can be passed to the :class:`isaaclab.managers.RewardTermCfg` object to +specify the reward function and its parameters. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.envs import mdp +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import ContactSensor +from isaaclab.utils.math import quat_apply_inverse, yaw_quat + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def feet_air_time( + env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg, threshold: float +) -> torch.Tensor: + """Reward long steps taken by the feet using L2-kernel. + + This function rewards the agent for taking steps that are longer than a threshold. This helps ensure + that the robot lifts its feet off the ground and takes steps. The reward is computed as the sum of + the time for which the feet are in the air. + + If the commands are small (i.e. the agent is not supposed to take a step), then the reward is zero. + """ + # extract the used quantities (to enable type-hinting) + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + # compute the reward + first_contact = contact_sensor.compute_first_contact(env.step_dt)[:, sensor_cfg.body_ids] + last_air_time = contact_sensor.data.last_air_time[:, sensor_cfg.body_ids] + reward = torch.sum((last_air_time - threshold) * first_contact, dim=1) + # no reward for zero command + reward *= torch.norm(env.command_manager.get_command(command_name)[:, :2], dim=1) > 0.1 + return reward + + +def feet_air_time_positive_biped(env, command_name: str, threshold: float, sensor_cfg: SceneEntityCfg) -> torch.Tensor: + """Reward long steps taken by the feet for bipeds. + + This function rewards the agent for taking steps up to a specified threshold and also keep one foot at + a time in the air. + + If the commands are small (i.e. the agent is not supposed to take a step), then the reward is zero. + """ + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + # compute the reward + air_time = contact_sensor.data.current_air_time[:, sensor_cfg.body_ids] + contact_time = contact_sensor.data.current_contact_time[:, sensor_cfg.body_ids] + in_contact = contact_time > 0.0 + in_mode_time = torch.where(in_contact, contact_time, air_time) + single_stance = torch.sum(in_contact.int(), dim=1) == 1 + reward = torch.min(torch.where(single_stance.unsqueeze(-1), in_mode_time, 0.0), dim=1)[0] + reward = torch.clamp(reward, max=threshold) + # no reward for zero command + reward *= torch.norm(env.command_manager.get_command(command_name)[:, :2], dim=1) > 0.1 + return reward + + +def feet_slide(env, sensor_cfg: SceneEntityCfg, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize feet sliding. + + This function penalizes the agent for sliding its feet on the ground. The reward is computed as the + norm of the linear velocity of the feet multiplied by a binary contact sensor. This ensures that the + agent is penalized only when the feet are in contact with the ground. + """ + # Penalize feet sliding + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + contacts = contact_sensor.data.net_forces_w_history[:, :, sensor_cfg.body_ids, :].norm(dim=-1).max(dim=1)[0] > 1.0 + asset = env.scene[asset_cfg.name] + + body_vel = asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2] + reward = torch.sum(body_vel.norm(dim=-1) * contacts, dim=1) + return reward + + +def track_lin_vel_xy_yaw_frame_exp( + env, std: float, command_name: str, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Reward tracking of linear velocity commands (xy axes) in the gravity aligned + robot frame using an exponential kernel. + """ + # extract the used quantities (to enable type-hinting) + asset = env.scene[asset_cfg.name] + vel_yaw = quat_apply_inverse(yaw_quat(asset.data.root_quat_w), asset.data.root_lin_vel_w[:, :3]) + lin_vel_error = torch.sum( + torch.square(env.command_manager.get_command(command_name)[:, :2] - vel_yaw[:, :2]), dim=1 + ) + return torch.exp(-lin_vel_error / std**2) + + +def track_ang_vel_z_world_exp( + env, command_name: str, std: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Reward tracking of angular velocity commands (yaw) in world frame using exponential kernel.""" + # extract the used quantities (to enable type-hinting) + asset = env.scene[asset_cfg.name] + ang_vel_error = torch.square(env.command_manager.get_command(command_name)[:, 2] - asset.data.root_ang_vel_w[:, 2]) + return torch.exp(-ang_vel_error / std**2) + + +def stand_still_joint_deviation_l1( + env, command_name: str, command_threshold: float = 0.06, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Penalize offsets from the default joint positions when the command is very small.""" + command = env.command_manager.get_command(command_name) + # Penalize motion when command is nearly zero. + return mdp.joint_deviation_l1(env, asset_cfg) * (torch.norm(command[:, :2], dim=1) < command_threshold) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/symmetry/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/symmetry/__init__.py new file mode 100644 index 0000000..abbd6c2 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/symmetry/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Symmetry functions for the velocity tasks. + +These functions are used to augment the observations and actions of the environment. +They are specific to the velocity task and the choice of the robot. +""" diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/symmetry/anymal.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/symmetry/anymal.py new file mode 100644 index 0000000..f4197cc --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/symmetry/anymal.py @@ -0,0 +1,261 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Functions to specify the symmetry in the observation and action space for ANYmal.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +from tensordict import TensorDict + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +# specify the functions that are available for import +__all__ = ["compute_symmetric_states"] + + +@torch.no_grad() +def compute_symmetric_states( + env: ManagerBasedRLEnv, + obs: TensorDict | None = None, + actions: torch.Tensor | None = None, +): + """Augments the given observations and actions by applying symmetry transformations. + + This function creates augmented versions of the provided observations and actions by applying + four symmetrical transformations: original, left-right, front-back, and diagonal. The symmetry + transformations are beneficial for reinforcement learning tasks by providing additional + diverse data without requiring additional data collection. + + Args: + env: The environment instance. + obs: The original observation tensor dictionary. Defaults to None. + actions: The original actions tensor. Defaults to None. + + Returns: + Augmented observations and actions tensors, or None if the respective input was None. + """ + + # observations + if obs is not None: + batch_size = obs.batch_size[0] + # since we have 4 different symmetries, we need to augment the batch size by 4 + obs_aug = obs.repeat(4) + + # policy observation group + # -- original + obs_aug["policy"][:batch_size] = obs["policy"][:] + # -- left-right + obs_aug["policy"][batch_size : 2 * batch_size] = _transform_policy_obs_left_right(env.unwrapped, obs["policy"]) + # -- front-back + obs_aug["policy"][2 * batch_size : 3 * batch_size] = _transform_policy_obs_front_back( + env.unwrapped, obs["policy"] + ) + # -- diagonal + obs_aug["policy"][3 * batch_size :] = _transform_policy_obs_front_back( + env.unwrapped, obs_aug["policy"][batch_size : 2 * batch_size] + ) + else: + obs_aug = None + + # actions + if actions is not None: + batch_size = actions.shape[0] + # since we have 4 different symmetries, we need to augment the batch size by 4 + actions_aug = torch.zeros(batch_size * 4, actions.shape[1], device=actions.device) + # -- original + actions_aug[:batch_size] = actions[:] + # -- left-right + actions_aug[batch_size : 2 * batch_size] = _transform_actions_left_right(actions) + # -- front-back + actions_aug[2 * batch_size : 3 * batch_size] = _transform_actions_front_back(actions) + # -- diagonal + actions_aug[3 * batch_size :] = _transform_actions_front_back(actions_aug[batch_size : 2 * batch_size]) + else: + actions_aug = None + + return obs_aug, actions_aug + + +""" +Symmetry functions for observations. +""" + + +def _transform_policy_obs_left_right(env: ManagerBasedRLEnv, obs: torch.Tensor) -> torch.Tensor: + """Apply a left-right symmetry transformation to the observation tensor. + + This function modifies the given observation tensor by applying transformations + that represent a symmetry with respect to the left-right axis. This includes + negating certain components of the linear and angular velocities, projected gravity, + velocity commands, and flipping the joint positions, joint velocities, and last actions + for the ANYmal robot. Additionally, if height-scan data is present, it is flipped + along the relevant dimension. + + Args: + env: The environment instance from which the observation is obtained. + obs: The observation tensor to be transformed. + + Returns: + The transformed observation tensor with left-right symmetry applied. + """ + # copy observation tensor + obs = obs.clone() + device = obs.device + # lin vel + obs[:, :3] = obs[:, :3] * torch.tensor([1, -1, 1], device=device) + # ang vel + obs[:, 3:6] = obs[:, 3:6] * torch.tensor([-1, 1, -1], device=device) + # projected gravity + obs[:, 6:9] = obs[:, 6:9] * torch.tensor([1, -1, 1], device=device) + # velocity command + obs[:, 9:12] = obs[:, 9:12] * torch.tensor([1, -1, -1], device=device) + # joint pos + obs[:, 12:24] = _switch_anymal_joints_left_right(obs[:, 12:24]) + # joint vel + obs[:, 24:36] = _switch_anymal_joints_left_right(obs[:, 24:36]) + # last actions + obs[:, 36:48] = _switch_anymal_joints_left_right(obs[:, 36:48]) + + # note: this is hard-coded for grid-pattern of ordering "xy" and size (1.6, 1.0) + if "height_scan" in env.observation_manager.active_terms["policy"]: + obs[:, 48:235] = obs[:, 48:235].view(-1, 11, 17).flip(dims=[1]).view(-1, 11 * 17) + + return obs + + +def _transform_policy_obs_front_back(env: ManagerBasedRLEnv, obs: torch.Tensor) -> torch.Tensor: + """Applies a front-back symmetry transformation to the observation tensor. + + This function modifies the given observation tensor by applying transformations + that represent a symmetry with respect to the front-back axis. This includes negating + certain components of the linear and angular velocities, projected gravity, velocity commands, + and flipping the joint positions, joint velocities, and last actions for the ANYmal robot. + Additionally, if height-scan data is present, it is flipped along the relevant dimension. + + Args: + env: The environment instance from which the observation is obtained. + obs: The observation tensor to be transformed. + + Returns: + The transformed observation tensor with front-back symmetry applied. + """ + # copy observation tensor + obs = obs.clone() + device = obs.device + # lin vel + obs[:, :3] = obs[:, :3] * torch.tensor([-1, 1, 1], device=device) + # ang vel + obs[:, 3:6] = obs[:, 3:6] * torch.tensor([1, -1, -1], device=device) + # projected gravity + obs[:, 6:9] = obs[:, 6:9] * torch.tensor([-1, 1, 1], device=device) + # velocity command + obs[:, 9:12] = obs[:, 9:12] * torch.tensor([-1, 1, -1], device=device) + # joint pos + obs[:, 12:24] = _switch_anymal_joints_front_back(obs[:, 12:24]) + # joint vel + obs[:, 24:36] = _switch_anymal_joints_front_back(obs[:, 24:36]) + # last actions + obs[:, 36:48] = _switch_anymal_joints_front_back(obs[:, 36:48]) + + # note: this is hard-coded for grid-pattern of ordering "xy" and size (1.6, 1.0) + if "height_scan" in env.observation_manager.active_terms["policy"]: + obs[:, 48:235] = obs[:, 48:235].view(-1, 11, 17).flip(dims=[2]).view(-1, 11 * 17) + + return obs + + +""" +Symmetry functions for actions. +""" + + +def _transform_actions_left_right(actions: torch.Tensor) -> torch.Tensor: + """Applies a left-right symmetry transformation to the actions tensor. + + This function modifies the given actions tensor by applying transformations + that represent a symmetry with respect to the left-right axis. This includes + flipping the joint positions, joint velocities, and last actions for the + ANYmal robot. + + Args: + actions: The actions tensor to be transformed. + + Returns: + The transformed actions tensor with left-right symmetry applied. + """ + actions = actions.clone() + actions[:] = _switch_anymal_joints_left_right(actions[:]) + return actions + + +def _transform_actions_front_back(actions: torch.Tensor) -> torch.Tensor: + """Applies a front-back symmetry transformation to the actions tensor. + + This function modifies the given actions tensor by applying transformations + that represent a symmetry with respect to the front-back axis. This includes + flipping the joint positions, joint velocities, and last actions for the + ANYmal robot. + + Args: + actions: The actions tensor to be transformed. + + Returns: + The transformed actions tensor with front-back symmetry applied. + """ + actions = actions.clone() + actions[:] = _switch_anymal_joints_front_back(actions[:]) + return actions + + +""" +Helper functions for symmetry. + +In Isaac Sim, the joint ordering is as follows: +[ + 'LF_HAA', 'LH_HAA', 'RF_HAA', 'RH_HAA', + 'LF_HFE', 'LH_HFE', 'RF_HFE', 'RH_HFE', + 'LF_KFE', 'LH_KFE', 'RF_KFE', 'RH_KFE' +] + +Correspondingly, the joint ordering for the ANYmal robot is: + +* LF = left front --> [0, 4, 8] +* LH = left hind --> [1, 5, 9] +* RF = right front --> [2, 6, 10] +* RH = right hind --> [3, 7, 11] +""" + + +def _switch_anymal_joints_left_right(joint_data: torch.Tensor) -> torch.Tensor: + """Applies a left-right symmetry transformation to the joint data tensor.""" + joint_data_switched = torch.zeros_like(joint_data) + # left <-- right + joint_data_switched[..., [0, 4, 8, 1, 5, 9]] = joint_data[..., [2, 6, 10, 3, 7, 11]] + # right <-- left + joint_data_switched[..., [2, 6, 10, 3, 7, 11]] = joint_data[..., [0, 4, 8, 1, 5, 9]] + + # Flip the sign of the HAA joints + joint_data_switched[..., [0, 1, 2, 3]] *= -1.0 + + return joint_data_switched + + +def _switch_anymal_joints_front_back(joint_data: torch.Tensor) -> torch.Tensor: + """Applies a front-back symmetry transformation to the joint data tensor.""" + joint_data_switched = torch.zeros_like(joint_data) + # front <-- hind + joint_data_switched[..., [0, 4, 8, 2, 6, 10]] = joint_data[..., [1, 5, 9, 3, 7, 11]] + # hind <-- front + joint_data_switched[..., [1, 5, 9, 3, 7, 11]] = joint_data[..., [0, 4, 8, 2, 6, 10]] + + # Flip the sign of the HFE and KFE joints + joint_data_switched[..., 4:] *= -1 + + return joint_data_switched diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/terminations.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/terminations.py new file mode 100644 index 0000000..6c037d0 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/mdp/terminations.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def terrain_out_of_bounds( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), distance_buffer: float = 3.0 +) -> torch.Tensor: + """Terminate when the actor move too close to the edge of the terrain. + + If the actor moves too close to the edge of the terrain, the termination is activated. The distance + to the edge of the terrain is calculated based on the size of the terrain and the distance buffer. + """ + if env.scene.cfg.terrain.terrain_type == "plane": + # we have infinite terrain because it is a plane + return torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + elif env.scene.cfg.terrain.terrain_type == "generator": + # obtain the size of the sub-terrains + terrain_gen_cfg = env.scene.terrain.cfg.terrain_generator + grid_width, grid_length = terrain_gen_cfg.size + n_rows, n_cols = terrain_gen_cfg.num_rows, terrain_gen_cfg.num_cols + border_width = terrain_gen_cfg.border_width + # compute the size of the map + map_width = n_rows * grid_width + 2 * border_width + map_height = n_cols * grid_length + 2 * border_width + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # check if the agent is out of bounds + x_out_of_bounds = torch.abs(asset.data.root_pos_w[:, 0]) > 0.5 * map_width - distance_buffer + y_out_of_bounds = torch.abs(asset.data.root_pos_w[:, 1]) > 0.5 * map_height - distance_buffer + return torch.logical_or(x_out_of_bounds, y_out_of_bounds) + else: + raise ValueError("Received unsupported terrain type, must be either 'plane' or 'generator'.") diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/rough_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/rough_env_cfg.py new file mode 100644 index 0000000..799a7b9 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/rough_env_cfg.py @@ -0,0 +1,142 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg, RewardsCfg + +## +# Pre-defined configs +## +from isaaclab_assets import H1_MINIMAL_CFG # isort: skip + + +@configclass +class H1Rewards(RewardsCfg): + """Reward terms for the MDP.""" + + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + lin_vel_z_l2 = None + track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp, + weight=1.0, + params={"command_name": "base_velocity", "std": 0.5}, + ) + track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_world_exp, weight=1.0, params={"command_name": "base_velocity", "std": 0.5} + ) + feet_air_time = RewTerm( + func=mdp.feet_air_time_positive_biped, + weight=0.25, + params={ + "command_name": "base_velocity", + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*ankle_link"), + "threshold": 0.4, + }, + ) + feet_slide = RewTerm( + func=mdp.feet_slide, + weight=-0.25, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*ankle_link"), + "asset_cfg": SceneEntityCfg("robot", body_names=".*ankle_link"), + }, + ) + # Penalize ankle joint limits + dof_pos_limits = RewTerm( + func=mdp.joint_pos_limits, weight=-1.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_ankle")} + ) + # Penalize deviation from default of the joints that are not essential for locomotion + joint_deviation_hip = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_yaw", ".*_hip_roll"])}, + ) + joint_deviation_arms = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_shoulder_.*", ".*_elbow"])}, + ) + joint_deviation_torso = RewTerm( + func=mdp.joint_deviation_l1, weight=-0.1, params={"asset_cfg": SceneEntityCfg("robot", joint_names="torso")} + ) + + +@configclass +class H1RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + rewards: H1Rewards = H1Rewards() + + def __post_init__(self): + # post init of parent + super().__post_init__() + # Scene + self.scene.robot = H1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + if self.scene.height_scanner: + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" + + # Randomization + self.events.push_robot = None + self.events.add_base_mass = None + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.base_external_force_torque.params["asset_cfg"].body_names = [".*torso_link"] + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + self.events.base_com = None + + # Rewards + self.rewards.undesired_contacts = None + self.rewards.flat_orientation_l2.weight = -1.0 + self.rewards.dof_torques_l2.weight = 0.0 + self.rewards.action_rate_l2.weight = -0.005 + self.rewards.dof_acc_l2.weight = -1.25e-7 + + # Commands + self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + + # Terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = ".*torso_link" + + +@configclass +class H1RoughEnvCfg_PLAY(H1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + self.episode_length_s = 40.0 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + self.commands.base_velocity.ranges.heading = (0.0, 0.0) + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/velocity_task__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/velocity_task__init__.py new file mode 100644 index 0000000..7971b7c --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/demo/h1_locomotion/velocity_task__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Locomotion environments with velocity-tracking commands. + +These environments are based on the `legged_gym` environments provided by Rudin et al. + +Reference: + https://github.com/leggedrobotics/legged_gym +""" 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 35d0290..192cd45 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 @@ -7,16 +7,15 @@ import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + ## # ✅ 使用你的本地 USD 绝对路径 ## MINDBOT_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - # ===================================================== - # ✅ 你的本地 USD 文件路径 - # ===================================================== - usd_path="/home/maic/LYT/Collected_mindrobot_cd2/mindrobot_cd2.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindbot/mindbot_cd2.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=1.0, 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 e9403a3..709dd0b 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 @@ -18,6 +18,7 @@ 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 @@ -38,6 +39,7 @@ 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( @@ -81,12 +83,12 @@ class MindRobotTeleopSceneCfg(InteractiveSceneCfg): 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="/home/maic/LYT/maic_usd_assets/twinlab/Collected_lab2/lab.usd", - ), - ) + # 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( @@ -184,6 +186,22 @@ class MindRobotTeleopTerminationsCfg: time_out = DoneTerm(func=mdp.time_out, time_out=True) +# ===================================================================== +# Events Configuration +# ===================================================================== + + +@configclass +class MindRobotTeleopEventsCfg: + """Reset events for teleoperation: R键重置时将场景恢复到初始状态。""" + + # 重置所有场景实体（机器人、物体）到其 init_state 定义的初始位置/姿态 + reset_scene = EventTerm( + func=mdp.reset_scene_to_default, + mode="reset", + ) + + # ===================================================================== # Main Environment Configuration # ===================================================================== @@ -210,11 +228,11 @@ class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): # MDP settings terminations: MindRobotTeleopTerminationsCfg = MindRobotTeleopTerminationsCfg() + events: MindRobotTeleopEventsCfg = MindRobotTeleopEventsCfg() # Unused managers commands: EmptyCfg = EmptyCfg() rewards: EmptyCfg = EmptyCfg() - events: EmptyCfg = EmptyCfg() curriculum: EmptyCfg = EmptyCfg() # XR configuration for hand tracking (if needed) diff --git a/source/mindbot/mindbot/tasks/manager_based/rl/centrifuge/centrifuge_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/rl/centrifuge/centrifuge_env_cfg.py index ffe86b9..806800d 100644 --- a/source/mindbot/mindbot/tasks/manager_based/rl/centrifuge/centrifuge_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/rl/centrifuge/centrifuge_env_cfg.py @@ -35,6 +35,8 @@ from . import mdp from mindbot.robot.mindbot import MINDBOT_CFG +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + # ====== 其他物体配置 ====== from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg from isaaclab.sim.schemas.schemas_cfg import ( @@ -52,7 +54,7 @@ TABLE_CFG = RigidObjectCfg( ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/table/Table_C.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/table/Table_C.usd", rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled=True, solver_position_iteration_count=32, @@ -78,7 +80,7 @@ LID_CFG = RigidObjectCfg( ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/assets/Collected_equipment001/Equipment/tube1.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/assets/Collected_equipment001/Equipment/tube1.usd", # scale=(0.2, 0.2, 0.2), rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled=True, @@ -133,7 +135,7 @@ LID_CFG = RigidObjectCfg( CENTRIFUGE_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/centrifuge/centrifuge.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/centrifuge/centrifuge.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=1.0, @@ -196,7 +198,7 @@ CENTRIFUGE_CFG = ArticulationCfg( ROOM_CFG = AssetBaseCfg( prim_path="{ENV_REGEX_NS}/Room", spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/twinlab/MIC_sim-3.0/244_140/room.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/twinlab/MIC_sim-3.0/244_140/room.usd", ), ) diff --git a/source/mindbot/mindbot/tasks/manager_based/rl/mindbot/mindbot_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/rl/mindbot/mindbot_env_cfg.py index d91617a..47d34e6 100644 --- a/source/mindbot/mindbot/tasks/manager_based/rl/mindbot/mindbot_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/rl/mindbot/mindbot_env_cfg.py @@ -34,6 +34,8 @@ from . import mdp from mindbot.robot.mindbot import MINDBOT_CFG +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + # ====== 其他物体配置 ====== from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg from isaaclab.sim.schemas.schemas_cfg import ( @@ -51,7 +53,7 @@ TABLE_CFG = RigidObjectCfg( ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/table/Table_C.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/table/Table_C.usd", rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled=True, solver_position_iteration_count=32, @@ -75,7 +77,7 @@ LID_CFG = RigidObjectCfg( ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/ultrasound/ultrasound_lid.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/ultrasound/ultrasound_lid.usd", # scale=(0.2, 0.2, 0.2), rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled=True, @@ -99,7 +101,7 @@ LID_CFG = RigidObjectCfg( ULTRASOUND_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/ultrasound/ultrasound.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/ultrasound/ultrasound.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=1.0 ), diff --git a/source/mindbot/mindbot/tasks/manager_based/rl/pull/pull_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/rl/pull/pull_env_cfg.py index 5367158..fe44b79 100644 --- a/source/mindbot/mindbot/tasks/manager_based/rl/pull/pull_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/rl/pull/pull_env_cfg.py @@ -34,6 +34,8 @@ from . import mdp from mindbot.robot.mindbot import MINDBOT_CFG +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + # ====== 其他物体配置 ====== from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg from isaaclab.sim.schemas.schemas_cfg import ( @@ -51,7 +53,7 @@ TABLE_CFG = RigidObjectCfg( ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/table/Table_C.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/table/Table_C.usd", rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled=True, solver_position_iteration_count=32, @@ -68,7 +70,7 @@ TABLE_CFG = RigidObjectCfg( DRYINGBOX_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/assets/Collected_Equipment_BB_0B/Equipment/Equipment_BB_13.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/assets/Collected_Equipment_BB_0B/Equipment/Equipment_BB_13.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=1.0 ), @@ -105,7 +107,7 @@ LID_CFG = RigidObjectCfg( ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/ultrasound/ultrasound_lid.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/ultrasound/ultrasound_lid.usd", # scale=(0.2, 0.2, 0.2), rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled=True, @@ -129,7 +131,7 @@ LID_CFG = RigidObjectCfg( ULTRASOUND_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/ultrasound/ultrasound.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/ultrasound/ultrasound.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=1.0 ), diff --git a/source/mindbot/mindbot/tasks/manager_based/rl/pullUltrasoundLidUp/mindbot_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/rl/pullUltrasoundLidUp/mindbot_env_cfg.py index 90037ed..9cd014b 100644 --- a/source/mindbot/mindbot/tasks/manager_based/rl/pullUltrasoundLidUp/mindbot_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/rl/pullUltrasoundLidUp/mindbot_env_cfg.py @@ -35,6 +35,8 @@ from . import mdp from mindbot.robot.mindbot import MINDBOT_CFG +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + # ====== 其他物体配置 ====== from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg from isaaclab.sim.schemas.schemas_cfg import ( @@ -52,7 +54,7 @@ TABLE_CFG = RigidObjectCfg( ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/table/Table_C.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/table/Table_C.usd", rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled=True, solver_position_iteration_count=32, @@ -76,7 +78,7 @@ LID_CFG = RigidObjectCfg( ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/ultrasound/ultrasound_lid.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/ultrasound/ultrasound_lid.usd", # scale=(0.2, 0.2, 0.2), rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled=True, @@ -106,7 +108,7 @@ LID_CFG = RigidObjectCfg( ROOM_CFG = AssetBaseCfg( prim_path="{ENV_REGEX_NS}/Room", spawn=UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/twinlab/Collected_lab2/lab.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/twinlab/Collected_lab2/lab.usd", ), ) @@ -143,7 +145,7 @@ ROOM_CFG = AssetBaseCfg( ULTRASOUND_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path="/home/maic/LYT/maic_usd_assets/equipment/ultrasound/ultrasound.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/ultrasound/ultrasound.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=1.0 ), diff --git a/source/mindbot/mindbot/utils/__init__.py b/source/mindbot/mindbot/utils/__init__.py new file mode 100644 index 0000000..1e6b30f --- /dev/null +++ b/source/mindbot/mindbot/utils/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2024, MindRobot Project +# SPDX-License-Identifier: BSD-3-Clause + +"""Utility functions and constants for MindBot project.""" + +from .assets import MINDBOT_ASSETS_DIR diff --git a/source/mindbot/mindbot/utils/assets.py b/source/mindbot/mindbot/utils/assets.py new file mode 100644 index 0000000..9aff58d --- /dev/null +++ b/source/mindbot/mindbot/utils/assets.py @@ -0,0 +1,24 @@ +# Copyright (c) 2024, MindRobot Project +# SPDX-License-Identifier: BSD-3-Clause + +"""Asset path utilities for MindBot project. + +Usage: + from mindbot.utils.assets import MINDBOT_ASSETS_DIR + + usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/table/Table_C.usd" + +To override on a different machine, set the environment variable before running: + export MINDBOT_ASSETS_DIR="/your/custom/path/to/assets" +""" + +import os + +## +# MindBot 资产根目录 +# 优先读取环境变量 MINDBOT_ASSETS_DIR，若未设置则使用默认路径 +## +MINDBOT_ASSETS_DIR: str = os.environ.get( + "MINDBOT_ASSETS_DIR", + "/home/tangger/DataDisk/maic_usd_assets_moudle", +)