diff --git a/deps/cloudxr/docker-compose.mindbot.patch.yaml b/deps/cloudxr/docker-compose.mindbot.patch.yaml new file mode 100644 index 0000000..0e1b320 --- /dev/null +++ b/deps/cloudxr/docker-compose.mindbot.patch.yaml @@ -0,0 +1,9 @@ +services: + isaac-lab-base: + volumes: + - type: bind + source: /home/maic/xh/mindbot + target: /workspace/mindbot + - type: bind + source: /home/maic/xh/maic_usd_assets_moudle + target: /workspace/maic_usd_assets_moudle diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index 26776ec..6e73595 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -83,14 +83,15 @@ from isaaclab.devices.teleop_device_factory import create_teleop_device from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import TerminationTermCfg as DoneTerm -import isaaclab_tasks # noqa: F401 +# import isaaclab_tasks # noqa: F401 import mindbot.tasks # noqa: F401 from isaaclab_tasks.manager_based.manipulation.lift import mdp from isaaclab_tasks.utils import parse_env_cfg if args_cli.enable_pinocchio: - import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 - import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + # import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 + # import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + import mindbot.tasks.manager_based.il.pick_place # import logger logger = logging.getLogger(__name__) @@ -288,6 +289,11 @@ def main() -> None: if teleoperation_active: # process actions actions = action.repeat(env.num_envs, 1) + # 自动补齐 action 维度（如有 scale=0 的 hold 关节） + expected_dim = env.single_action_space.shape[0] + if actions.shape[1] < expected_dim: + pad = torch.zeros(env.num_envs, expected_dim - actions.shape[1], device=actions.device) + actions = torch.cat([actions, pad], dim=1) # apply actions env.step(actions) else: diff --git a/source/mindbot/mindbot/assets/centrigue.py b/source/mindbot/mindbot/assets/centrifuge.py similarity index 70% rename from source/mindbot/mindbot/assets/centrigue.py rename to source/mindbot/mindbot/assets/centrifuge.py index f084d39..3cf6685 100644 --- a/source/mindbot/mindbot/assets/centrigue.py +++ b/source/mindbot/mindbot/assets/centrifuge.py @@ -8,16 +8,17 @@ from isaaclab.sim.schemas.schemas_cfg import ( CollisionPropertiesCfg, ) from mindbot.utils.assets import MINDBOT_ASSETS_DIR +import math -CENTIRGUE_CFG = ArticulationCfg( +CENTRIFUGE_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path=f"{MINDBOT_ASSETS_DIR}/devices/centrigue/centrigue.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/devices/centrifuge/centrifuge.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=1.0 ), articulation_props=sim_utils.ArticulationRootPropertiesCfg( - fix_root_link=False, + fix_root_link=True, # 由 USD 内的 FixedJoint_ 负责固定底座 enabled_self_collisions=False, solver_position_iteration_count=32, solver_velocity_iteration_count=16, @@ -25,20 +26,18 @@ CENTIRGUE_CFG = ArticulationCfg( ), ), init_state=ArticulationCfg.InitialStateCfg( - pos=[1.95, -0.45, 0.9], + pos=[1.95, -0.45, 0.9], rot=[-0.7071, 0.0, 0.0, 0.7071], joint_pos={ - "RevoluteJoint":-1.74, + "RevoluteJoint": math.radians(-100.0), # -100°，盖子打开位置 }, ), - # actuators={} actuators={ "lid_open": ImplicitActuatorCfg( - # ".*" 表示匹配该USD文件内的所有关节（无论是轮子、屏幕转轴还是其他） joint_names_expr=["RevoluteJoint"], - stiffness=10000.0, - damping=1000.0, - effort_limit_sim=10000.0, + stiffness=200, # 覆盖 USD 的 100，足够对抗重力保持开盖 + damping=20, # 阻尼同步加大，防止振荡 + effort_limit_sim=1000.0, velocity_limit_sim=100.0, ), }, diff --git a/source/mindbot/mindbot/assets/table.py b/source/mindbot/mindbot/assets/table.py index d5a4192..0a97f07 100644 --- a/source/mindbot/mindbot/assets/table.py +++ b/source/mindbot/mindbot/assets/table.py @@ -1,9 +1,15 @@ -from isaaclab.assets import AssetBaseCfg +# SPDX-FileCopyrightText: Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg, RigidObjectCfg from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg from isaaclab.sim.schemas.schemas_cfg import CollisionPropertiesCfg from mindbot.utils.assets import MINDBOT_ASSETS_DIR +# 静态桌子（无重力，仅碰撞）：用于 ManagerBasedEnv 场景固定摆设 +# 通过 AssetBaseCfg 在 SceneEntityCfg 中使用，不受物理仿真影响 TABLE_CFG = AssetBaseCfg( # prim_path="{ENV_REGEX_NS}/Table", init_state=AssetBaseCfg.InitialStateCfg( @@ -13,5 +19,24 @@ TABLE_CFG = AssetBaseCfg( spawn=UsdFileCfg( usd_path=f"{MINDBOT_ASSETS_DIR}/sences/Table_C_coll/Table_C.usd", collision_props=CollisionPropertiesCfg(collision_enabled=True), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), ), -) \ No newline at end of file +) + +# 动态桌子（有重力）：受物理仿真影响，需通过 RigidObject(cfg=TABLE_RIGID_CFG) 实例化 +# disable_gravity=False 表示启用重力；桌子质量由 USD 内部定义或 PhysX 自动估算 +TABLE_RIGID_CFG = RigidObjectCfg( + # prim_path="{ENV_REGEX_NS}/Table", + init_state=RigidObjectCfg.InitialStateCfg( + pos=[1.95, -0.3, 0.005], + rot=[0.7071, 0.0, 0.0, -0.7071], + ), + spawn=sim_utils.UsdFileCfg( + usd_path=f"{MINDBOT_ASSETS_DIR}/sences/Table_C_coll/Table_C.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, # 启用重力 + max_depenetration_velocity=1.0, + ), + collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True), + ), +) diff --git a/source/mindbot/mindbot/robot/gr1t2.py b/source/mindbot/mindbot/robot/gr1t2.py new file mode 100644 index 0000000..c05e416 --- /dev/null +++ b/source/mindbot/mindbot/robot/gr1t2.py @@ -0,0 +1,163 @@ +# 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 + +"""Configuration for the Fourier Robots. + +The following configuration parameters are available: + +* :obj:`GR1T2_CFG`: The GR1T2 humanoid. +* :obj:`GR1T2_HIGH_PD_CFG`: The GR1T2 humanoid configured with high PD gains on upper body joints for pick-place manipulation tasks. + +Reference: https://www.fftai.com/products-gr1 +""" + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + + +GR1T2_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=( + f"{ISAAC_NUCLEUS_DIR}/Robots/FourierIntelligence/GR-1/GR1T2_fourier_hand_6dof/GR1T2_fourier_hand_6dof.usd" + ), + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.95), + joint_pos={".*": 0.0}, + joint_vel={".*": 0.0}, + ), + actuators={ + "head": ImplicitActuatorCfg( + joint_names_expr=[ + "head_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "trunk": ImplicitActuatorCfg( + joint_names_expr=[ + "waist_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "legs": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_hip_.*", + ".*_knee_.*", + ".*_ankle_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "right-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "right_shoulder_.*", + "right_elbow_.*", + "right_wrist_.*", + ], + effort_limit=torch.inf, + velocity_limit=torch.inf, + stiffness=None, + damping=None, + armature=0.0, + ), + "left-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "left_shoulder_.*", + "left_elbow_.*", + "left_wrist_.*", + ], + effort_limit=torch.inf, + velocity_limit=torch.inf, + stiffness=None, + damping=None, + armature=0.0, + ), + "right-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "R_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "left-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "L_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + }, +) +"""Configuration for the GR1T2 Humanoid robot.""" + + +GR1T2_HIGH_PD_CFG = GR1T2_CFG.replace( + actuators={ + "trunk": ImplicitActuatorCfg( + joint_names_expr=["waist_.*"], + effort_limit=None, + velocity_limit=None, + stiffness=4400, + damping=40.0, + armature=0.01, + ), + "right-arm": ImplicitActuatorCfg( + joint_names_expr=["right_shoulder_.*", "right_elbow_.*", "right_wrist_.*"], + stiffness=4400.0, + damping=40.0, + armature=0.01, + ), + "left-arm": ImplicitActuatorCfg( + joint_names_expr=["left_shoulder_.*", "left_elbow_.*", "left_wrist_.*"], + stiffness=4400.0, + damping=40.0, + armature=0.01, + ), + "right-hand": ImplicitActuatorCfg( + joint_names_expr=["R_.*"], + stiffness=None, + damping=None, + ), + "left-hand": ImplicitActuatorCfg( + joint_names_expr=["L_.*"], + stiffness=None, + damping=None, + ), + }, +) +"""Configuration for the GR1T2 Humanoid robot configured for with high PD gains for pick-place manipulation tasks.""" diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/__init__.py new file mode 100644 index 0000000..7f2bd7d --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/__init__.py @@ -0,0 +1,58 @@ +# 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 + +import gymnasium as gym + +from . import agents + +gym.register( + id="Isaac-PickPlace-GR1T2-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.pickplace_gr1t2_env_cfg:PickPlaceGR1T2EnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-NutPour-GR1T2-Pink-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.nutpour_gr1t2_pink_ik_env_cfg:NutPourGR1T2PinkIKEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_image_nut_pouring.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-ExhaustPipe-GR1T2-Pink-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.exhaustpipe_gr1t2_pink_ik_env_cfg:ExhaustPipeGR1T2PinkIKEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_image_exhaust_pipe.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.pickplace_gr1t2_waist_enabled_env_cfg:PickPlaceGR1T2WaistEnabledEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-PickPlace-G1-InspireFTP-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.pickplace_unitree_g1_inspire_hand_env_cfg:PickPlaceG1InspireFTPEnvCfg", + "robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json", + }, + disable_env_checker=True, +) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_image_exhaust_pipe.json b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_image_exhaust_pipe.json new file mode 100644 index 0000000..5af2a9f --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_image_exhaust_pipe.json @@ -0,0 +1,220 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_image_gr1_exhaust_pipe", + "validate": false, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 20, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 500, + "env": null, + "additional_envs": null, + "render": false, + "render_video": false, + "rollout": { + "enabled": false + } + }, + "train": { + "data": null, + "num_data_workers": 4, + "hdf5_cache_mode": "low_dim", + "hdf5_use_swmr": true, + "hdf5_load_next_obs": false, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "hdf5_validation_filter_key": null, + "seq_length": 10, + "pad_seq_length": true, + "frame_stack": 1, + "pad_frame_stack": true, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 16, + "num_epochs": 600, + "seed": 101 + }, + "algo": { + "optim_params": { + "policy": { + "optimizer_type": "adam", + "learning_rate": { + "initial": 0.0001, + "decay_factor": 0.1, + "epoch_schedule": [], + "scheduler_type": "multistep" + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [], + "gaussian": { + "enabled": false, + "fixed_std": false, + "init_std": 0.1, + "min_std": 0.01, + "std_activation": "softplus", + "low_noise_eval": true + }, + "gmm": { + "enabled": true, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": false, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "rnn": { + "enabled": true, + "horizon": 10, + "hidden_dim": 1000, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + }, + "transformer": { + "enabled": false, + "context_length": 10, + "embed_dim": 512, + "num_layers": 6, + "num_heads": 8, + "emb_dropout": 0.1, + "attn_dropout": 0.1, + "block_output_dropout": 0.1, + "sinusoidal_embedding": false, + "activation": "gelu", + "supervise_all_steps": false, + "nn_parameter_for_timesteps": true + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "left_eef_pos", + "left_eef_quat", + "right_eef_pos", + "right_eef_quat", + "hand_joint_state" + ], + "rgb": [ + "robot_pov_cam" + ], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": "CropRandomizer", + "obs_randomizer_kwargs": { + "crop_height": 144, + "crop_width": 236, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + } + } + } +} diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_image_nut_pouring.json b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_image_nut_pouring.json new file mode 100644 index 0000000..dbe527d --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_image_nut_pouring.json @@ -0,0 +1,220 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_image_gr1_nut_pouring", + "validate": false, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 20, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 500, + "env": null, + "additional_envs": null, + "render": false, + "render_video": false, + "rollout": { + "enabled": false + } + }, + "train": { + "data": null, + "num_data_workers": 4, + "hdf5_cache_mode": "low_dim", + "hdf5_use_swmr": true, + "hdf5_load_next_obs": false, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "hdf5_validation_filter_key": null, + "seq_length": 10, + "pad_seq_length": true, + "frame_stack": 1, + "pad_frame_stack": true, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 16, + "num_epochs": 600, + "seed": 101 + }, + "algo": { + "optim_params": { + "policy": { + "optimizer_type": "adam", + "learning_rate": { + "initial": 0.0001, + "decay_factor": 0.1, + "epoch_schedule": [], + "scheduler_type": "multistep" + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [], + "gaussian": { + "enabled": false, + "fixed_std": false, + "init_std": 0.1, + "min_std": 0.01, + "std_activation": "softplus", + "low_noise_eval": true + }, + "gmm": { + "enabled": true, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": false, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "rnn": { + "enabled": true, + "horizon": 10, + "hidden_dim": 1000, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + }, + "transformer": { + "enabled": false, + "context_length": 10, + "embed_dim": 512, + "num_layers": 6, + "num_heads": 8, + "emb_dropout": 0.1, + "attn_dropout": 0.1, + "block_output_dropout": 0.1, + "sinusoidal_embedding": false, + "activation": "gelu", + "supervise_all_steps": false, + "nn_parameter_for_timesteps": true + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "left_eef_pos", + "left_eef_quat", + "right_eef_pos", + "right_eef_quat", + "hand_joint_state" + ], + "rgb": [ + "robot_pov_cam" + ], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": "CropRandomizer", + "obs_randomizer_kwargs": { + "crop_height": 144, + "crop_width": 236, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + } + } + } +} diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_low_dim.json b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_low_dim.json new file mode 100644 index 0000000..d2e0f8f --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/agents/robomimic/bc_rnn_low_dim.json @@ -0,0 +1,117 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_low_dim_gr1t2", + "validate": false, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 100, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 100, + "env": null, + "additional_envs": null, + "render": false, + "render_video": false, + "rollout": { + "enabled": false + } + }, + "train": { + "data": null, + "num_data_workers": 4, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "hdf5_validation_filter_key": null, + "seq_length": 10, + "dataset_keys": [ + "actions" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 2000, + "seed": 101 + }, + "algo": { + "optim_params": { + "policy": { + "optimizer_type": "adam", + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [], + "scheduler_type": "multistep" + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [], + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "rnn": { + "enabled": true, + "horizon": 10, + "hidden_dim": 400, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + }, + "transformer": { + "enabled": false, + "context_length": 10, + "embed_dim": 512, + "num_layers": 6, + "num_heads": 8, + "emb_dropout": 0.1, + "attn_dropout": 0.1, + "block_output_dropout": 0.1, + "sinusoidal_embedding": false, + "activation": "gelu", + "supervise_all_steps": false, + "nn_parameter_for_timesteps": true + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "left_eef_pos", + "left_eef_quat", + "right_eef_pos", + "right_eef_quat", + "hand_joint_state", + "object" + ], + "rgb": [], + "depth": [], + "scan": [] + } + } + } +} diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/exhaustpipe_gr1t2_base_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/exhaustpipe_gr1t2_base_env_cfg.py new file mode 100644 index 0000000..6e14d2e --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/exhaustpipe_gr1t2_base_env_cfg.py @@ -0,0 +1,336 @@ +# Copyright (c) 2025-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 + +import tempfile +from dataclasses import MISSING + +import torch + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg, SceneEntityCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg + +# from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +from . import mdp + +from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the GR1T2 Exhaust Pipe Base Scene.""" + + # Table + table = AssetBaseCfg( + prim_path="/World/envs/env_.*/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, 0.0], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/exhaust_pipe_task/exhaust_pipe_assets/table.usd", + scale=(1.0, 1.0, 1.3), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + blue_exhaust_pipe = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlueExhaustPipe", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.04904, 0.31, 1.2590], rot=[0, 0, 1.0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/exhaust_pipe_task/exhaust_pipe_assets/blue_exhaust_pipe.usd", + scale=(0.5, 0.5, 1.5), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + blue_sorting_bin = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlueSortingBin", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.16605, 0.39, 0.98634], rot=[1.0, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/exhaust_pipe_task/exhaust_pipe_assets/blue_sorting_bin.usd", + scale=(1.0, 1.7, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + black_sorting_bin = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlackSortingBin", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.40132, 0.39, 0.98634], rot=[1.0, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/exhaust_pipe_task/exhaust_pipe_assets/black_sorting_bin.usd", + scale=(1.0, 1.7, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Humanoid robot w/ arms higher + robot: ArticulationCfg = GR1T2_CFG.replace( + prim_path="/World/envs/env_.*/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0, 0, 0.93), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + # right-arm + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": 0.0, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_pitch_joint": -1.5708, + "right_wrist_yaw_joint": 0.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + # left-arm + "left_shoulder_pitch_joint": -0.10933163, + "left_shoulder_roll_joint": 0.43292055, + "left_shoulder_yaw_joint": -0.15983289, + "left_elbow_pitch_joint": -1.48233023, + "left_wrist_yaw_joint": 0.2359135, + "left_wrist_roll_joint": 0.26184522, + "left_wrist_pitch_joint": 0.00830735, + # right hand + "R_index_intermediate_joint": 0.0, + "R_index_proximal_joint": 0.0, + "R_middle_intermediate_joint": 0.0, + "R_middle_proximal_joint": 0.0, + "R_pinky_intermediate_joint": 0.0, + "R_pinky_proximal_joint": 0.0, + "R_ring_intermediate_joint": 0.0, + "R_ring_proximal_joint": 0.0, + "R_thumb_distal_joint": 0.0, + "R_thumb_proximal_pitch_joint": 0.0, + "R_thumb_proximal_yaw_joint": -1.57, + # left hand + "L_index_intermediate_joint": 0.0, + "L_index_proximal_joint": 0.0, + "L_middle_intermediate_joint": 0.0, + "L_middle_proximal_joint": 0.0, + "L_pinky_intermediate_joint": 0.0, + "L_pinky_proximal_joint": 0.0, + "L_ring_intermediate_joint": 0.0, + "L_ring_proximal_joint": 0.0, + "L_thumb_distal_joint": 0.0, + "L_thumb_proximal_pitch_joint": 0.0, + "L_thumb_proximal_yaw_joint": -1.57, + # -- + "head_.*": 0.0, + "waist_.*": 0.0, + ".*_hip_.*": 0.0, + ".*_knee_.*": 0.0, + ".*_ankle_.*": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) + + # Set table view camera + robot_pov_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/RobotPOVCam", + update_period=0.0, + height=160, + width=256, + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg(focal_length=18.15, clipping_range=(0.1, 2)), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.12, 1.85418), rot=(-0.17246, 0.98502, 0.0, 0.0), convention="ros"), + ) + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + gr1_action: ActionTermCfg = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) + + robot_pov_cam = ObsTerm( + func=mdp.image, + params={"sensor_cfg": SceneEntityCfg("robot_pov_cam"), "data_type": "rgb", "normalize": False}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + blue_exhaust_pipe_dropped = DoneTerm( + func=mdp.root_height_below_minimum, + params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("blue_exhaust_pipe")}, + ) + + success = DoneTerm(func=mdp.task_done_exhaust_pipe) + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_blue_exhaust_pipe = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": [-0.01, 0.01], + "y": [-0.01, 0.01], + }, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("blue_exhaust_pipe"), + }, + ) + + +@configclass +class ExhaustPipeGR1T2BaseEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the GR1T2 environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events = EventCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + # Idle action to hold robot in default pose + # Action format: [left arm pos (3), left arm quat (4), right arm pos (3), + # right arm quat (4), left/right hand joint pos (22)] + idle_action = torch.tensor( + [ + [ + -0.2909, + 0.2778, + 1.1247, + 0.5253, + 0.5747, + -0.4160, + 0.4699, + 0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ] + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 5 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 100 + self.sim.render_interval = 2 + + # Set settings for camera rendering + self.num_rerenders_on_reset = 3 + self.sim.render.antialiasing_mode = "DLAA" # Use DLAA for higher quality rendering + + # List of image observations in policy observations + self.image_obs_list = ["robot_pov_cam"] diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py new file mode 100644 index 0000000..66ebfca --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py @@ -0,0 +1,155 @@ +# Copyright (c) 2025-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 pink.tasks import DampingTask, FrameTask + +import carb + +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg +from isaaclab.devices import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters import GR1T2RetargeterCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.pick_place.exhaustpipe_gr1t2_base_env_cfg import ( + ExhaustPipeGR1T2BaseEnvCfg, +) + + +@configclass +class ExhaustPipeGR1T2PinkIKEnvCfg(ExhaustPipeGR1T2BaseEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.actions.gr1_action = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "left_wrist_yaw_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "right_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + ], + hand_joint_names=[ + "L_index_proximal_joint", + "L_middle_proximal_joint", + "L_pinky_proximal_joint", + "L_ring_proximal_joint", + "L_thumb_proximal_yaw_joint", + "R_index_proximal_joint", + "R_middle_proximal_joint", + "R_pinky_proximal_joint", + "R_ring_proximal_joint", + "R_thumb_proximal_yaw_joint", + "L_index_intermediate_joint", + "L_middle_intermediate_joint", + "L_pinky_intermediate_joint", + "L_ring_intermediate_joint", + "L_thumb_proximal_pitch_joint", + "R_index_intermediate_joint", + "R_middle_intermediate_joint", + "R_pinky_intermediate_joint", + "R_ring_intermediate_joint", + "R_thumb_proximal_pitch_joint", + "L_thumb_distal_joint", + "R_thumb_distal_joint", + ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_link", + }, + # the robot in the sim scene we are controlling + asset_name="robot", + # Configuration for the IK controller + # The frames names are the ones present in the URDF file + # The urdf has to be generated from the USD that is being used in the scene + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=22, + show_ik_warnings=False, + # Determines whether Pink IK solver will fail due to a joint limit violation + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + FrameTask( + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + FrameTask( + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.2, + lm_damping=1, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + ), + ], + fixed_input_tasks=[], + xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), + ), + ) + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.gr1_action.controller.urdf_path = temp_urdf_output_path + self.actions.gr1_action.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, + sim_device=self.sim.device, + hand_joint_names=self.actions.gr1_action.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/__init__.py new file mode 100644 index 0000000..555bfb7 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/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 lift environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .pick_place_events import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/observations.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/observations.py new file mode 100644 index 0000000..01e52e7 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/observations.py @@ -0,0 +1,86 @@ +# 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 + +import torch + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_obs( + env: ManagerBasedRLEnv, + left_eef_link_name: str, + right_eef_link_name: str, +) -> torch.Tensor: + """ + Object observations (in world frame): + object pos, + object quat, + left_eef to object, + right_eef_to object, + """ + + body_pos_w = env.scene["robot"].data.body_pos_w + left_eef_idx = env.scene["robot"].data.body_names.index(left_eef_link_name) + right_eef_idx = env.scene["robot"].data.body_names.index(right_eef_link_name) + left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins + right_eef_pos = body_pos_w[:, right_eef_idx] - env.scene.env_origins + + object_pos = env.scene["object"].data.root_pos_w - env.scene.env_origins + object_quat = env.scene["object"].data.root_quat_w + + left_eef_to_object = object_pos - left_eef_pos + right_eef_to_object = object_pos - right_eef_pos + + return torch.cat( + ( + object_pos, + object_quat, + left_eef_to_object, + right_eef_to_object, + ), + dim=1, + ) + + +def get_eef_pos(env: ManagerBasedRLEnv, link_name: str) -> torch.Tensor: + body_pos_w = env.scene["robot"].data.body_pos_w + left_eef_idx = env.scene["robot"].data.body_names.index(link_name) + left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins + + return left_eef_pos + + +def get_eef_quat(env: ManagerBasedRLEnv, link_name: str) -> torch.Tensor: + body_quat_w = env.scene["robot"].data.body_quat_w + left_eef_idx = env.scene["robot"].data.body_names.index(link_name) + left_eef_quat = body_quat_w[:, left_eef_idx] + + return left_eef_quat + + +def get_robot_joint_state( + env: ManagerBasedRLEnv, + joint_names: list[str], +) -> torch.Tensor: + # hand_joint_names is a list of regex, use find_joints + indexes, _ = env.scene["robot"].find_joints(joint_names) + indexes = torch.tensor(indexes, dtype=torch.long) + robot_joint_states = env.scene["robot"].data.joint_pos[:, indexes] + + return robot_joint_states + + +def get_all_robot_link_state( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + body_pos_w = env.scene["robot"].data.body_link_state_w[:, :, :] + all_robot_link_pos = body_pos_w + + return all_robot_link_pos diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/pick_place_events.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/pick_place_events.py new file mode 100644 index 0000000..ca1fd94 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/pick_place_events.py @@ -0,0 +1,96 @@ +# Copyright (c) 2025-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 + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def reset_object_poses_nut_pour( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + pose_range: dict[str, tuple[float, float]], + sorting_beaker_cfg: SceneEntityCfg = SceneEntityCfg("sorting_beaker"), + factory_nut_cfg: SceneEntityCfg = SceneEntityCfg("factory_nut"), + sorting_bowl_cfg: SceneEntityCfg = SceneEntityCfg("sorting_bowl"), + sorting_scale_cfg: SceneEntityCfg = SceneEntityCfg("sorting_scale"), +): + """Reset the asset root states to a random position and orientation uniformly within the given ranges. + + Args: + env: The RL environment instance. + env_ids: The environment IDs to reset the object poses for. + sorting_beaker_cfg: The configuration for the sorting beaker asset. + factory_nut_cfg: The configuration for the factory nut asset. + sorting_bowl_cfg: The configuration for the sorting bowl asset. + sorting_scale_cfg: The configuration for the sorting scale asset. + pose_range: The dictionary of pose ranges for the objects. Keys are + ``x``, ``y``, ``z``, ``roll``, ``pitch``, and ``yaw``. + """ + # extract the used quantities (to enable type-hinting) + sorting_beaker = env.scene[sorting_beaker_cfg.name] + factory_nut = env.scene[factory_nut_cfg.name] + sorting_bowl = env.scene[sorting_bowl_cfg.name] + sorting_scale = env.scene[sorting_scale_cfg.name] + + # get default root state + sorting_beaker_root_states = sorting_beaker.data.default_root_state[env_ids].clone() + factory_nut_root_states = factory_nut.data.default_root_state[env_ids].clone() + sorting_bowl_root_states = sorting_bowl.data.default_root_state[env_ids].clone() + sorting_scale_root_states = sorting_scale.data.default_root_state[env_ids].clone() + + # get pose ranges + range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + ranges = torch.tensor(range_list, device=sorting_beaker.device) + + # randomize sorting beaker and factory nut together + rand_samples = math_utils.sample_uniform( + ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=sorting_beaker.device + ) + orientations_delta = math_utils.quat_from_euler_xyz(rand_samples[:, 3], rand_samples[:, 4], rand_samples[:, 5]) + positions_sorting_beaker = ( + sorting_beaker_root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + ) + positions_factory_nut = factory_nut_root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + orientations_sorting_beaker = math_utils.quat_mul(sorting_beaker_root_states[:, 3:7], orientations_delta) + orientations_factory_nut = math_utils.quat_mul(factory_nut_root_states[:, 3:7], orientations_delta) + + # randomize sorting bowl + rand_samples = math_utils.sample_uniform( + ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=sorting_beaker.device + ) + orientations_delta = math_utils.quat_from_euler_xyz(rand_samples[:, 3], rand_samples[:, 4], rand_samples[:, 5]) + positions_sorting_bowl = sorting_bowl_root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + orientations_sorting_bowl = math_utils.quat_mul(sorting_bowl_root_states[:, 3:7], orientations_delta) + + # randomize scorting scale + rand_samples = math_utils.sample_uniform( + ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=sorting_beaker.device + ) + orientations_delta = math_utils.quat_from_euler_xyz(rand_samples[:, 3], rand_samples[:, 4], rand_samples[:, 5]) + positions_sorting_scale = sorting_scale_root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + orientations_sorting_scale = math_utils.quat_mul(sorting_scale_root_states[:, 3:7], orientations_delta) + + # set into the physics simulation + sorting_beaker.write_root_pose_to_sim( + torch.cat([positions_sorting_beaker, orientations_sorting_beaker], dim=-1), env_ids=env_ids + ) + factory_nut.write_root_pose_to_sim( + torch.cat([positions_factory_nut, orientations_factory_nut], dim=-1), env_ids=env_ids + ) + sorting_bowl.write_root_pose_to_sim( + torch.cat([positions_sorting_bowl, orientations_sorting_bowl], dim=-1), env_ids=env_ids + ) + sorting_scale.write_root_pose_to_sim( + torch.cat([positions_sorting_scale, orientations_sorting_scale], dim=-1), env_ids=env_ids + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/terminations.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/terminations.py new file mode 100644 index 0000000..6252b9c --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/mdp/terminations.py @@ -0,0 +1,222 @@ +# 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 RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def task_done_pick_place( + env: ManagerBasedRLEnv, + task_link_name: str = "", + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), + right_wrist_max_x: float = 0.26, + min_x: float = 0.40, + max_x: float = 0.85, + min_y: float = 0.35, + max_y: float = 0.60, + max_height: float = 1.10, + min_vel: float = 0.20, +) -> torch.Tensor: + """Determine if the object placement task is complete. + + This function checks whether all success conditions for the task have been met: + 1. object is within the target x/y range + 2. object is below a minimum height + 3. object velocity is below threshold + 4. Right robot wrist is retracted back towards body (past a given x pos threshold) + + Args: + env: The RL environment instance. + object_cfg: Configuration for the object entity. + right_wrist_max_x: Maximum x position of the right wrist for task completion. + min_x: Minimum x position of the object for task completion. + max_x: Maximum x position of the object for task completion. + min_y: Minimum y position of the object for task completion. + max_y: Maximum y position of the object for task completion. + max_height: Maximum height (z position) of the object for task completion. + min_vel: Minimum velocity magnitude of the object for task completion. + + Returns: + Boolean tensor indicating which environments have completed the task. + """ + if task_link_name == "": + raise ValueError("task_link_name must be provided to task_done_pick_place") + + # Get object entity from the scene + object: RigidObject = env.scene[object_cfg.name] + + # Extract wheel position relative to environment origin + object_x = object.data.root_pos_w[:, 0] - env.scene.env_origins[:, 0] + object_y = object.data.root_pos_w[:, 1] - env.scene.env_origins[:, 1] + object_height = object.data.root_pos_w[:, 2] - env.scene.env_origins[:, 2] + object_vel = torch.abs(object.data.root_vel_w) + + # Get right wrist position relative to environment origin + robot_body_pos_w = env.scene["robot"].data.body_pos_w + right_eef_idx = env.scene["robot"].data.body_names.index(task_link_name) + right_wrist_x = robot_body_pos_w[:, right_eef_idx, 0] - env.scene.env_origins[:, 0] + + # Check all success conditions and combine with logical AND + done = object_x < max_x + done = torch.logical_and(done, object_x > min_x) + done = torch.logical_and(done, object_y < max_y) + done = torch.logical_and(done, object_y > min_y) + done = torch.logical_and(done, object_height < max_height) + done = torch.logical_and(done, right_wrist_x < right_wrist_max_x) + done = torch.logical_and(done, object_vel[:, 0] < min_vel) + done = torch.logical_and(done, object_vel[:, 1] < min_vel) + done = torch.logical_and(done, object_vel[:, 2] < min_vel) + + return done + + +def task_done_nut_pour( + env: ManagerBasedRLEnv, + sorting_scale_cfg: SceneEntityCfg = SceneEntityCfg("sorting_scale"), + sorting_bowl_cfg: SceneEntityCfg = SceneEntityCfg("sorting_bowl"), + sorting_beaker_cfg: SceneEntityCfg = SceneEntityCfg("sorting_beaker"), + factory_nut_cfg: SceneEntityCfg = SceneEntityCfg("factory_nut"), + sorting_bin_cfg: SceneEntityCfg = SceneEntityCfg("black_sorting_bin"), + max_bowl_to_scale_x: float = 0.055, + max_bowl_to_scale_y: float = 0.055, + max_bowl_to_scale_z: float = 0.025, + max_nut_to_bowl_x: float = 0.050, + max_nut_to_bowl_y: float = 0.050, + max_nut_to_bowl_z: float = 0.019, + max_beaker_to_bin_x: float = 0.08, + max_beaker_to_bin_y: float = 0.12, + max_beaker_to_bin_z: float = 0.07, +) -> torch.Tensor: + """Determine if the nut pouring task is complete. + + This function checks whether all success conditions for the task have been met: + 1. The factory nut is in the sorting bowl + 2. The sorting beaker is in the sorting bin + 3. The sorting bowl is placed on the sorting scale + + Args: + env: The RL environment instance. + sorting_scale_cfg: Configuration for the sorting scale entity. + sorting_bowl_cfg: Configuration for the sorting bowl entity. + sorting_beaker_cfg: Configuration for the sorting beaker entity. + factory_nut_cfg: Configuration for the factory nut entity. + sorting_bin_cfg: Configuration for the sorting bin entity. + max_bowl_to_scale_x: Maximum x position of the sorting bowl relative to the sorting scale for task completion. + max_bowl_to_scale_y: Maximum y position of the sorting bowl relative to the sorting scale for task completion. + max_bowl_to_scale_z: Maximum z position of the sorting bowl relative to the sorting scale for task completion. + max_nut_to_bowl_x: Maximum x position of the factory nut relative to the sorting bowl for task completion. + max_nut_to_bowl_y: Maximum y position of the factory nut relative to the sorting bowl for task completion. + max_nut_to_bowl_z: Maximum z position of the factory nut relative to the sorting bowl for task completion. + max_beaker_to_bin_x: Maximum x position of the sorting beaker relative to the sorting bin for task completion. + max_beaker_to_bin_y: Maximum y position of the sorting beaker relative to the sorting bin for task completion. + max_beaker_to_bin_z: Maximum z position of the sorting beaker relative to the sorting bin for task completion. + + Returns: + Boolean tensor indicating which environments have completed the task. + """ + # Get object entities from the scene + sorting_scale: RigidObject = env.scene[sorting_scale_cfg.name] + sorting_bowl: RigidObject = env.scene[sorting_bowl_cfg.name] + factory_nut: RigidObject = env.scene[factory_nut_cfg.name] + sorting_beaker: RigidObject = env.scene[sorting_beaker_cfg.name] + sorting_bin: RigidObject = env.scene[sorting_bin_cfg.name] + + # Get positions relative to environment origin + scale_pos = sorting_scale.data.root_pos_w - env.scene.env_origins + bowl_pos = sorting_bowl.data.root_pos_w - env.scene.env_origins + sorting_beaker_pos = sorting_beaker.data.root_pos_w - env.scene.env_origins + nut_pos = factory_nut.data.root_pos_w - env.scene.env_origins + bin_pos = sorting_bin.data.root_pos_w - env.scene.env_origins + + # nut to bowl + nut_to_bowl_x = torch.abs(nut_pos[:, 0] - bowl_pos[:, 0]) + nut_to_bowl_y = torch.abs(nut_pos[:, 1] - bowl_pos[:, 1]) + nut_to_bowl_z = nut_pos[:, 2] - bowl_pos[:, 2] + + # bowl to scale + bowl_to_scale_x = torch.abs(bowl_pos[:, 0] - scale_pos[:, 0]) + bowl_to_scale_y = torch.abs(bowl_pos[:, 1] - scale_pos[:, 1]) + bowl_to_scale_z = bowl_pos[:, 2] - scale_pos[:, 2] + + # beaker to bin + beaker_to_bin_x = torch.abs(sorting_beaker_pos[:, 0] - bin_pos[:, 0]) + beaker_to_bin_y = torch.abs(sorting_beaker_pos[:, 1] - bin_pos[:, 1]) + beaker_to_bin_z = sorting_beaker_pos[:, 2] - bin_pos[:, 2] + + done = nut_to_bowl_x < max_nut_to_bowl_x + done = torch.logical_and(done, nut_to_bowl_y < max_nut_to_bowl_y) + done = torch.logical_and(done, nut_to_bowl_z < max_nut_to_bowl_z) + done = torch.logical_and(done, bowl_to_scale_x < max_bowl_to_scale_x) + done = torch.logical_and(done, bowl_to_scale_y < max_bowl_to_scale_y) + done = torch.logical_and(done, bowl_to_scale_z < max_bowl_to_scale_z) + done = torch.logical_and(done, beaker_to_bin_x < max_beaker_to_bin_x) + done = torch.logical_and(done, beaker_to_bin_y < max_beaker_to_bin_y) + done = torch.logical_and(done, beaker_to_bin_z < max_beaker_to_bin_z) + + return done + + +def task_done_exhaust_pipe( + env: ManagerBasedRLEnv, + blue_exhaust_pipe_cfg: SceneEntityCfg = SceneEntityCfg("blue_exhaust_pipe"), + blue_sorting_bin_cfg: SceneEntityCfg = SceneEntityCfg("blue_sorting_bin"), + max_blue_exhaust_to_bin_x: float = 0.085, + max_blue_exhaust_to_bin_y: float = 0.200, + min_blue_exhaust_to_bin_y: float = -0.090, + max_blue_exhaust_to_bin_z: float = 0.070, +) -> torch.Tensor: + """Determine if the exhaust pipe task is complete. + + This function checks whether all success conditions for the task have been met: + 1. The blue exhaust pipe is placed in the correct position + + Args: + env: The RL environment instance. + blue_exhaust_pipe_cfg: Configuration for the blue exhaust pipe entity. + blue_sorting_bin_cfg: Configuration for the blue sorting bin entity. + max_blue_exhaust_to_bin_x: Maximum x position of the blue exhaust pipe + relative to the blue sorting bin for task completion. + max_blue_exhaust_to_bin_y: Maximum y position of the blue exhaust pipe + relative to the blue sorting bin for task completion. + max_blue_exhaust_to_bin_z: Maximum z position of the blue exhaust pipe + relative to the blue sorting bin for task completion. + + Returns: + Boolean tensor indicating which environments have completed the task. + """ + # Get object entities from the scene + blue_exhaust_pipe: RigidObject = env.scene[blue_exhaust_pipe_cfg.name] + blue_sorting_bin: RigidObject = env.scene[blue_sorting_bin_cfg.name] + + # Get positions relative to environment origin + blue_exhaust_pipe_pos = blue_exhaust_pipe.data.root_pos_w - env.scene.env_origins + blue_sorting_bin_pos = blue_sorting_bin.data.root_pos_w - env.scene.env_origins + + # blue exhaust to bin + blue_exhaust_to_bin_x = torch.abs(blue_exhaust_pipe_pos[:, 0] - blue_sorting_bin_pos[:, 0]) + blue_exhaust_to_bin_y = blue_exhaust_pipe_pos[:, 1] - blue_sorting_bin_pos[:, 1] + blue_exhaust_to_bin_z = blue_exhaust_pipe_pos[:, 2] - blue_sorting_bin_pos[:, 2] + + done = blue_exhaust_to_bin_x < max_blue_exhaust_to_bin_x + done = torch.logical_and(done, blue_exhaust_to_bin_y < max_blue_exhaust_to_bin_y) + done = torch.logical_and(done, blue_exhaust_to_bin_y > min_blue_exhaust_to_bin_y) + done = torch.logical_and(done, blue_exhaust_to_bin_z < max_blue_exhaust_to_bin_z) + + return done diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/nutpour_gr1t2_base_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/nutpour_gr1t2_base_env_cfg.py new file mode 100644 index 0000000..01caf58 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/nutpour_gr1t2_base_env_cfg.py @@ -0,0 +1,371 @@ +# Copyright (c) 2025-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 + +import tempfile +from dataclasses import MISSING + +import torch + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg, SceneEntityCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg + +# from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +from . import mdp + +from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the GR1T2 Nut Pour Base Scene.""" + + # Table + table = AssetBaseCfg( + prim_path="/World/envs/env_.*/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, 0.0], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/table.usd", + scale=(1.0, 1.0, 1.3), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + sorting_scale = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/SortingScale", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.22236, 0.56, 0.9859], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_scale.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + sorting_bowl = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/SortingBowl", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.02779, 0.43007, 0.9860], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_bowl_yellow.usd", + scale=(1.0, 1.0, 1.5), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005), + ), + ) + + sorting_beaker = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/SortingBeaker", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.13739, 0.45793, 0.9861], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_beaker_red.usd", + scale=(0.45, 0.45, 1.3), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + factory_nut = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/FactoryNut", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.13739, 0.45793, 0.9995], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/factory_m16_nut_green.usd", + scale=(0.5, 0.5, 0.5), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005), + ), + ) + + black_sorting_bin = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BlackSortingBin", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.32688, 0.46793, 0.98634], rot=[1.0, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/sorting_bin_blue.usd", + scale=(0.75, 1.0, 1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + robot: ArticulationCfg = GR1T2_CFG.replace( + prim_path="/World/envs/env_.*/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0, 0, 0.93), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + # right-arm + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": 0.0, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_pitch_joint": -1.5708, + "right_wrist_yaw_joint": 0.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + # left-arm + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.0, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_pitch_joint": -1.5708, + "left_wrist_yaw_joint": 0.0, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + # right hand + "R_index_intermediate_joint": 0.0, + "R_index_proximal_joint": 0.0, + "R_middle_intermediate_joint": 0.0, + "R_middle_proximal_joint": 0.0, + "R_pinky_intermediate_joint": 0.0, + "R_pinky_proximal_joint": 0.0, + "R_ring_intermediate_joint": 0.0, + "R_ring_proximal_joint": 0.0, + "R_thumb_distal_joint": 0.0, + "R_thumb_proximal_pitch_joint": 0.0, + "R_thumb_proximal_yaw_joint": -1.57, + # left hand + "L_index_intermediate_joint": 0.0, + "L_index_proximal_joint": 0.0, + "L_middle_intermediate_joint": 0.0, + "L_middle_proximal_joint": 0.0, + "L_pinky_intermediate_joint": 0.0, + "L_pinky_proximal_joint": 0.0, + "L_ring_intermediate_joint": 0.0, + "L_ring_proximal_joint": 0.0, + "L_thumb_distal_joint": 0.0, + "L_thumb_proximal_pitch_joint": 0.0, + "L_thumb_proximal_yaw_joint": -1.57, + # -- + "head_.*": 0.0, + "waist_.*": 0.0, + ".*_hip_.*": 0.0, + ".*_knee_.*": 0.0, + ".*_ankle_.*": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) + + # Set table view camera + robot_pov_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/RobotPOVCam", + update_period=0.0, + height=160, + width=256, + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg(focal_length=18.15, clipping_range=(0.1, 2)), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.12, 1.67675), rot=(-0.19848, 0.9801, 0.0, 0.0), convention="ros"), + ) + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + gr1_action: ActionTermCfg = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) + + robot_pov_cam = ObsTerm( + func=mdp.image, + params={"sensor_cfg": SceneEntityCfg("robot_pov_cam"), "data_type": "rgb", "normalize": False}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + sorting_bowl_dropped = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("sorting_bowl")} + ) + sorting_beaker_dropped = DoneTerm( + func=mdp.root_height_below_minimum, + params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("sorting_beaker")}, + ) + factory_nut_dropped = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("factory_nut")} + ) + + success = DoneTerm(func=mdp.task_done_nut_pour) + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + set_factory_nut_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("factory_nut"), + "mass_distribution_params": (0.2, 0.2), + "operation": "abs", + }, + ) + + reset_object = EventTerm( + func=mdp.reset_object_poses_nut_pour, + mode="reset", + params={ + "pose_range": { + "x": [-0.01, 0.01], + "y": [-0.01, 0.01], + }, + }, + ) + + +@configclass +class NutPourGR1T2BaseEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the GR1T2 environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events = EventCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + # Idle action to hold robot in default pose + # Action format: [left arm pos (3), left arm quat (4), right arm pos (3), + # right arm quat (4), left/right hand joint pos (22)] + idle_action = torch.tensor( + [ + [ + -0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ] + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 5 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 100 + self.sim.render_interval = 2 + + # Set settings for camera rendering + self.num_rerenders_on_reset = 3 + self.sim.render.antialiasing_mode = "DLAA" # Use DLAA for higher quality rendering + + # List of image observations in policy observations + self.image_obs_list = ["robot_pov_cam"] diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py new file mode 100644 index 0000000..818fba1 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py @@ -0,0 +1,153 @@ +# Copyright (c) 2025-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 pink.tasks import DampingTask, FrameTask + +import carb + +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg +from isaaclab.devices import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters import GR1T2RetargeterCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.pick_place.nutpour_gr1t2_base_env_cfg import NutPourGR1T2BaseEnvCfg + + +@configclass +class NutPourGR1T2PinkIKEnvCfg(NutPourGR1T2BaseEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.actions.gr1_action = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "left_wrist_yaw_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "right_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + ], + hand_joint_names=[ + "L_index_proximal_joint", + "L_middle_proximal_joint", + "L_pinky_proximal_joint", + "L_ring_proximal_joint", + "L_thumb_proximal_yaw_joint", + "R_index_proximal_joint", + "R_middle_proximal_joint", + "R_pinky_proximal_joint", + "R_ring_proximal_joint", + "R_thumb_proximal_yaw_joint", + "L_index_intermediate_joint", + "L_middle_intermediate_joint", + "L_pinky_intermediate_joint", + "L_ring_intermediate_joint", + "L_thumb_proximal_pitch_joint", + "R_index_intermediate_joint", + "R_middle_intermediate_joint", + "R_pinky_intermediate_joint", + "R_ring_intermediate_joint", + "R_thumb_proximal_pitch_joint", + "L_thumb_distal_joint", + "R_thumb_distal_joint", + ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_link", + }, + # the robot in the sim scene we are controlling + asset_name="robot", + # Configuration for the IK controller + # The frames names are the ones present in the URDF file + # The urdf has to be generated from the USD that is being used in the scene + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=22, + show_ik_warnings=False, + # Determines whether Pink IK solver will fail due to a joint limit violation + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + FrameTask( + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + FrameTask( + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.2, + lm_damping=1, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + ), + ], + fixed_input_tasks=[], + xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), + ), + ) + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.gr1_action.controller.urdf_path = temp_urdf_output_path + self.actions.gr1_action.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, + sim_device=self.sim.device, + hand_joint_names=self.actions.gr1_action.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_env_cfg.py new file mode 100644 index 0000000..64ee21e --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_env_cfg.py @@ -0,0 +1,490 @@ +# 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 + +import tempfile + +import torch +from pink.tasks import DampingTask, FrameTask + +import carb + +import isaaclab.controllers.utils as ControllerUtils +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import ManusViveCfg, OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2RetargeterCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + +from . import mdp + +from mindbot.robot.gr1t2 import GR1T2_HIGH_PD_CFG # isort: skip +from mindbot.assets.table import TABLE_CFG +from mindbot.assets.centrifuge import CENTRIFUGE_CFG +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the GR1T2 Pick Place Base Scene.""" + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, 0.0], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + # table = AssetBaseCfg( + # prim_path="/World/envs/env_.*/Table", + # init_state=AssetBaseCfg.InitialStateCfg(pos=[0.6,-0.5,0.0],rot=[0.7071,0,0,0.7071]), + # spawn=UsdFileCfg( + # usd_path=f"{MINDBOT_ASSETS_DIR}/sences/Table_C_coll/Table_C.usd", + # rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + # ), + # ) + + centrifuge01 = CENTRIFUGE_CFG.replace( + prim_path="{ENV_REGEX_NS}/Centrifuge01", + init_state=ArticulationCfg.InitialStateCfg( + pos=[0.0, 0.55, 1.0], + rot=[1.0, 0.0, 0.0, 0.0], + joint_pos={"RevoluteJoint": -1.7453}, # -100° + ), + ) + # centrifuge02 = CENTRIFUGE_CFG.replace( + # prim_path="{ENV_REGEX_NS}/Centrifuge02", + # init_state=ArticulationCfg.InitialStateCfg( + # pos=[0.5, -0.3, 0.85], + # rot=[-0.7071, 0.0, 0.0, 0.7071], + # joint_pos={"RevoluteJoint": -1.74}, + # ), + # ) + + rack= RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Rack", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.3,0.4,0.9996],rot=[1,0,0,0]), + spawn=UsdFileCfg( + # usd_path=f"{MINDBOT_ASSETS_DIR}/laboratory_equipment/Test_Tube_Rack (2)/tube.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/laboratory_equipment/Test_Tube_Rack (2)/Test_Tube_Rack_AA_01.usdc", + scale=(1.0,1.0,1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + tube= RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Tube", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.31574,0.32615,1.02],rot=[1,0,0,0]), + spawn=UsdFileCfg( + # usd_path=f"{MINDBOT_ASSETS_DIR}/laboratory_equipment/Test_Tube_Rack (2)/tube.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/laboratory_equipment/Test_Tube/Test_Tube_AA_01.usdc", + scale=(0.8,0.8,1.0), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + # rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + # init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.45, 0.45, 0.9996], rot=[1, 0, 0, 0]), + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.65, 0.45, 0.9996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Humanoid robot configured for pick-place manipulation tasks + robot: ArticulationCfg = GR1T2_HIGH_PD_CFG.replace( + prim_path="/World/envs/env_.*/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0, 0, 0.93), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + # right-arm + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": 0.0, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_pitch_joint": -1.5708, + "right_wrist_yaw_joint": 0.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + # left-arm + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.0, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_pitch_joint": -1.5708, + "left_wrist_yaw_joint": 0.0, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + # -- + "head_.*": 0.0, + "waist_.*": 0.0, + ".*_hip_.*": 0.0, + ".*_knee_.*": 0.0, + ".*_ankle_.*": 0.0, + "R_.*": 0.0, + "L_.*": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "left_wrist_yaw_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "right_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + ], + hand_joint_names=[ + "L_index_proximal_joint", + "L_middle_proximal_joint", + "L_pinky_proximal_joint", + "L_ring_proximal_joint", + "L_thumb_proximal_yaw_joint", + "R_index_proximal_joint", + "R_middle_proximal_joint", + "R_pinky_proximal_joint", + "R_ring_proximal_joint", + "R_thumb_proximal_yaw_joint", + "L_index_intermediate_joint", + "L_middle_intermediate_joint", + "L_pinky_intermediate_joint", + "L_ring_intermediate_joint", + "L_thumb_proximal_pitch_joint", + "R_index_intermediate_joint", + "R_middle_intermediate_joint", + "R_pinky_intermediate_joint", + "R_ring_intermediate_joint", + "R_thumb_proximal_pitch_joint", + "L_thumb_distal_joint", + "R_thumb_distal_joint", + ], + target_eef_link_names={ + "left_wrist": "left_hand_pitch_link", + "right_wrist": "right_hand_pitch_link", + }, + # the robot in the sim scene we are controlling + asset_name="robot", + # Configuration for the IK controller + # The frames names are the ones present in the URDF file + # The urdf has to be generated from the USD that is being used in the scene + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=22, + show_ik_warnings=False, + # Determines whether Pink IK solver will fail due to a joint limit violation + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + FrameTask( + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=12, # dampening for solver for step jumps + gain=0.5, + ), + FrameTask( + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=12, # dampening for solver for step jumps + gain=0.5, + ), + DampingTask( + cost=0.5, # [cost] * [s] / [rad] + ), + NullSpacePostureTask( + cost=0.5, + lm_damping=1, + controlled_frames=[ + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + ), + ], + fixed_input_tasks=[], + xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), + ), + ) + + # 每步强制把离心机盖子 PD 目标设为 -100°，防止重力或初始化问题导致盖子下落 + centrifuge_lid_hold = base_mdp.JointPositionActionCfg( + asset_name="centrifuge01", + joint_names=["RevoluteJoint"], + scale=0.0, # RL/遥操作不控制此关节 + offset=-1.7453, # -100°，每步作为 PD 目标写入 + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) + + object = ObsTerm( + func=mdp.object_obs, + params={"left_eef_link_name": "left_hand_roll_link", "right_eef_link_name": "right_hand_roll_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=mdp.task_done_pick_place, params={"task_link_name": "right_hand_roll_link"}) + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_centrifuge_joints = EventTerm( + func=base_mdp.reset_joints_by_offset, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("centrifuge01"), + "position_range": (0.0, 0.0), + "velocity_range": (0.0, 0.0), + }, + ) + # reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + # reset_object = EventTerm( + # func=mdp.reset_root_state_uniform, + # mode="reset", + # params={ + # "pose_range": { + # "x": [-0.01, 0.01], + # "y": [-0.01, 0.01], + # }, + # "velocity_range": {}, + # "asset_cfg": SceneEntityCfg("object"), + # }, + # ) + + +@configclass +class PickPlaceGR1T2EnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the GR1T2 environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events = EventCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + # Idle action to hold robot in default pose + # Action format: [left arm pos (3), left arm quat (4), right arm pos (3), right arm quat (4), + # left hand joint pos (11), right hand joint pos (11), centrifuge lid (1)] + idle_action = torch.tensor( + [ + -0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, # centrifuge lid（scale=0，值无关紧要） + ] + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 6 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 120 # 120Hz + self.sim.render_interval = 2 + + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.actions.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "manusvive": ManusViveCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py new file mode 100644 index 0000000..23ed8d9 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -0,0 +1,87 @@ +# 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 + +import tempfile + +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2RetargeterCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.utils import configclass + +from .pickplace_gr1t2_env_cfg import ActionsCfg, EventCfg, ObjectTableSceneCfg, ObservationsCfg, TerminationsCfg + + +@configclass +class PickPlaceGR1T2WaistEnabledEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the GR1T2 environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events = EventCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + # OpenXR hand tracking has 26 joints per hand + NUM_OPENXR_HAND_JOINTS = 26 + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 6 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 120 # 120Hz + self.sim.render_interval = 2 + + # Add waist joint to pink_ik_cfg + waist_joint_names = ["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"] + for joint_name in waist_joint_names: + self.actions.upper_body_ik.pink_controlled_joint_names.append(joint_name) + + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.actions.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py new file mode 100644 index 0000000..85af79e --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py @@ -0,0 +1,415 @@ +# 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 +import tempfile + +import torch +from pink.tasks import FrameTask + +import carb + +import isaaclab.controllers.utils as ControllerUtils +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import ManusViveCfg, OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.inspire.g1_upper_body_retargeter import UnitreeG1RetargeterCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.schemas.schemas_cfg import MassPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + +from . import mdp + +from isaaclab_assets.robots.unitree import G1_INSPIRE_FTP_CFG # isort: skip + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the Unitree G1 Inspire Hand Pick Place Base Scene.""" + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, 0.0], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.45, 0.9996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=MassPropertiesCfg( + mass=0.05, + ), + ), + ) + + # Humanoid robot w/ arms higher + robot: ArticulationCfg = G1_INSPIRE_FTP_CFG.replace( + prim_path="/World/envs/env_.*/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0, 0, 1.0), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + # right-arm + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": 0.0, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_joint": 0.0, + "right_wrist_yaw_joint": 0.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + # left-arm + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.0, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_joint": 0.0, + "left_wrist_yaw_joint": 0.0, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + # -- + "waist_.*": 0.0, + ".*_hip_.*": 0.0, + ".*_knee_.*": 0.0, + ".*_ankle_.*": 0.0, + # -- left/right hand + ".*_thumb_.*": 0.0, + ".*_index_.*": 0.0, + ".*_middle_.*": 0.0, + ".*_ring_.*": 0.0, + ".*_pinky_.*": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + pink_ik_cfg = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_yaw_joint", + ".*_wrist_roll_joint", + ".*_wrist_pitch_joint", + ], + hand_joint_names=[ + # All the drive and mimic joints, total 24 joints + "L_index_proximal_joint", + "L_middle_proximal_joint", + "L_pinky_proximal_joint", + "L_ring_proximal_joint", + "L_thumb_proximal_yaw_joint", + "R_index_proximal_joint", + "R_middle_proximal_joint", + "R_pinky_proximal_joint", + "R_ring_proximal_joint", + "R_thumb_proximal_yaw_joint", + "L_index_intermediate_joint", + "L_middle_intermediate_joint", + "L_pinky_intermediate_joint", + "L_ring_intermediate_joint", + "L_thumb_proximal_pitch_joint", + "R_index_intermediate_joint", + "R_middle_intermediate_joint", + "R_pinky_intermediate_joint", + "R_ring_intermediate_joint", + "R_thumb_proximal_pitch_joint", + "L_thumb_intermediate_joint", + "R_thumb_intermediate_joint", + "L_thumb_distal_joint", + "R_thumb_distal_joint", + ], + target_eef_link_names={ + "left_wrist": "left_wrist_yaw_link", + "right_wrist": "right_wrist_yaw_link", + }, + # the robot in the sim scene we are controlling + asset_name="robot", + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="pelvis", + num_hand_joints=24, + show_ik_warnings=False, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + FrameTask( + "g1_29dof_rev_1_0_left_wrist_yaw_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=2.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + FrameTask( + "g1_29dof_rev_1_0_right_wrist_yaw_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=2.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + NullSpacePostureTask( + cost=0.5, + lm_damping=1, + controlled_frames=[ + "g1_29dof_rev_1_0_left_wrist_yaw_link", + "g1_29dof_rev_1_0_right_wrist_yaw_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + gain=0.3, + ), + ], + fixed_input_tasks=[], + xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), + ), + enable_gravity_compensation=False, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_wrist_yaw_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_wrist_yaw_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_wrist_yaw_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_wrist_yaw_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + + object = ObsTerm( + func=mdp.object_obs, + params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=mdp.task_done_pick_place, params={"task_link_name": "right_wrist_yaw_link"}) + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_object = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": [-0.01, 0.01], + "y": [-0.01, 0.01], + }, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("object"), + }, + ) + + +@configclass +class PickPlaceG1InspireFTPEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the GR1T2 environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events = EventCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + # Idle action to hold robot in default pose + # Action format: [left arm pos (3), left arm quat (4), right arm pos (3), right arm quat (4), + # left hand joint pos (12), right hand joint pos (12)] + idle_action = torch.tensor( + [ + # 14 hand joints for EEF control + -0.1487, + 0.2038, + 1.0952, + 0.707, + 0.0, + 0.0, + 0.707, + 0.1487, + 0.2038, + 1.0952, + 0.707, + 0.0, + 0.0, + 0.707, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 6 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 120 # 120Hz + self.sim.render_interval = 2 + + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.pink_ik_cfg.controller.urdf_path = temp_urdf_output_path + self.actions.pink_ik_cfg.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + UnitreeG1RetargeterCfg( + enable_visualization=True, + # number of joints in both hands + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + # Please confirm that self.actions.pink_ik_cfg.hand_joint_names is + # consistent with robot.joint_names[-24:] + # The order of the joints does matter as it will be used for + # converting pink_ik actions to final control actions in IsaacLab. + hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "manusvive": ManusViveCfg( + retargeters=[ + UnitreeG1RetargeterCfg( + enable_visualization=True, + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + }, + ) diff --git a/source/mindbot/mindbot/utils/assets.py b/source/mindbot/mindbot/utils/assets.py index b8cd9f6..c68fc90 100644 --- a/source/mindbot/mindbot/utils/assets.py +++ b/source/mindbot/mindbot/utils/assets.py @@ -20,7 +20,8 @@ import os ## MINDBOT_ASSETS_DIR: str = os.environ.get( "MINDBOT_ASSETS_DIR", - "/home/tangger/LYT/maic_usd_assets_moudle", + # "/home/tangger/LYT/maic_usd_assets_moudle", # "/home/maic/xh/maic_usd_assets_moudle" + "/workspace/maic_usd_assets_moudle" # "/home/maic/LYT/maic_usd_assets_moudle", ) diff --git a/source/mindbot/mindbot/utils/test.sence.py b/source/mindbot/mindbot/utils/test.sence.py new file mode 100644 index 0000000..6213100 --- /dev/null +++ b/source/mindbot/mindbot/utils/test.sence.py @@ -0,0 +1,95 @@ +# SPDX-FileCopyrightText: Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# SPDX-License-Identifier: BSD-3-Clause + +"""Test scene: spawn room (static), table (rigid body with gravity), centrifuge (articulation with gravity). + +.. code-block:: bash + + # Usage + ~/IsaacLab/isaaclab.sh -p source/mindbot/mindbot/utils/test.sence.py + +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Test scene with physics assets.") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, RigidObject + +from mindbot.assets.lab import ROOM_CFG +from mindbot.assets.table import TABLE_RIGID_CFG +from mindbot.assets.centrifuge import CENTRIFUGE_CFG + + +def design_scene() -> dict: + """搭建场景：地面、灯光、静态房间、动态桌子（刚体）、离心机（关节体）。""" + # 地面 + cfg_ground = sim_utils.GroundPlaneCfg() + cfg_ground.func("/World/defaultGroundPlane", cfg_ground) + + # 远光灯 + cfg_light = sim_utils.DistantLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + cfg_light.func("/World/lightDistant", cfg_light, translation=(1, 0, 10)) + + # 静态房间（无物理，直接 spawn USD prim，不受重力影响） + room_cfg = ROOM_CFG.replace(prim_path="/World/Room") + room_cfg.spawn.func("/World/Room", room_cfg.spawn) + + # ✅ 动态桌子（RigidObject，有重力） + # 关键：必须用 RigidObject 实例化，spawn.func() 只放 USD prim，不会应用 rigid_props + # TABLE_RIGID_CFG 包含 rigid_props(disable_gravity=False) + table_cfg = TABLE_RIGID_CFG.replace(prim_path="/World/Table") + table = RigidObject(cfg=table_cfg) + + # ✅ 离心机（Articulation，有重力） + # 关键：必须用 Articulation 实例化，才能应用 rigid_props 和 init_state + # CENTRIFUGE_CFG 包含 rigid_props(disable_gravity=False) 和 fix_root_link=False + centrifuge_cfg = CENTRIFUGE_CFG.replace(prim_path="/World/Centrifuge") + centrifuge = Articulation(cfg=centrifuge_cfg) + + return {"table": table, "centrifuge": centrifuge} + + +def main(): + """主函数。""" + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + + # 搭建场景（必须在 sim.reset() 之前完成） + scene_entities = design_scene() + table: RigidObject = scene_entities["table"] + centrifuge: Articulation = scene_entities["centrifuge"] + + # 启动仿真（触发 PhysX 物理初始化） + sim.reset() + + # 将 init_state（位置/旋转/关节角）写入仿真，并初始化内部缓存 + table.reset() + centrifuge.reset() + + print("[INFO]: Setup complete. Table and centrifuge have gravity enabled.") + + # 仿真主循环 + while simulation_app.is_running(): + # 推进物理步（重力在此生效） + sim.step() + # 更新资产内部状态缓存（读取最新物理状态，供后续查询用） + table.update(sim_cfg.dt) + centrifuge.update(sim_cfg.dt) + + +if __name__ == "__main__": + main() + simulation_app.close()