diff --git a/scripts/rsl_rl/play.py b/scripts/rsl_rl/play.py index baef6c2..1b997ee 100644 --- a/scripts/rsl_rl/play.py +++ b/scripts/rsl_rl/play.py @@ -89,59 +89,6 @@ import numpy as np """use gpu compute to record video""" -class MultiCameraRecorder: - def __init__(self, env, camera_names: list[str], env_indices: list[int], output_dir: str, fps: int = 30): - self.env = env - self.camera_names = camera_names - self.env_indices = env_indices - self.output_dir = output_dir - self.fps = fps - self.frames = {cam_name: {env_idx: [] for env_idx in env_indices} for cam_name in camera_names} - os.makedirs(self.output_dir, exist_ok=True) - - self.cameras = {} - for name in camera_names: - if name in self.env.unwrapped.scene.keys(): - self.cameras[name] = self.env.unwrapped.scene[name] - print(f"[INFO] Camera {name} linked.") - - def record_step(self): - """保持在 GPU 上克隆数据""" - for cam_name, camera_obj in self.cameras.items(): - # 获取数据前强制同步一次（防止后端丢失） - rgb_data = camera_obj.data.output["rgb"] - - for env_idx in self.env_indices: - # 使用 .clone() 保持在 GPU，但要注意显存占用 - self.frames[cam_name][env_idx].append(rgb_data[env_idx].clone()) - - def save_videos(self, filename_suffix=""): - print(f"[INFO] Saving videos from GPU to Disk...") - for cam_name, env_dict in self.frames.items(): - for env_idx, frame_list in env_dict.items(): - if not frame_list: continue - - # 转换为 torchvision 格式 (T, C, H, W) - video_tensor = torch.stack(frame_list) - if video_tensor.shape[-1] == 4: # RGBA -> RGB - video_tensor = video_tensor[..., :3] - - # 移动到 CPU 并保存 - video_cpu = video_tensor.cpu() - output_path = os.path.join(self.output_dir, f"{cam_name}_env{env_idx}_{filename_suffix}.mp4") - - # 使用 torchvision 写入 (T, H, W, C) - torchvision.io.write_video(output_path, video_cpu, fps=self.fps) - - # 【关键】保存后立即释放显存 - del video_tensor - del video_cpu - frame_list.clear() - - torch.cuda.empty_cache() - -"""use cpu compute to record video""" -# # 2. 修改 MultiCameraRecorder 类 # class MultiCameraRecorder: # def __init__(self, env, camera_names: list[str], env_indices: list[int], output_dir: str, fps: int = 30): # self.env = env @@ -151,57 +98,110 @@ class MultiCameraRecorder: # self.fps = fps # self.frames = {cam_name: {env_idx: [] for env_idx in env_indices} for cam_name in camera_names} # os.makedirs(self.output_dir, exist_ok=True) + # self.cameras = {} # for name in camera_names: -# try: +# if name in self.env.unwrapped.scene.keys(): # self.cameras[name] = self.env.unwrapped.scene[name] -# print(f"[INFO][MultiCameraRecorder] Found camera: {name}") -# except KeyError: -# print(f"[WARN][MultiCameraRecorder] Camera '{name}' not found!") +# print(f"[INFO] Camera {name} linked.") # def record_step(self): -# """在每个仿真步调用""" +# """保持在 GPU 上克隆数据""" # for cam_name, camera_obj in self.cameras.items(): -# # [关键修改] 获取数据前先确保数据已同步 -# # 这可以防止读取到正在渲染中的内存导致 access violation -# rgb_data = camera_obj.data.output["rgb"] +# # 获取数据前强制同步一次（防止后端丢失） +# rgb_data = camera_obj.data.output["rgb"] # for env_idx in self.env_indices: -# if env_idx >= rgb_data.shape[0]: continue - -# # 转换为 CPU 上的 numpy，这种方式通常比 torchvision 的 tensor 堆叠更稳 -# frame = rgb_data[env_idx].clone().detach().cpu().numpy() -# self.frames[cam_name][env_idx].append(frame) +# # 使用 .clone() 保持在 GPU，但要注意显存占用 +# self.frames[cam_name][env_idx].append(rgb_data[env_idx].clone()) # def save_videos(self, filename_suffix=""): -# """循环结束后调用""" -# print(f"[INFO][MultiCameraRecorder] Saving videos...") +# print(f"[INFO] Saving videos from GPU to Disk...") # for cam_name, env_dict in self.frames.items(): # for env_idx, frame_list in env_dict.items(): # if not frame_list: continue -# print(f" -> Saving {cam_name} (Env {env_idx})...") +# # 转换为 torchvision 格式 (T, C, H, W) +# video_tensor = torch.stack(frame_list) +# if video_tensor.shape[-1] == 4: # RGBA -> RGB +# video_tensor = video_tensor[..., :3] -# # 处理格式并使用 imageio 保存 -# processed_frames = [] -# for img in frame_list: -# # [0, 1] -> [0, 255] -# if img.dtype != np.uint8: -# if img.max() <= 1.01: img = (img * 255).astype(np.uint8) -# else: img = img.astype(np.uint8) -# # 去掉 Alpha 通道 -# if img.shape[-1] == 4: img = img[:, :, :3] -# processed_frames.append(img) +# # 移动到 CPU 并保存 +# video_cpu = video_tensor.cpu() +# output_path = os.path.join(self.output_dir, f"{cam_name}_env{env_idx}_{filename_suffix}.mp4") + +# # 使用 torchvision 写入 (T, H, W, C) +# torchvision.io.write_video(output_path, video_cpu, fps=self.fps) + +# # 【关键】保存后立即释放显存 +# del video_tensor +# del video_cpu +# frame_list.clear() + +# torch.cuda.empty_cache() -# fname = f"{cam_name}_env{env_idx}_{filename_suffix}.mp4" -# output_path = os.path.join(self.output_dir, fname) +"""use cpu compute to record video""" +# # 2. 修改 MultiCameraRecorder 类 +class MultiCameraRecorder: + def __init__(self, env, camera_names: list[str], env_indices: list[int], output_dir: str, fps: int = 30): + self.env = env + self.camera_names = camera_names + self.env_indices = env_indices + self.output_dir = output_dir + self.fps = fps + self.frames = {cam_name: {env_idx: [] for env_idx in env_indices} for cam_name in camera_names} + os.makedirs(self.output_dir, exist_ok=True) + self.cameras = {} + for name in camera_names: + try: + self.cameras[name] = self.env.unwrapped.scene[name] + print(f"[INFO][MultiCameraRecorder] Found camera: {name}") + except KeyError: + print(f"[WARN][MultiCameraRecorder] Camera '{name}' not found!") + + def record_step(self): + """在每个仿真步调用""" + for cam_name, camera_obj in self.cameras.items(): + # [关键修改] 获取数据前先确保数据已同步 + # 这可以防止读取到正在渲染中的内存导致 access violation + rgb_data = camera_obj.data.output["rgb"] + + for env_idx in self.env_indices: + if env_idx >= rgb_data.shape[0]: continue -# try: -# # 使用 imageio 写入视频 -# imageio.mimsave(output_path, processed_frames, fps=self.fps) -# print(f" Saved: {output_path}") -# except Exception as e: -# print(f" [ERROR] Failed to save {fname}: {e}") + # 转换为 CPU 上的 numpy，这种方式通常比 torchvision 的 tensor 堆叠更稳 + frame = rgb_data[env_idx].clone().detach().cpu().numpy() + self.frames[cam_name][env_idx].append(frame) + + def save_videos(self, filename_suffix=""): + """循环结束后调用""" + print(f"[INFO][MultiCameraRecorder] Saving videos...") + for cam_name, env_dict in self.frames.items(): + for env_idx, frame_list in env_dict.items(): + if not frame_list: continue + + print(f" -> Saving {cam_name} (Env {env_idx})...") + + # 处理格式并使用 imageio 保存 + processed_frames = [] + for img in frame_list: + # [0, 1] -> [0, 255] + if img.dtype != np.uint8: + if img.max() <= 1.01: img = (img * 255).astype(np.uint8) + else: img = img.astype(np.uint8) + # 去掉 Alpha 通道 + if img.shape[-1] == 4: img = img[:, :, :3] + processed_frames.append(img) + + fname = f"{cam_name}_env{env_idx}_{filename_suffix}.mp4" + output_path = os.path.join(self.output_dir, fname) + + try: + # 使用 imageio 写入视频 + imageio.mimsave(output_path, processed_frames, fps=self.fps) + print(f" Saved: {output_path}") + except Exception as e: + print(f" [ERROR] Failed to save {fname}: {e}") diff --git a/scripts/zero_agent.py b/scripts/zero_agent.py index 3f2d6b0..5bf8d65 100644 --- a/scripts/zero_agent.py +++ b/scripts/zero_agent.py @@ -52,6 +52,11 @@ def main(): print(f"[INFO]: Gym action space: {env.action_space}") # reset environment env.reset() + + # 计数器：每 120 步打印一次 + step_count = 0 + print_interval = 120 + # simulate environment while simulation_app.is_running(): # run everything in inference mode @@ -60,6 +65,84 @@ def main(): actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device) # apply actions env.step(actions) + + # ... (前面的代码保持不变) + + # 每 120 步打印一次坐标 + step_count += 1 + if step_count % print_interval == 0: + scene = env.unwrapped.scene + + # 1. 获取 centrifuge 的世界坐标 + try: + centrifuge = scene["centrifuge"] + centrifuge_pos = centrifuge.data.root_pos_w[0].cpu().numpy() + centrifuge_quat = centrifuge.data.root_quat_w[0].cpu().numpy() + print(f"[Step {step_count}] CENTRIFUGE - Pos: {centrifuge_pos}, Quat: {centrifuge_quat}") + except KeyError: + print(f"[Step {step_count}] CENTRIFUGE - Not found") + + # 2. 获取 Reservoir_A 的世界坐标 + # 注意：这里的 key ("reservoir_a") 必须与你在 SceneCfg 中定义的名称一致 + try: + # 如果你在配置里命名为 "reservoir_a" + reservoir = scene["reservoir_a"] + res_pos = reservoir.data.root_pos_w[0].cpu().numpy() + res_quat = reservoir.data.root_quat_w[0].cpu().numpy() + print(f"[Step {step_count}] RESERVOIR_A - Pos: {res_pos}, Quat: {res_quat}") + except KeyError: + # 如果 Reservoir_A 是 centrifuge 机器人(Articulation)的一个 Link（身体部件） + # 我们可以从 centrifuge 的 body 数据中获取 + try: + centrifuge = scene["centrifuge"] + # 找到名为 'Reservoir_A' 的 link 索引 + body_names = centrifuge.body_names + if "Reservoir_A" in body_names: + idx = body_names.index("Reservoir_A") + res_pos = centrifuge.data.body_pos_w[0, idx].cpu().numpy() + res_quat = centrifuge.data.body_quat_w[0, idx].cpu().numpy() + print(f"[Step {step_count}] RESERVOIR_A (Link) - Pos: {res_pos}, Quat: {res_quat}") + else: + print(f"[Step {step_count}] RESERVOIR_A - Not found in scene keys or links") + except Exception: + print(f"[Step {step_count}] RESERVOIR_A - Not found") + + # 3. 获取 lid 的世界坐标 + try: + lid = scene["lid"] + lid_pos = lid.data.root_pos_w[0].cpu().numpy() + lid_quat = lid.data.root_quat_w[0].cpu().numpy() + print(f"[Step {step_count}] LID - Pos: {lid_pos}, Quat: {lid_quat}") + except KeyError: + print(f"[Step {step_count}] LID - Not found") + + print("-" * 80) + +# ... (后面的代码保持不变) + # 每 120 步打印一次坐标 + # step_count += 1 + # if step_count % print_interval == 0: + # scene = env.unwrapped.scene + + # # 获取 centrifuge 的世界坐标（root position） + # try: + # centrifuge = scene["centrifuge"] + # centrifuge_pos = centrifuge.data.root_pos_w[0].cpu().numpy() # 取第一个环境 + # centrifuge_quat = centrifuge.data.root_quat_w[0].cpu().numpy() + # print(f"[Step {step_count}] CENTRIFUGE_CFG - World Position: {centrifuge_pos}, Quaternion: {centrifuge_quat}") + # except KeyError: + # print(f"[Step {step_count}] CENTRIFUGE_CFG - Not found in scene") + + # # 获取 lid 的世界坐标 + # try: + # lid = scene["lid"] + # lid_pos = lid.data.root_pos_w[0].cpu().numpy() # 取第一个环境 + # lid_quat = lid.data.root_quat_w[0].cpu().numpy() + # print(f"[Step {step_count}] LID_CFG - World Position: {lid_pos}, Quaternion: {lid_quat}") + # except KeyError: + # print(f"[Step {step_count}] LID_CFG - Not found in scene") + + # print("-" * 80) # close the simulator env.close() @@ -70,3 +153,80 @@ if __name__ == "__main__": main() # close sim app simulation_app.close() + +# # 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 + +# """Script to run an environment with zero action agent.""" + +# """Launch Isaac Sim Simulator first.""" + +# import argparse + +# from isaaclab.app import AppLauncher + +# # add argparse arguments +# parser = argparse.ArgumentParser(description="Zero agent for Isaac Lab environments.") +# parser.add_argument( +# "--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations." +# ) +# parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.") +# parser.add_argument("--task", type=str, default=None, help="Name of the task.") +# # append AppLauncher cli args +# AppLauncher.add_app_launcher_args(parser) +# # parse the arguments +# args_cli = parser.parse_args() + +# # launch omniverse app +# app_launcher = AppLauncher(args_cli) +# simulation_app = app_launcher.app + +# """Rest everything follows.""" + +# import gymnasium as gym +# import torch + +# import isaaclab_tasks # noqa: F401 +# from isaaclab_tasks.utils import parse_env_cfg + +# import mindbot.tasks # noqa: F401 + + +# def main(): +# """Zero actions agent with Isaac Lab environment.""" +# # parse configuration +# env_cfg = parse_env_cfg( +# args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric +# ) +# # create environment +# env = gym.make(args_cli.task, cfg=env_cfg) + +# # print info (this is vectorized environment) +# print(f"[INFO]: Gym observation space: {env.observation_space}") +# print(f"[INFO]: Gym action space: {env.action_space}") +# # reset environment +# env.reset() +# # simulate environment + +# step_count = 0 +# print_interval = 120 + +# while simulation_app.is_running(): +# # run everything in inference mode +# with torch.inference_mode(): +# # compute zero actions +# actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device) +# # apply actions +# env.step(actions) + +# # close the simulator +# env.close() + + +# if __name__ == "__main__": +# # run the main function +# main() +# # close sim app +# simulation_app.close() diff --git a/source/mindbot/mindbot/robot/mindbot.py b/source/mindbot/mindbot/robot/mindbot.py index 301d188..821630a 100644 --- a/source/mindbot/mindbot/robot/mindbot.py +++ b/source/mindbot/mindbot/robot/mindbot.py @@ -66,26 +66,26 @@ MINDBOT_CFG = ArticulationCfg( # "l_joint5": 0.0, # "l_joint6": 0.0, # Right arm joints - # -45° -> -0.7854 - "r_joint1": -0.7854, - # 70° -> 1.2217 - "r_joint2": 1.2217, - # 90° -> 1.5708 - "r_joint3": 1.5708, - # -90° -> -1.5708 - "r_joint4": -1.5708, - # 90° -> 1.5708 - "r_joint5": 1.5708, - # 70° -> 1.2217 - "r_joint6": 1.2217, + # # -45° -> -0.7854 + # "r_joint1": -0.7854, + # # 70° -> 1.2217 + # "r_joint2": 1.2217, + # # 90° -> 1.5708 + # "r_joint3": 1.5708, + # # -90° -> -1.5708 + # "r_joint4": -1.5708, + # # 90° -> 1.5708 + # "r_joint5": 1.5708, + # # 70° -> 1.2217 + # "r_joint6": 1.2217, # Right arm joints - # "r_joint1": 0.0, - # # "r_joint2": -1.5708, - # "r_joint2": 0.0, - # "r_joint3": 0.0, - # "r_joint4": 0.0, - # "r_joint5": 0.0, - # "r_joint6": 0.0, + "r_joint1": 0.0, + # "r_joint2": -1.5708, + "r_joint2": 0.0, + "r_joint3": 0.0, + "r_joint4": 0.0, + "r_joint5": 0.0, + "r_joint6": 0.0, # # # left wheel # "left_b_revolute_Joint": 0.0, # "left_f_revolute_Joint": 0.0, @@ -101,7 +101,7 @@ MINDBOT_CFG = ArticulationCfg( "right_hand_joint_left": 0.0, "right_hand_joint_right": 0.0, # trunk - "PrismaticJoint": 0.3, + "PrismaticJoint": 0.23, # 0.30 # head "head_revoluteJoint": 0.0 #0.5236 }, @@ -115,7 +115,7 @@ MINDBOT_CFG = ArticulationCfg( joint_names_expr=["l_joint[1-6]"], # This matches l_joint1, l_joint2, ..., l_joint6 effort_limit_sim=100.0, # Note: Tune this based on your robot's specs velocity_limit_sim=100.0, # Note: Tune this based on your robot's specs - stiffness=10000.0, #10000.0, # Note: Tune this for desired control performance + stiffness=0.0, #10000.0, # Note: Tune this for desired control performance damping=1000.0, #10000.0, # Note: Tune this for desired control performance ), # Group for the 6 right arm joints using a regular expression @@ -123,7 +123,7 @@ MINDBOT_CFG = ArticulationCfg( joint_names_expr=["r_joint[1-6]"], # This matches r_joint1, r_joint2, ..., r_joint6 effort_limit_sim=100.0, # Note: Tune this based on your robot's specs velocity_limit_sim=100.0, # Note: Tune this based on your robot's specs - stiffness=0.0, #10000.0, # Note: Tune this for desired control performance1 + stiffness=10000.0, #10000.0, # Note: Tune this for desired control performance1 damping=1000.0, #10000.0, # Note: Tune this for desired control performance ), "head": ImplicitActuatorCfg(joint_names_expr=["head_revoluteJoint"], stiffness=10000.0, damping=1000.0), diff --git a/source/mindbot/mindbot/tasks/manager_based/centrifuge/agents/rsl_rl_ppo_cfg.py b/source/mindbot/mindbot/tasks/manager_based/centrifuge/agents/rsl_rl_ppo_cfg.py index 800b162..dec1d54 100644 --- a/source/mindbot/mindbot/tasks/manager_based/centrifuge/agents/rsl_rl_ppo_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/centrifuge/agents/rsl_rl_ppo_cfg.py @@ -14,7 +14,7 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 5000 # 增加迭代次数，给它足够的时间学习 save_interval = 100 # experiment_name = "mindbot_grasp" # 建议改个名，不要叫 cartpole 了 - experiment_name = "mindbot_pullUltrasoundLidUp" + experiment_name = "mindbot_centrifuge_LidUp" policy = RslRlPpoActorCriticCfg( init_noise_std=0.7, diff --git a/source/mindbot/mindbot/tasks/manager_based/centrifuge/centrifuge_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/centrifuge/centrifuge_env_cfg.py index 387c459..6e24fe5 100644 --- a/source/mindbot/mindbot/tasks/manager_based/centrifuge/centrifuge_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/centrifuge/centrifuge_env_cfg.py @@ -64,13 +64,15 @@ TABLE_CFG=RigidObjectCfg( LID_CFG = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/Lid", init_state=RigidObjectCfg.InitialStateCfg( - pos=[0.95, 0.73, 0.1055], - rot=[1.0, 0.0, 0.0, 0.0], + # pos=[0.9523,-0.2512,1.0923], + pos=[0.803 , -0.25, 1.0282],#(0.9988, -0.2977, 1.0498633) + # rot=[-0.7071, 0.0, 0.0, 0.7071], + rot=[0.0, 0.0, 0.0, 1.0], lin_vel=[0.0, 0.0, 0.0], ang_vel=[0.0, 0.0, 0.0], ), spawn=UsdFileCfg( - usd_path="C:/Users/PC/workpalce/maic_usd_assets/equipment/centrifuge/Lid_B.usd", + usd_path="C:/Users/PC/workpalce/maic_usd_assets/assets/Collected_equipment001/Equipment/tube1.usd", # scale=(0.2, 0.2, 0.2), rigid_props=RigidBodyPropertiesCfg( rigid_body_enabled= True, @@ -91,6 +93,38 @@ LID_CFG = RigidObjectCfg( ), ) +# LID_CFG = RigidObjectCfg( +# prim_path="{ENV_REGEX_NS}/Lid", +# init_state=RigidObjectCfg.InitialStateCfg( +# # pos=[0.9523,-0.2512,1.0923], +# pos=[0.8488, -0.2477, 1.0498633],#(0.9988, -0.2977, 1.0498633) +# # rot=[-0.7071, 0.0, 0.0, 0.7071], +# rot=[0.0, 0.0, 0.0, 1.0], +# lin_vel=[0.0, 0.0, 0.0], +# ang_vel=[0.0, 0.0, 0.0], +# ), +# spawn=UsdFileCfg( +# usd_path="C:/Users/PC/workpalce/maic_usd_assets/equipment/centrifuge/Lid_B.usd", +# # scale=(0.2, 0.2, 0.2), +# rigid_props=RigidBodyPropertiesCfg( +# rigid_body_enabled= True, +# solver_position_iteration_count=32, +# solver_velocity_iteration_count=16, +# max_angular_velocity=1000.0, +# max_linear_velocity=1000.0, +# max_depenetration_velocity=0.5,#original 5.0 +# linear_damping=5.0, #original 0.5 +# angular_damping=5.0, #original 0.5 +# disable_gravity=False, +# ), +# collision_props=CollisionPropertiesCfg( +# collision_enabled=True, +# contact_offset=0.0005,#original 0.02 +# rest_offset=0 +# ), +# ), +# ) + CENTRIFUGE_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path="C:/Users/PC/workpalce/maic_usd_assets/equipment/centrifuge/centrifuge.usd", @@ -108,6 +142,11 @@ CENTRIFUGE_CFG = ArticulationCfg( # 【重要】必须要固定底座，否则机器人按盖子时，离心机会翻倒 # fix_root_link=True, ), + collision_props=CollisionPropertiesCfg( + collision_enabled=True, + contact_offset=0.0005,#original 0.02 + rest_offset=0 + ), ), init_state=ArticulationCfg.InitialStateCfg( # 1. 参照机器人配置，在这里定义初始关节角度 @@ -118,8 +157,9 @@ CENTRIFUGE_CFG = ArticulationCfg( # 您的 USD 限位是 (-100, 0)，-100度为最大开启 "r2": math.radians(-100.0), }, - pos=(0.95, -0.3, 0.855), + pos=(0.80, -0.25, 0.8085),#(0.95, -0.3, 0.8085) rot=[-0.7071, 0.0, 0.0, 0.7071], + # rot=[0.0, 0.0, 0.0, 1.0], ), actuators={ "lid_passive_mechanism": ImplicitActuatorCfg( @@ -144,68 +184,11 @@ CENTRIFUGE_CFG = ArticulationCfg( # 转子可以硬一点，不需要被机器人按动 "rotor_control": ImplicitActuatorCfg( joint_names_expr=["r1"], - stiffness=1000.0, + stiffness=0.0, damping=10.0, ), } ) -# CENTRIFUGE_CFG = ArticulationCfg( -# spawn=sim_utils.UsdFileCfg( -# usd_path="C:/Users/PC/workpalce/maic_usd_assets/equipment/centrifuge/centrifuge.usd", -# rigid_props=sim_utils.RigidBodyPropertiesCfg( -# disable_gravity=False, -# max_depenetration_velocity=1.0, -# linear_damping=0.5, -# angular_damping=0.5, -# ), -# articulation_props=sim_utils.ArticulationRootPropertiesCfg( -# enabled_self_collisions=False,# -# solver_position_iteration_count=32, -# solver_velocity_iteration_count=16, -# stabilization_threshold=1e-6, -# fix_root_link=True, -# ), -# ), -# init_state=ArticulationCfg.InitialStateCfg( -# joint_pos={ -# "r1": math.radians(-100.0), -# "r2": math.radians(-100.0), -# }, -# pos=(0.95, -0.3, 0.855), -# rot=[-0.7071, 0.0, 0.0, 0.7071], -# ), -# # actuators={} -# actuators={ -# "passive_damper": ImplicitActuatorCfg( -# # ".*" 表示匹配该USD文件内的所有关节（无论是轮子、屏幕转轴还是其他） -# joint_names_expr=["r2"], - -# # 【关键逻辑】 -# # 使用较高的 Stiffness (刚度) 模拟电机或弹簧锁死在特定位置 -# # 这样盖子才不会掉下来 -# stiffness=200.0, - -# # 适当的阻尼，防止盖子像弹簧一样疯狂抖动 -# damping=20.0, - -# # 确保力矩足够克服盖子的重力 -# effort_limit_sim=1000.0, -# velocity_limit_sim=100.0, -# ), -# # 如果有其他关节(如转子r1)，可以单独配置 -# "rotor_control": ImplicitActuatorCfg( -# joint_names_expr=["r1"], -# stiffness=1000.0, -# damping=10.0, -# # joint_names_expr=[".*"], - -# # stiffness=0.0, -# # damping=10.0, -# # effort_limit_sim=100.0, -# # velocity_limit_sim=100.0, -# ), -# } -# ) ROOM_CFG = AssetBaseCfg( @@ -300,36 +283,36 @@ class ActionsCfg: """Action specifications for the MDP.""" # 使用任务空间控制器：策略输出末端执行器位置+姿态增量（6维） - left_arm_ee = DifferentialInverseKinematicsActionCfg( - asset_name="Mindbot", - joint_names=["l_joint[1-6]"], # 左臂的6个关节 - body_name="left_hand_body", # 末端执行器body名称 - controller=DifferentialIKControllerCfg( - command_type="pose", # 控制位置+姿态 - use_relative_mode=True, # 相对模式：动作是增量 - ik_method="dls", # Damped Least Squares方法 - ), - scale=(0.01, 0.01, 0.01, 0.025, 0.025, 0.025), - ) - - # right_arm_fixed = mdp.JointPositionActionCfg( + # left_arm_ee = DifferentialInverseKinematicsActionCfg( # asset_name="Mindbot", - # joint_names=["r_joint[1-6]"], # 对应 l_joint1 到 l_joint6 - - # # 1. 缩放设为 0：这让 RL 策略无法控制它，它不会动 - # scale=0.0, - - # # 2. 偏移设为你的目标角度（弧度）：这告诉电机“永远停在这里” - # # 对应 (135, -70, -90, 90, 90, -70) - # offset={ - # "r_joint1": 2.3562, - # "r_joint2": -1.2217, - # "r_joint3": -1.5708, - # "r_joint4": 1.5708, - # "r_joint5": 1.5708, - # "r_joint6": -1.2217, - # }, + # joint_names=["r_joint[1-6]"], # 左臂的6个关节 + # body_name="right_hand_body", # 末端执行器body名称 + # controller=DifferentialIKControllerCfg( + # command_type="pose", # 控制位置+姿态 + # use_relative_mode=True, # 相对模式：动作是增量 + # ik_method="dls", # Damped Least Squares方法 + # ), + # scale=(0.01, 0.01, 0.01, 0.025, 0.025, 0.025), # ) + + right_arm_fixed = mdp.JointPositionActionCfg( + asset_name="Mindbot", + joint_names=["r_joint[1-6]"], # 对应 l_joint1 到 l_joint6 + + # 1. 缩放设为 0：这让 RL 策略无法控制它，它不会动 + scale=0.0, + + # 2. 偏移设为你的目标角度（弧度）：这告诉电机“永远停在这里” + # 对应 (135, -70, -90, 90, 90, -70) + offset={ + "r_joint1": 2.3562, + "r_joint2": -1.2217, + "r_joint3": -1.5708, + "r_joint4": 1.5708, + "r_joint5": 1.5708, + "r_joint6": -1.2217, + }, + ) grippers_position = mdp.BinaryJointPositionActionCfg( asset_name="Mindbot", @@ -348,10 +331,19 @@ class ActionsCfg: asset_name="Mindbot", joint_names=["PrismaticJoint"], scale=0.00, - offset=0.3, + offset=0.24, # 0.3 clip=None ) + centrifuge_lid_passive = mdp.JointPositionActionCfg( + asset_name="centrifuge", # 对应场景中的名称 + joint_names=["r2"], + # 将 scale 设为 0，意味着 RL 算法输出的任何值都会被乘 0，即无法干扰它 + scale=0.00, + # 将 offset 设为目标角度，这会成为 PD 控制器的恒定 Target + offset= -1.7453, + clip=None + ) @configclass class ObservationsCfg: @@ -412,7 +404,7 @@ class EventCfg: func=mdp.reset_joints_by_offset, mode="reset", params={ - "asset_cfg": SceneEntityCfg("Mindbot", joint_names=["l_joint[1-6]"]), + "asset_cfg": SceneEntityCfg("Mindbot", joint_names=["r_joint[1-6]"]), "position_range": (-0.01, 0.01), "velocity_range": (0.0, 0.0), }, @@ -425,7 +417,8 @@ class EventCfg: mode="reset", params={ "asset_cfg": SceneEntityCfg("centrifuge"), - "pose_range": {"x": (-0.07, 0.07), "y": (-0.07, 0.07), "yaw": (-0.1, 0.1)}, + # "pose_range": {"x": (-0.07, 0.07), "y": (-0.07, 0.07), "yaw": (-0.1, 0.1)}, + "pose_range": {"x": (0.0, 0.0), "y": (0.0, 0.0), "z": (0.0, 0.0)}, "velocity_range": {"x": (0.0, 0.0)} } ) @@ -434,7 +427,8 @@ class EventCfg: mode="reset", params={ "asset_cfg": SceneEntityCfg("lid"), - "pose_range": {"x": (-0.03, 0.03), "y": (-0.03, 0.03), "yaw": (-1.5, 1.5)}, + # "pose_range": {"x": (-0.03, 0.03), "y": (-0.03, 0.03), "yaw": (-1.5, 1.5)}, + "pose_range": {"x": (0.0, 0.0), "y": (0.0, 0.0), "z": (0.0, 0.0)}, "velocity_range": {"x": (0.0, 0.0)} } ) @@ -449,7 +443,7 @@ class RewardsCfg: params={ "lid_cfg": SceneEntityCfg("lid"), "robot_cfg": SceneEntityCfg("Mindbot"), - "gripper_body_name": "left_hand_body", + "gripper_body_name": "right_hand_body", "gripper_forward_axis": (0.0, 0.0, 1.0), "gripper_width_axis": (0.0, 1.0, 0.0), "lid_handle_axis": (0.0, 1.0, 0.0), @@ -465,9 +459,9 @@ class RewardsCfg: params={ "lid_cfg": SceneEntityCfg("lid"), "robot_cfg": SceneEntityCfg("Mindbot"), - "left_gripper_name": "left_hand__l", - "right_gripper_name": "left_hand_r", - "height_offset": 0.07, # Z方向：lid 上方 0.1m + "left_gripper_name": "right_hand_l", + "right_gripper_name": "right_hand__r", + "height_offset": 0.115, # Z方向：lid 上方 0.1m "std": 0.3, # 位置对齐的容忍度 }, weight=3.0 #original 3.0 @@ -477,64 +471,64 @@ class RewardsCfg: params={ "lid_cfg": SceneEntityCfg("lid"), "robot_cfg": SceneEntityCfg("Mindbot"), - "left_gripper_name": "left_hand__l", - "right_gripper_name": "left_hand_r", - "height_offset": 0.07, + "left_gripper_name": "right_hand_l", + "right_gripper_name": "right_hand__r", + "height_offset": 0.115, }, # weight 为负数表示惩罚。 # 假设距离 0.5m，惩罚就是 -0.5 * 2.0 = -1.0 分。 weight=-1.0 ) - # 【新增】精细对齐 (引导进入 2cm 圈) - gripper_lid_fine_alignment = RewTerm( - func=mdp.gripper_lid_position_alignment, - params={ - "lid_cfg": SceneEntityCfg("lid"), - "robot_cfg": SceneEntityCfg("Mindbot"), - "left_gripper_name": "left_hand__l", - "right_gripper_name": "left_hand_r", - "height_offset": 0.07, # 【注意】这个高度必须非常准，和 interaction 判定高度一致 - "std": 0.05, # 非常陡峭的梯度，只在 5-10cm 内有效 - }, - weight=10.0 # 高权重 - ) + # # 【新增】精细对齐 (引导进入 2cm 圈) + # gripper_lid_fine_alignment = RewTerm( + # func=mdp.gripper_lid_position_alignment, + # params={ + # "lid_cfg": SceneEntityCfg("lid"), + # "robot_cfg": SceneEntityCfg("Mindbot"), + # "left_gripper_name": "left_hand__l", + # "right_gripper_name": "left_hand_r", + # "height_offset": 0.07, # 【注意】这个高度必须非常准，和 interaction 判定高度一致 + # "std": 0.05, # 非常陡峭的梯度，只在 5-10cm 内有效 + # }, + # weight=10.0 # 高权重 + # ) - gripper_close_interaction = RewTerm( - func=mdp.gripper_close_when_near, - params={ - "lid_cfg": SceneEntityCfg("lid"), - "robot_cfg": SceneEntityCfg("Mindbot"), - "left_gripper_name": "left_hand__l", - "right_gripper_name": "left_hand_r", - "joint_names": ["left_hand_joint_left", "left_hand_joint_right"], - "target_close_pos": 0.03, - "height_offset": 0.04, - "grasp_radius": 0.03, - }, - weight=10.0 - ) + # gripper_close_interaction = RewTerm( + # func=mdp.gripper_close_when_near, + # params={ + # "lid_cfg": SceneEntityCfg("lid"), + # "robot_cfg": SceneEntityCfg("Mindbot"), + # "left_gripper_name": "left_hand__l", + # "right_gripper_name": "left_hand_r", + # "joint_names": ["left_hand_joint_left", "left_hand_joint_right"], + # "target_close_pos": 0.03, + # "height_offset": 0.04, + # "grasp_radius": 0.03, + # }, + # weight=10.0 + # ) - lid_lifted_reward = RewTerm( - func=mdp.lid_is_lifted, # 使用 rewards.py 中已有的函数 - params={ - "lid_cfg": SceneEntityCfg("lid"), - "minimal_height": 1.0, # 根据你的场景调整，比初始高度高 5cm 左右 - }, - weight=10.0 # 给一个足够大的权重 - ) + # lid_lifted_reward = RewTerm( + # func=mdp.lid_is_lifted, # 使用 rewards.py 中已有的函数 + # params={ + # "lid_cfg": SceneEntityCfg("lid"), + # "minimal_height": 1.0, # 根据你的场景调整，比初始高度高 5cm 左右 + # }, + # weight=10.0 # 给一个足够大的权重 + # ) - lid_lifting_reward = RewTerm( - func=mdp.lid_lift_progress_reward, - params={ - "lid_cfg": SceneEntityCfg("lid"), - "initial_height": 0.8, # 请务必核实这个值！在 print 里看一下 lid 的 z 坐标 - "target_height_lift": 0.2, - "height_offset": 0.07, # 与其他奖励保持一致 - "std": 0.1 - }, - # 权重设大一点，告诉它这是最终目标 - weight=10.0 - ) + # lid_lifting_reward = RewTerm( + # func=mdp.lid_lift_progress_reward, + # params={ + # "lid_cfg": SceneEntityCfg("lid"), + # "initial_height": 0.8, # 请务必核实这个值！在 print 里看一下 lid 的 z 坐标 + # "target_height_lift": 0.2, + # "height_offset": 0.07, # 与其他奖励保持一致 + # "std": 0.1 + # }, + # # 权重设大一点，告诉它这是最终目标 + # weight=10.0 + # ) @@ -550,20 +544,20 @@ class TerminationsCfg: params={"asset_cfg": SceneEntityCfg("Mindbot"), "maximum_height": 2.0}, ) - # lid_fly_away = DoneTerm( - # func=mdp.base_height_failure, - # params={"asset_cfg": SceneEntityCfg("lid"), "maximum_height": 2.0}, - # ) + lid_fly_away = DoneTerm( + func=mdp.base_height_failure, + params={"asset_cfg": SceneEntityCfg("lid"), "maximum_height": 2.0}, + ) # 新增：盖子掉落判定 # 如果盖子掉回桌面（高度接近初始高度），且已经运行了一段时间（避开刚开始重置的瞬间） - # lid_dropped = DoneTerm( - # func=mdp.base_height_failure, # 复用高度判定函数 - # params={ - # "asset_cfg": SceneEntityCfg("lid"), - # "minimum_height": 0.79, # 假设初始高度是 0.9，低于 0.88 说明掉下去了或者被砸进去了 - # }, - # ) + lid_dropped = DoneTerm( + func=mdp.base_height_failure, # 复用高度判定函数 + params={ + "asset_cfg": SceneEntityCfg("lid"), + "minimum_height": 0.79, # 假设初始高度是 0.9，低于 0.88 说明掉下去了或者被砸进去了 + }, + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/pullUltrasoundLidUp/mindbot_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/pullUltrasoundLidUp/mindbot_env_cfg.py index c8e5971..72c1bac 100644 --- a/source/mindbot/mindbot/tasks/manager_based/pullUltrasoundLidUp/mindbot_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/pullUltrasoundLidUp/mindbot_env_cfg.py @@ -97,6 +97,12 @@ ROOM_CFG = AssetBaseCfg( usd_path="C:/Users/PC/workpalce/maic_usd_assets/twinlab/MIC_sim-3.0/244_140/room.usd", ), ) +ROOM_CFG = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Room", + spawn=UsdFileCfg( + usd_path="C:/Users/PC/workpalce/maic_usd_assets/twinlab/Collected_lab2/lab.usd", + ), +) # ROOM_CFG = RigidObjectCfg( # prim_path="{ENV_REGEX_NS}/Room", @@ -226,7 +232,7 @@ class MindbotSceneCfg(InteractiveSceneCfg): data_types=["rgb"], spawn=None, ) -## +# # ## # MDP settings ## @@ -544,7 +550,7 @@ class CurriculumCfg: @configclass class MindbotEnvCfg(ManagerBasedRLEnvCfg): # Scene settings - scene: MindbotSceneCfg = MindbotSceneCfg(num_envs=5, env_spacing=5.0) + scene: MindbotSceneCfg = MindbotSceneCfg(num_envs=5, env_spacing=3.0) # Basic settings observations: ObservationsCfg = ObservationsCfg() actions: ActionsCfg = ActionsCfg()