离心机加试管初步配置
This commit is contained in:
@@ -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),
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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 说明掉下去了或者被砸进去了
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user