diff --git a/rl_game/get_up/config/t1_env_cfg.py b/rl_game/get_up/config/t1_env_cfg.py index 85921f1..7d31f6f 100644 --- a/rl_game/get_up/config/t1_env_cfg.py +++ b/rl_game/get_up/config/t1_env_cfg.py @@ -15,106 +15,55 @@ from isaaclab.utils import configclass from rl_game.get_up.env.t1_env import T1SceneCfg -# --- 1. 自定义 MDP 逻辑函数 --- +# --- 1. 自定义逻辑：阶段性解锁奖励 --- -def standing_with_feet_reward( +def sequenced_getup_reward( env: ManagerBasedRLEnv, - min_head_height: float, - min_pelvis_height: float, - sensor_cfg: SceneEntityCfg, - force_threshold: float = 20.0, - max_v_z: float = 0.5 + crouch_threshold: float = 0.7, # 蜷缩完成度达到多少解锁下一阶段 + target_knee: float = 1.5, + target_hip: float = 1.2 ) -> torch.Tensor: - """终极高度目标：头高、盆骨高、足部受力稳定""" - head_idx, _ = env.scene["robot"].find_bodies("H2") - pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk") - - curr_head_h = env.scene["robot"].data.body_state_w[:, head_idx[0], 2] - curr_pelvis_h = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2] - - # 归一化高度评分 - head_score = torch.clamp(curr_head_h / min_head_height, 0.0, 1.2) - pelvis_score = torch.clamp(curr_pelvis_h / min_pelvis_height, 0.0, 1.2) - height_reward = (head_score + pelvis_score) / 2.0 - - # 足部受力判定 - contact_sensor = env.scene.sensors.get(sensor_cfg.name) - if contact_sensor is None: return torch.zeros(env.num_envs, device=env.device) - - foot_forces_z = torch.sum(contact_sensor.data.net_forces_w[:, :, 2], dim=-1) - force_weight = torch.sigmoid((foot_forces_z - force_threshold) / 5.0) - - # 垂直速度惩罚（防止跳跃不稳） - root_vel_z = env.scene["robot"].data.root_lin_vel_w[:, 2] - vel_penalty = torch.exp(-torch.abs(root_vel_z) / max_v_z) - - return height_reward * (0.5 + 0.5 * force_weight * vel_penalty) - - -def dynamic_getup_strategy_reward(env: ManagerBasedRLEnv) -> torch.Tensor: """ - 全姿态对称起立策略： - 1. 核心蜷缩 (Spring Loading)：无论仰卧还是俯卧，只要高度低，就必须强制收腿。 - 2. 仰卧支撑 (Back-Pushing)：在仰卧状态下，鼓励手臂向后发力并抬高盆骨。 - 3. 协同爆发 (Explosive Jump)：蜷缩状态下产生的向上动量获得最高倍率奖励。 + 【核心修改】只有先蜷缩，才能拿高度分： + 1. 计算蜷缩程度。 + 2. 记录当前 Episode 是否曾经达到过蜷缩目标。 + 3. 返回 基础蜷缩奖 + (解锁标志 * 站立奖)。 """ - # --- 1. 获取物理状态 --- - gravity_z = env.scene["robot"].data.projected_gravity_b[:, 2] # 1:仰卧, -1:俯卧 - pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk") - curr_pelvis_h = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2] - root_vel_z = env.scene["robot"].data.root_lin_vel_w[:, 2] + # --- 1. 初始化/重置状态位 --- + if "has_crouched" not in env.extras: + env.extras["has_crouched"] = torch.zeros(env.num_envs, device=env.device, dtype=torch.bool) - # 关节索引：11,12髋, 17,18膝 (确保与T1模型一致) - knee_joints = [17, 18] - hip_pitch_joints = [11, 12] + # 每一回合开始时（reset_buf 为 1），重置该机器人的状态位 + env.extras["has_crouched"] &= ~env.reset_buf + + # --- 2. 计算当前蜷缩质量 --- + knee_names = ['Left_Knee_Pitch', 'Right_Knee_Pitch'] + hip_names = ['Left_Hip_Pitch', 'Right_Hip_Pitch'] + knee_indices, _ = env.scene["robot"].find_joints(knee_names) + hip_indices, _ = env.scene["robot"].find_joints(hip_names) joint_pos = env.scene["robot"].data.joint_pos - # --- 2. 核心蜷缩评分 (Crouch Score) --- - # 无论仰俯，蜷缩是起立的绝对前提。目标是让脚尽可能靠近质心。 - # 提高膝盖弯曲目标 (1.5 rad)，引导更深度的折叠 - knee_flex_err = torch.abs(joint_pos[:, knee_joints] - 1.5).sum(dim=-1) - hip_flex_err = torch.abs(joint_pos[:, hip_pitch_joints] - 1.2).sum(dim=-1) - crouch_score = torch.exp(-(knee_flex_err + hip_flex_err) * 0.6) + knee_error = torch.mean(torch.abs(joint_pos[:, knee_indices] - target_knee), dim=-1) + hip_error = torch.mean(torch.abs(joint_pos[:, hip_indices] - target_hip), dim=-1) - # 基础蜷缩奖励 (Spring Base) - 权重加大 - crouch_trigger = torch.clamp(0.6 - curr_pelvis_h, min=0.0) - base_crouch_reward = crouch_trigger * crouch_score * 40.0 + # 蜷缩得分 (0.0 ~ 1.0) + crouch_score = torch.exp(-(knee_error + hip_error) / 0.6) - # --- 3. 支撑力奖励 (Support Force) --- - push_reward = torch.zeros_like(curr_pelvis_h) - contact_sensor = env.scene.sensors.get("contact_sensor") - if contact_sensor is not None: - # 监测非足部Link（手、臂）的受力 - # 无论正反，只要手能提供垂直向上的推力，就是好手 - arm_forces_z = contact_sensor.data.net_forces_w[:, :, 2] - push_reward = torch.tanh(torch.max(arm_forces_z, dim=-1)[0] / 30.0) + # --- 3. 判断是否触发解锁 --- + # 只要在这一回合内，crouch_score 曾经超过阈值，就永久解锁高度奖 + current_success = crouch_score > crouch_threshold + env.extras["has_crouched"] |= current_success - # --- 4. 姿态特定引导 (Orientation-Neutral) --- - is_back = torch.clamp(gravity_z, min=0.0) # 仰卧程度 - is_belly = torch.clamp(-gravity_z, min=0.0) # 俯卧程度 + # --- 4. 计算高度奖励 --- + pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk") + curr_pelvis_h = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2] + # 只有解锁后，高度奖励才生效 (0.0 或 高度值) + standing_reward = torch.clamp(curr_pelvis_h - 0.3, min=0.0) * 20.0 + gated_standing_reward = env.extras["has_crouched"].float() * standing_reward - # A. 仰卧直接起立逻辑： - # 在仰卧时，如果能把盆骨撑起来 (curr_pelvis_h 增加)，给予重奖 - # 配合crouch_score，鼓励“收腿-撑地-挺髋”的动作链 - back_lift_reward = is_back * torch.clamp(curr_pelvis_h - 0.15, min=0.0) * crouch_score * 50.0 + # 总奖励 = 持续引导蜷缩 + 只有解锁后才有的站立奖 + return 5.0 * crouch_score + gated_standing_reward - # B. 俯卧/翻身辅助逻辑 (保留一定的翻身倾向，但不再是唯一路径) - flip_reward = is_back * (1.0 - gravity_z) * 5.0 # 权重降低，仅作为备选 - - # --- 5. 最终爆发项 (The Jump) --- - # 核心公式：蜷缩程度 * 向上速度 * 支撑力感应 - # 这是一个通用的“起跳”奖励，无论正反面，只要满足“缩得紧、跳得快、手有撑”，奖励就爆炸 - explosion_reward = crouch_score * torch.clamp(root_vel_z, min=0.0) * (0.5 + 0.5 * push_reward) * 80.0 - - # --- 6. 汇总 --- - total_reward = ( - base_crouch_reward + # 必须缩腿 - back_lift_reward + # 仰卧挺髋 - flip_reward + # 翻身尝试 - explosion_reward # 终极爆发 - ) - - return total_reward def is_standing_still( env: ManagerBasedRLEnv, @@ -193,7 +142,7 @@ class T1EventCfg: "yaw": (-3.14, 3.14), "x": (0.0, 0.0), "y": (0.0, 0.0), - "z": (0.3, 0.4), + "z": (0.35, 0.45), }, "velocity_range": {}, }, @@ -210,14 +159,14 @@ class T1ActionCfg: 'Left_Shoulder_Pitch', 'Left_Shoulder_Roll', 'Left_Elbow_Pitch', 'Left_Elbow_Yaw', 'Right_Shoulder_Pitch', 'Right_Shoulder_Roll', 'Right_Elbow_Pitch', 'Right_Elbow_Yaw' ], - scale=1.2, # 给了手臂相对充裕的自由度去摸索 + scale=1.0, # 给了手臂相对充裕的自由度去摸索 use_default_offset=True ) torso_action = JointPositionActionCfg( asset_name="robot", joint_names=['Waist', 'AAHead_yaw', 'Head_pitch'], - scale=0.8, + scale=0.7, use_default_offset=True ) @@ -228,59 +177,39 @@ class T1ActionCfg: 'Left_Hip_Yaw', 'Right_Hip_Yaw', 'Left_Knee_Pitch', 'Right_Knee_Pitch', 'Left_Ankle_Pitch', 'Right_Ankle_Pitch', 'Left_Ankle_Roll', 'Right_Ankle_Roll' ], - scale=0.6, + scale=0.5, use_default_offset=True ) @configclass class T1GetUpRewardCfg: - # 1. 核心阶段性引导 (翻身 -> 蜷缩 -> 支撑) - dynamic_strategy = RewTerm( - func=dynamic_getup_strategy_reward, - weight=1.5 + # 核心：顺序阶段奖励 + sequenced_task = RewTerm( + func=sequenced_getup_reward, + weight=10.0, + params={"crouch_threshold": 0.75} # 必须完成 75% 的收腿动作才解锁高度奖 ) - # 2. 站立质量奖励 (强化双脚受力) - height_with_feet = RewTerm( - func=standing_with_feet_reward, - weight=40.0, # 大权重 - params={ - "min_head_height": 1.1, - "min_pelvis_height": 0.7, - "sensor_cfg": SceneEntityCfg("contact_sensor", body_names=[".*_foot_link"]), - "force_threshold": 40.0, # 必须达到一定压力，防止脚尖点地作弊 - "max_v_z": 0.2 - } + # 姿态惩罚：即便解锁了高度奖，如果姿态歪了也要扣分 + orientation = RewTerm( + func=mdp.flat_orientation_l2, + weight=-2.5 ) - # 3. 惩罚项：防止钻空子 - # 严厉惩罚：如果躯干(Trunk)或头(H2)直接接触地面，扣大分 - body_contact_penalty = RewTerm( - func=mdp.contact_forces, - weight=-20.0, - params={ - "sensor_cfg": SceneEntityCfg("contact_sensor", body_names=["Trunk", "H2"]), - "threshold": 1.0 - } - ) + # 抑制抽搐 + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.08) - # 4. 关节功耗惩罚 (防止高频抽搐) - action_rate = RewTerm( - func=mdp.action_rate_l2, - weight=-0.01 - ) - - # 5. 成功维持奖励 + # 最终站稳奖 is_success_maintain = RewTerm( func=is_standing_still, - weight=1000.0, # 巨大的成功奖励 + weight=100.0, params={ "min_head_height": 1.08, "min_pelvis_height": 0.72, - "max_angle_error": 0.2, - "standing_time": 0.4, # 必须站稳 0.4s - "velocity_threshold": 0.3 + "max_angle_error": 0.25, + "standing_time": 0.4, + "velocity_threshold": 0.2 } ) @@ -291,11 +220,11 @@ class T1GetUpTerminationsCfg: standing_success = DoneTerm( func=is_standing_still, params={ - "min_head_height": 1.05, - "min_pelvis_height": 0.75, + "min_head_height": 1.08, + "min_pelvis_height": 0.72, "max_angle_error": 0.3, - "standing_time": 0.2, - "velocity_threshold": 0.5 + "standing_time": 0.3, + "velocity_threshold": 0.4 } ) @@ -303,12 +232,10 @@ class T1GetUpTerminationsCfg: @configclass class T1EnvCfg(ManagerBasedRLEnvCfg): scene = T1SceneCfg(num_envs=8192, env_spacing=2.5) - observations = T1ObservationCfg() rewards = T1GetUpRewardCfg() terminations = T1GetUpTerminationsCfg() events = T1EventCfg() actions = T1ActionCfg() - episode_length_s = 10.0 decimation = 4 \ No newline at end of file