Apollo3D/Gym_GPU
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

reward modification and add stage reward

Browse Source
This commit is contained in:
ChenXi
2026-03-23 10:17:31 -04:00
parent 4bc205399c
commit f1bd15d434
1 changed files with 60 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

193
rl_game/get_up/config/t1_env_cfg.py
View File

@@ -15,106 +15,55 @@ from isaaclab.utils import configclass
from rl_game.get_up.env.t1_env import T1SceneCfg from rl_game.get_up.env.t1_env import T1SceneCfg
# --- 1. 自定义 MDP 逻辑函数 --- # --- 1. 自定义逻辑：阶段性解锁奖励 ---
def standing_with_feet_reward( def sequenced_getup_reward(
env: ManagerBasedRLEnv, env: ManagerBasedRLEnv,
min_head_height: float, crouch_threshold: float = 0.7, # 蜷缩完成度达到多少解锁下一阶段
min_pelvis_height: float, target_knee: float = 1.5,
sensor_cfg: SceneEntityCfg, target_hip: float = 1.2
force_threshold: float = 20.0,
max_v_z: float = 0.5
) -> torch.Tensor: ) -> 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)：无论仰卧还是俯卧，只要高度低，就必须强制收腿。 1. 计算蜷缩程度。
2. 仰卧支撑 (Back-Pushing)：在仰卧状态下，鼓励手臂向后发力并抬高盆骨。 2. 记录当前 Episode 是否曾经达到过蜷缩目标。
3. 协同爆发 (Explosive Jump)：蜷缩状态下产生的向上动量获得最高倍率奖励。 3. 返回 基础蜷缩奖 + (解锁标志 * 站立奖)。
""" """
# --- 1. 获取物理状态 --- # --- 1. 初始化/重置状态位 ---
gravity_z = env.scene["robot"].data.projected_gravity_b[:, 2] # 1:仰卧, -1:俯卧 if "has_crouched" not in env.extras:
pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk") env.extras["has_crouched"] = torch.zeros(env.num_envs, device=env.device, dtype=torch.bool)
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]
# 关节索引：11,12髋, 17,18膝 (确保与T1模型一致) # 每一回合开始时（reset_buf 为 1），重置该机器人的状态位
knee_joints = [17, 18] env.extras["has_crouched"] &= ~env.reset_buf
hip_pitch_joints = [11, 12]
# --- 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 joint_pos = env.scene["robot"].data.joint_pos
# --- 2. 核心蜷缩评分 (Crouch Score) --- 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)
# 提高膝盖弯曲目标 (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)
# 基础蜷缩奖励 (Spring Base) - 权重加大 # 蜷缩得分 (0.0 ~ 1.0)
crouch_trigger = torch.clamp(0.6 - curr_pelvis_h, min=0.0) crouch_score = torch.exp(-(knee_error + hip_error) / 0.6)
base_crouch_reward = crouch_trigger * crouch_score * 40.0
# --- 3. 支撑力奖励 (Support Force) --- # --- 3. 判断是否触发解锁 ---
push_reward = torch.zeros_like(curr_pelvis_h) # 只要在这一回合内，crouch_score 曾经超过阈值，就永久解锁高度奖
contact_sensor = env.scene.sensors.get("contact_sensor") current_success = crouch_score > crouch_threshold
if contact_sensor is not None: env.extras["has_crouched"] |= current_success
# 监测非足部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)
# --- 4. 姿态特定引导 (Orientation-Neutral) --- # --- 4. 计算高度奖励 ---
is_back = torch.clamp(gravity_z, min=0.0) # 仰卧程度 pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
is_belly = torch.clamp(-gravity_z, min=0.0) # 俯卧程度 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 增加)，给予重奖 return 5.0 * crouch_score + gated_standing_reward
# 配合crouch_score，鼓励“收腿-撑地-挺髋”的动作链
back_lift_reward = is_back * torch.clamp(curr_pelvis_h - 0.15, min=0.0) * crouch_score * 50.0
# 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( def is_standing_still(
env: ManagerBasedRLEnv, env: ManagerBasedRLEnv,
@@ -193,7 +142,7 @@ class T1EventCfg:
"yaw": (-3.14, 3.14), "yaw": (-3.14, 3.14),
"x": (0.0, 0.0), "x": (0.0, 0.0),
"y": (0.0, 0.0), "y": (0.0, 0.0),
"z": (0.3, 0.4), "z": (0.35, 0.45),
}, },
"velocity_range": {}, "velocity_range": {},
}, },
@@ -210,14 +159,14 @@ class T1ActionCfg:
'Left_Shoulder_Pitch', 'Left_Shoulder_Roll', 'Left_Elbow_Pitch', 'Left_Elbow_Yaw', 'Left_Shoulder_Pitch', 'Left_Shoulder_Roll', 'Left_Elbow_Pitch', 'Left_Elbow_Yaw',
'Right_Shoulder_Pitch', 'Right_Shoulder_Roll', 'Right_Elbow_Pitch', 'Right_Elbow_Yaw' 'Right_Shoulder_Pitch', 'Right_Shoulder_Roll', 'Right_Elbow_Pitch', 'Right_Elbow_Yaw'
], ],
scale=1.2, # 给了手臂相对充裕的自由度去摸索 scale=1.0, # 给了手臂相对充裕的自由度去摸索
use_default_offset=True use_default_offset=True
) )
torso_action = JointPositionActionCfg( torso_action = JointPositionActionCfg(
asset_name="robot", asset_name="robot",
joint_names=['Waist', 'AAHead_yaw', 'Head_pitch'], joint_names=['Waist', 'AAHead_yaw', 'Head_pitch'],
scale=0.8, scale=0.7,
use_default_offset=True use_default_offset=True
) )
@@ -228,59 +177,39 @@ class T1ActionCfg:
'Left_Hip_Yaw', 'Right_Hip_Yaw', 'Left_Knee_Pitch', 'Right_Knee_Pitch', 'Left_Hip_Yaw', 'Right_Hip_Yaw', 'Left_Knee_Pitch', 'Right_Knee_Pitch',
'Left_Ankle_Pitch', 'Right_Ankle_Pitch', 'Left_Ankle_Roll', 'Right_Ankle_Roll' 'Left_Ankle_Pitch', 'Right_Ankle_Pitch', 'Left_Ankle_Roll', 'Right_Ankle_Roll'
], ],
scale=0.6, scale=0.5,
use_default_offset=True use_default_offset=True
) )
@configclass @configclass
class T1GetUpRewardCfg: class T1GetUpRewardCfg:
# 1. 核心阶段性引导 (翻身 -> 蜷缩 -> 支撑) # 核心：顺序阶段奖励
dynamic_strategy = RewTerm( sequenced_task = RewTerm(
func=dynamic_getup_strategy_reward, func=sequenced_getup_reward,
weight=1.5 weight=10.0,
params={"crouch_threshold": 0.75} # 必须完成 75% 的收腿动作才解锁高度奖
) )
# 2. 站立质量奖励 (强化双脚受力) # 姿态惩罚：即便解锁了高度奖，如果姿态歪了也要扣分
height_with_feet = RewTerm( orientation = RewTerm(
func=standing_with_feet_reward, func=mdp.flat_orientation_l2,
weight=40.0, # 大权重 weight=-2.5
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
}
) )
# 3. 惩罚项：防止钻空子 # 抑制抽搐
# 严厉惩罚：如果躯干(Trunk)或头(H2)直接接触地面，扣大分 action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.08)
body_contact_penalty = RewTerm(
func=mdp.contact_forces,
weight=-20.0,
params={
"sensor_cfg": SceneEntityCfg("contact_sensor", body_names=["Trunk", "H2"]),
"threshold": 1.0
}
)
# 4. 关节功耗惩罚 (防止高频抽搐) # 最终站稳奖
action_rate = RewTerm(
func=mdp.action_rate_l2,
weight=-0.01
)
# 5. 成功维持奖励
is_success_maintain = RewTerm( is_success_maintain = RewTerm(
func=is_standing_still, func=is_standing_still,
weight=1000.0, # 巨大的成功奖励 weight=100.0,
params={ params={
"min_head_height": 1.08, "min_head_height": 1.08,
"min_pelvis_height": 0.72, "min_pelvis_height": 0.72,
"max_angle_error": 0.2, "max_angle_error": 0.25,
"standing_time": 0.4, # 必须站稳 0.4s "standing_time": 0.4,
"velocity_threshold": 0.3 "velocity_threshold": 0.2
} }
) )
@@ -291,11 +220,11 @@ class T1GetUpTerminationsCfg:
standing_success = DoneTerm( standing_success = DoneTerm(
func=is_standing_still, func=is_standing_still,
params={ params={
"min_head_height": 1.05, "min_head_height": 1.08,
"min_pelvis_height": 0.75, "min_pelvis_height": 0.72,
"max_angle_error": 0.3, "max_angle_error": 0.3,
"standing_time": 0.2, "standing_time": 0.3,
"velocity_threshold": 0.5 "velocity_threshold": 0.4
} }
) )
@@ -303,12 +232,10 @@ class T1GetUpTerminationsCfg:
@configclass @configclass
class T1EnvCfg(ManagerBasedRLEnvCfg): class T1EnvCfg(ManagerBasedRLEnvCfg):
scene = T1SceneCfg(num_envs=8192, env_spacing=2.5) scene = T1SceneCfg(num_envs=8192, env_spacing=2.5)
observations = T1ObservationCfg() observations = T1ObservationCfg()
rewards = T1GetUpRewardCfg() rewards = T1GetUpRewardCfg()
terminations = T1GetUpTerminationsCfg() terminations = T1GetUpTerminationsCfg()
events = T1EventCfg() events = T1EventCfg()
actions = T1ActionCfg() actions = T1ActionCfg()
episode_length_s = 10.0 episode_length_s = 10.0
decimation = 4 decimation = 4