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
# --- 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