离心机加试管初步配置

This commit is contained in:
2026-02-02 16:39:33 +08:00
parent 196378f2d3
commit 9325184f4d
6 changed files with 431 additions and 271 deletions

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

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,

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

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