GR1T2抓取试管环境
This commit is contained in:
9
deps/cloudxr/docker-compose.mindbot.patch.yaml
vendored
Normal file
9
deps/cloudxr/docker-compose.mindbot.patch.yaml
vendored
Normal file
@@ -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
|
||||
@@ -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:
|
||||
|
||||
@@ -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,
|
||||
),
|
||||
},
|
||||
@@ -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),
|
||||
),
|
||||
)
|
||||
)
|
||||
|
||||
# 动态桌子（有重力）：受物理仿真影响，需通过 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),
|
||||
),
|
||||
)
|
||||
|
||||
163
source/mindbot/mindbot/robot/gr1t2.py
Normal file
163
source/mindbot/mindbot/robot/gr1t2.py
Normal file
@@ -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."""
|
||||
@@ -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,
|
||||
)
|
||||
@@ -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": {}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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": {}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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": []
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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"]
|
||||
@@ -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,
|
||||
),
|
||||
}
|
||||
)
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
)
|
||||
@@ -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
|
||||
@@ -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"]
|
||||
@@ -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,
|
||||
),
|
||||
}
|
||||
)
|
||||
@@ -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,
|
||||
),
|
||||
}
|
||||
)
|
||||
@@ -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,
|
||||
),
|
||||
}
|
||||
)
|
||||
@@ -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,
|
||||
),
|
||||
},
|
||||
)
|
||||
@@ -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",
|
||||
)
|
||||
|
||||
95
source/mindbot/mindbot/utils/test.sence.py
Normal file
95
source/mindbot/mindbot/utils/test.sence.py
Normal file
@@ -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()
|
||||
Reference in New Issue
Block a user