hangX/mindbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

离心机加试管初步配置

Browse Source
This commit is contained in:
hangX
2026-02-02 16:39:33 +08:00
parent 196378f2d3
commit 9325184f4d
6 changed files with 431 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files

174
scripts/rsl_rl/play.py
View File

@@ -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}")

160
scripts/zero_agent.py
View File

@@ -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()

44
source/mindbot/mindbot/robot/mindbot.py
View File

@@ -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),

2
source/mindbot/mindbot/tasks/manager_based/centrifuge/agents/rsl_rl_ppo_cfg.py
View File

@@ -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,

312
source/mindbot/mindbot/tasks/manager_based/centrifuge/centrifuge_env_cfg.py
View File

@@ -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 说明掉下去了或者被砸进去了
},
)

10
source/mindbot/mindbot/tasks/manager_based/pullUltrasoundLidUp/mindbot_env_cfg.py
View File

@@ -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()