From 3e3e1ed8eaa9ea57dc7e0daac0e0b46a1a33146d Mon Sep 17 00:00:00 2001 From: hangX Date: Mon, 13 Apr 2026 10:09:38 +0800 Subject: [PATCH] =?UTF-8?q?analytical=5Fbalance=E7=9A=84=E7=9B=B8=E5=85=B3?= =?UTF-8?q?=E9=85=8D=E7=BD=AE=EF=BC=8C=E4=BD=BF=E7=94=A8cloudxr=E9=81=A5?= =?UTF-8?q?=E6=93=8D=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- deps/cloudxr/INSTALL.md | 3 +- .../mindbot/assets/analytical_balance.py | 47 ++ .../mindbot/mindbot/robot/mindbot_cloudxr.py | 204 ++++++ .../manager_based/il/pick_place/__init__.py | 10 + .../pick_place/openbalnace_gr1t2_env_cfg.py | 591 ++++++++++++++++++ .../il/pick_place/pickplace_gr1t2_env_cfg.py | 2 +- 6 files changed, 855 insertions(+), 2 deletions(-) create mode 100644 source/mindbot/mindbot/assets/analytical_balance.py create mode 100644 source/mindbot/mindbot/robot/mindbot_cloudxr.py create mode 100644 source/mindbot/mindbot/tasks/manager_based/il/pick_place/openbalnace_gr1t2_env_cfg.py diff --git a/deps/cloudxr/INSTALL.md b/deps/cloudxr/INSTALL.md index d6c74c3..eaabbac 100644 --- a/deps/cloudxr/INSTALL.md +++ b/deps/cloudxr/INSTALL.md @@ -160,6 +160,7 @@ npm install ../nvidia-cloudxr-6.0.0-beta.tgz npm install # 启动 HTTPS 开发服务器（端口 8080） +sudo sysctl fs.inotify.max_user_watches=524288 npm run dev-server:https ``` @@ -177,7 +178,7 @@ npm run dev-server:https cd ~/IsaacLab sudo ./docker/container.py start \ --files docker-compose.cloudxr-runtime.patch.yaml \ - docker-compose.mindbot.patch.yaml \ + /home/maic/xh/mindbot/deps/cloudxr/docker-compose.mindbot.patch.yaml \ --env-files .env.cloudxr-runtime # 进入 Isaac Lab 容器 diff --git a/source/mindbot/mindbot/assets/analytical_balance.py b/source/mindbot/mindbot/assets/analytical_balance.py new file mode 100644 index 0000000..ee88551 --- /dev/null +++ b/source/mindbot/mindbot/assets/analytical_balance.py @@ -0,0 +1,47 @@ +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.sim.schemas.schemas_cfg import ( + RigidBodyPropertiesCfg, + CollisionPropertiesCfg, +) +from mindbot.utils.assets import MINDBOT_ASSETS_DIR +import math + +BALANCE_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{MINDBOT_ASSETS_DIR}/devices/Analytical_Balance/Equipment_02.usd", + scale=(0.012,0.012,0.012), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=1.0 + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + fix_root_link=True, # 由 USD 内的 FixedJoint_ 负责固定底座 + enabled_self_collisions=False, + solver_position_iteration_count=32, + solver_velocity_iteration_count=16, + stabilization_threshold=1e-6, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=[1.95, -0.45, 0.9], + rot=[-0.7071, 0.0, 0.0, 0.7071], + joint_pos={ + "RevoluteJoint": math.radians(0.0), # -100°，盖子打开位置 + "PrismaticJoint_0":math.radians(0.0), + "PrismaticJoint_1":math.radians(0.0), + }, + ), + actuators={ + "lid_open": ImplicitActuatorCfg( + joint_names_expr=['PrismaticJoint', 'PrismaticJoint_0', 'PrismaticJoint_1'], + stiffness=0, # 覆盖 USD 的 100，足够对抗重力保持开盖 + damping=5, # 阻尼同步加大，防止振荡 + effort_limit_sim=1000.0, + velocity_limit_sim=100.0, + ), + }, +) diff --git a/source/mindbot/mindbot/robot/mindbot_cloudxr.py b/source/mindbot/mindbot/robot/mindbot_cloudxr.py new file mode 100644 index 0000000..e8314b8 --- /dev/null +++ b/source/mindbot/mindbot/robot/mindbot_cloudxr.py @@ -0,0 +1,204 @@ +# 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:'MINDBOT_CFG' +* :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 +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + +## +# Configuration +## + + +MINDBOT_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=( + f"{MINDBOT_ASSETS_DIR}/robots/mindrobot-1105-lab/mindrobot_cd2.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=16, + solver_velocity_iteration_count=8, + stablization_threshold=1e-6, + ), + collision_props=sim_utils.CollisionPropertiesCfg( + collision_enabled=True, + contact_offset=0.0005, # 尝试从默认值增大到 0.01 或 0.02 + rest_offset=0, # 保持略小于 contact_offset + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.7), + joint_pos={ + # ====== Left arm (RM-65) — away from singularities ====== + # Elbow singularity: q3=0; Wrist singularity: q5=0. + # Small offsets on q2/q4/q6 to avoid exact 90° Jacobian symmetry. + "l_joint1": 1.5708, # 90° + "l_joint2": -1.2217, # -70° (offset from -90° for better elbow workspace) + "l_joint3": -1.5708, # -90° (away from elbow singularity q3=0) + "l_joint4": 1.3963, # 80° (offset from 90° to break wrist symmetry) + "l_joint5": 1.5708, # 90° (away from wrist singularity q5=0) + "l_joint6": -1.3963, # -80° (offset from -90° to break wrist symmetry) + # ====== Right arm (RM-65) — same joint values as left (verified visually) ====== + "r_joint1": 1.5708, # 90° + "r_joint2": -1.2217, # -70° + "r_joint3": -1.5708, # -90° (away from elbow singularity q3=0) + "r_joint4": 1.3963, # 80° + "r_joint5": 1.5708, # 90° (away from wrist singularity q5=0) + "r_joint6": 1.3963, # -80° + # ====== Grippers (0=open) ====== + "left_hand_joint_left": 0.0, + "left_hand_joint_right": 0.0, + "right_hand_joint_left": 0.0, + "right_hand_joint_right": 0.0, + # ====== Trunk & Head ====== + # NOTE: pos z = 0.44 (base clearance) + PrismaticJoint value. + # If you change PrismaticJoint, update pos z accordingly to avoid + # geometry clipping that causes the robot to fall on spawn. + "PrismaticJoint": 0.26, + "head_pitch_Joint": 0.0, + "head_yaw_Joint": 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=[ + "PrismaticJoint", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "wheels": ImplicitActuatorCfg( + # joint_names_expr=[".*_revolute_Joint"], + joint_names_expr=[ + "right_b_revolute_Joint", + "left_b_revolute_Joint", + "left_f_revolute_Joint", + "right_f_revolute_Joint", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "left_arm_shoulder": ImplicitActuatorCfg( + joint_names_expr=["l_joint[1-3]"], + effort_limit=torch.inf, + velocity_limit=torch.inf, + stiffness=None, + damping=None, + armature=0.0, + ), + # RM-65 J4-J6: wrist — max 225°/s ≈ 3.93 rad/s + "left_arm_wrist": ImplicitActuatorCfg( + joint_names_expr=["l_joint[4-6]"], + effort_limit=torch.inf, + velocity_limit=torch.inf, + stiffness=None, + damping=None, + armature=0.0, + ), + "right_arm_shoulder": ImplicitActuatorCfg( + joint_names_expr=["r_joint[1-3]"], + effort_limit=torch.inf, + velocity_limit=torch.inf, + stiffness=None, + damping=None, + armature=0.0, + ), + "right_arm_wrist": ImplicitActuatorCfg( + joint_names_expr=["r_joint[4-6]"], + effort_limit=torch.inf, + velocity_limit=torch.inf, + stiffness=None, + damping=None, + armature=0.0, + ), + "grippers": ImplicitActuatorCfg( + joint_names_expr=[".*_hand_joint.*"], + effort_limit_sim=200.0, + stiffness=2e3, + damping=1e2, + ), + # "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, + # ), + }, +) \ No newline at end of file diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/__init__.py index 7f2bd7d..07e319b 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/__init__.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/__init__.py @@ -17,6 +17,16 @@ gym.register( disable_env_checker=True, ) +gym.register( + id="Isaac-Balance-GR1T2-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.openbalnace_gr1t2_env_cfg:OpenBalanceEnvCfg", + "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", diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/openbalnace_gr1t2_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/openbalnace_gr1t2_env_cfg.py new file mode 100644 index 0000000..92d13b2 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/openbalnace_gr1t2_env_cfg.py @@ -0,0 +1,591 @@ +# 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.assets.analytical_balance import BALANCE_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.1], 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, 0.85], + # rot=[1.0, 0.0, 0.0, 0.0], + # joint_pos={"RevoluteJoint": -1.7453}, # -100° + # ), + # ) + analytical_balance = BALANCE_CFG.replace( + prim_path="{ENV_REGEX_NS}/Balance", + init_state=ArticulationCfg.InitialStateCfg( + pos=[0.0, 0.45, 0.90], + 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.4,0.4,0.8996],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.42374,0.32415,0.912],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/Collected_Test_Tube_AA_01/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), + ), + ) + + beaker = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Beaker", + init_state=RigidObjectCfg.InitialStateCfg( + pos=[-0.20, 0.324, 0.93], # 根据你的天平中心位置微调 + rot=[1, 0, 0, 0] + ), + spawn=UsdFileCfg( + usd_path=f"{MINDBOT_ASSETS_DIR}/laboratory_equipment/Beaker/beaker_500ml.usd", + + # 尺寸适配：假设原始模型是 1:1 或需要缩放至 120mm 高度 + scale=(0.35, 0.35, 0.35), + + rigid_props=sim_utils.RigidBodyPropertiesCfg( + # solver_velocity_iteration_count=4, + # solver_position_iteration_count=16, + # max_angular_velocity=1000.0, + # max_linear_velocity=1000.0, + # # 启用 GPU 动力学，确保与 Isaac Lab 流程兼容 + # disable_gravity=False, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=0.15), # 500ml 玻璃烧杯约 150g + collision_props=sim_utils.CollisionPropertiesCfg(), + ), +) + + # 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_balance_joints = EventTerm( + func=base_mdp.reset_joints_by_offset, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("analytical_balance"), + "position_range": (0.0, 0.0), + "velocity_range": (0.0, 0.0), + }, + ) + + reset_beaker_root = EventTerm( + func=base_mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {}, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("beaker"), + }, + ) + + reset_robot_joints = EventTerm( + func=base_mdp.reset_joints_by_offset, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot"), + "position_range": (0.0, 0.0), + "velocity_range": (0.0, 0.0), + }, + ) + + reset_robot_root = EventTerm( + func=base_mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {}, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("robot"), + }, + ) + + reset_tube = EventTerm( + func=base_mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {}, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("tube"), + }, + ) + + reset_rack = EventTerm( + func=base_mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {}, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("rack"), + }, + ) + +# @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 OpenBalanceEnvCfg(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 + + # self._setup_tube_material() + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.actions.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + # number of joints in both hands + num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "manusvive": ManusViveCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) + diff --git a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_env_cfg.py index 584a15d..5335bff 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/pick_place/pickplace_gr1t2_env_cfg.py @@ -96,7 +96,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.32374,0.32415,0.862],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", + usd_path=f"{MINDBOT_ASSETS_DIR}/laboratory_equipment/Collected_Test_Tube_AA_01/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),