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

Add weighting function, change the reward logic

Browse Source
This commit is contained in:
ChenXi
2026-03-22 21:11:46 -04:00
parent a642274fa6
commit 7f7ec781c5
4 changed files with 84 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,87 +26,98 @@ def standing_with_feet_reward(
force_threshold: float = 20.0,
max_v_z: float = 0.5
) -> torch.Tensor:
# 增加防护：从场景中安全获取 body 索引
"""终极高度目标：头高、盆骨高、足部受力稳定"""
head_idx, _ = env.scene["robot"].find_bodies("H2")
pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
# 1. 高度奖励：使用更稳定的归一化，限制范围在 [0, 1]
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]
# 使用 sigmoid 或简单的 min-max 映射，避免除以极小值
# 归一化高度评分
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
# 2. 足部受力：增加对 NaN 的防御
# 足部受力判定
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)
# 对巨大的冲击力做剪裁，防止 sigmoid 输入过大
foot_forces_z = torch.clamp(foot_forces_z, 0.0, 500.0)
force_weight = torch.sigmoid((foot_forces_z - force_threshold) / 5.0)
# 3. 垂直速度惩罚：使用更平滑的惩罚
# 垂直速度惩罚（防止跳跃不稳）
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 universal_arm_support_reward(
def arm_tuck_incremental_reward(
env: ManagerBasedRLEnv,
sensor_cfg: SceneEntityCfg,
height_threshold: float = 0.60,
min_force: float = 15.0
pitch_threshold: float = 1.4,
shaping_weight: float = 0.2
) -> torch.Tensor:
"""增量式收手奖励：鼓励向弯曲方向运动，达到阈值给大奖"""
joint_names = ["Left_Elbow_Pitch", "Right_Elbow_Pitch"]
joint_ids, _ = env.scene["robot"].find_joints(joint_names)
elbow_pos = env.scene["robot"].data.joint_pos[:, joint_ids]
elbow_vel = env.scene["robot"].data.joint_vel[:, joint_ids]
# 1. 速度引导：只要在收缩（速度为正）就给小奖，伸直则惩罚
avg_vel = torch.mean(elbow_vel, dim=-1)
shaping_reward = torch.tanh(avg_vel) * shaping_weight
# 2. 阈值触发：一旦收缩到位，给稳定的静态奖
is_tucked = torch.all(elbow_pos > pitch_threshold, dim=-1).float()
goal_bonus = is_tucked * 1.5
return shaping_reward + goal_bonus
def dynamic_getup_strategy_reward(env: ManagerBasedRLEnv) -> torch.Tensor:
"""
通用手臂支撑奖励：同时支持仰卧起坐支撑和俯卧撑起。
逻辑：只要手臂有向上的推力，且身体正在向上移动，就给奖。
状态机奖励切换逻辑：
- 仰卧时：重点是 翻身 + 缩手。
- 俯卧时：重点是 撑地起立。
"""
# 1. 获取传感器数据
contact_sensor = env.scene.sensors.get(sensor_cfg.name)
if contact_sensor is None:
return torch.zeros(env.num_envs, device=env.device)
# 获取重力投影：Z轴分量 > 0 表示仰卧
gravity_z = env.scene["robot"].data.projected_gravity_b[:, 2]
# 获取所有定义的手臂/手部 link 的垂直总受力 (World Z)
# net_forces_w 形状: (num_envs, num_bodies, 3)
arm_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
# 取所有受力点的最大值或平均值，代表支撑强度
max_arm_force = torch.max(arm_forces_z, dim=-1)[0]
# 状态掩码
is_on_back = (gravity_z > 0.2).float()
is_on_belly = (gravity_z < -0.2).float()
is_transition = (1.0 - is_on_back - is_on_belly)
# 2. 获取状态数据
pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
pelvis_pos_z = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2]
root_vel_z = env.scene["robot"].data.root_lin_vel_w[:, 2]
# 1. 翻身势能：引导 gravity_z 向 -1.0 靠拢
flip_shaping = torch.clamp(-gravity_z, min=-1.0, max=1.0)
# 3. 计算奖励项
# A. 受力奖励：鼓励手部与地面产生大于 min_force 的推力
# 使用 tanh 归一化，防止力矩过大导致奖励爆炸 (NaN 风险)
force_reward = torch.tanh(torch.clamp(max_arm_force - min_force, min=0.0) / 50.0)
# 2. 缩手动作
tuck_rew = arm_tuck_incremental_reward(env)
# B. 速度引导：只有当机器人正在“向上起”时，支撑奖励才翻倍
# 这样可以防止它趴在地上乱按手骗分
velocity_factor = torch.clamp(root_vel_z, min=0.0, max=2.0)
# 3. 撑地动作 (复用原逻辑，但去掉内部的高度衰减，统一由状态机控制)
contact_sensor = env.scene.sensors.get("contact_sensor")
max_arm_force = torch.zeros(env.num_envs, device=env.device)
if contact_sensor is not None:
# 假设手臂/手部 link 的受力
arm_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
max_arm_force = torch.max(arm_forces_z, dim=-1)[0]
# C. 姿态惩罚回避：
# 不再检查手是否在盆骨下方，而是检查手是否“在干活”
# 只要受力足够大，就认为是在支撑
is_supporting = (max_arm_force > min_force).float()
push_rew = torch.tanh(torch.clamp(max_arm_force - 15.0, min=0.0) / 40.0)
# 4. 阶段性退出机制 (Curriculum)
# 当盆骨高度超过 height_threshold (0.6m) 时，奖励线性消失
# 强迫机器人最终依靠腿部力量平衡，而不是一直扶着地
height_fade = torch.clamp((height_threshold - pelvis_pos_z) / 0.15, min=0.0, max=1.0)
# --- 权重动态合成 ---
# 仰卧区：翻身(8.0) + 缩手(4.0)
back_strategy = is_on_back * (8.0 * flip_shaping + 4.0 * tuck_rew)
# 最终组合
# 逻辑：受力 * (1 + 垂直速度) * 高度衰减
total_reward = force_reward * (1.0 + 2.0 * velocity_factor) * is_supporting * height_fade
# 俯卧区：撑地(25.0) + 缩手维持(1.0)
# 这里撑地权重远高于翻身，确保机器人更愿意待在俯卧区尝试站立
belly_strategy = is_on_belly * (25.0 * push_rew + 1.0 * tuck_rew)
# 过渡区
trans_strategy = is_transition * (4.0 * flip_shaping + 10.0 * push_rew + 2.0 * tuck_rew)
return back_strategy + belly_strategy + trans_strategy
return total_reward
def is_standing_still(
env: ManagerBasedRLEnv,
@@ -126,7 +137,6 @@ def is_standing_still(
gravity_error = torch.norm(env.scene["robot"].data.projected_gravity_b[:, :2], dim=-1)
root_vel_norm = torch.norm(env.scene["robot"].data.root_lin_vel_w, dim=-1)
# 判定条件：头够高 且 盆骨够高 且 垂直误差小 且 速度低
is_stable_now = (
(current_head_h > min_head_height) &
(current_pelvis_h > min_pelvis_height) &
@@ -143,21 +153,17 @@ def is_standing_still(
return env.extras["stable_timer"] > standing_time
# --- 2. 配置类 ---
T1_JOINT_NAMES = [
'AAHead_yaw', 'Head_pitch',
'Left_Shoulder_Pitch', 'Left_Shoulder_Roll', 'Left_Elbow_Pitch', 'Left_Elbow_Yaw',
'Right_Shoulder_Pitch', 'Right_Shoulder_Roll', 'Right_Elbow_Pitch', 'Right_Elbow_Yaw',
'Waist',
'Left_Hip_Pitch', 'Right_Hip_Pitch', 'Left_Hip_Roll', 'Right_Hip_Roll',
'Left_Hip_Yaw', 'Right_Hip_Yaw', 'Left_Knee_Pitch', 'Right_Knee_Pitch',
'Left_Ankle_Pitch', 'Right_Ankle_Pitch', 'Left_Ankle_Roll', 'Right_Ankle_Roll'
]
@@ -186,14 +192,13 @@ class T1EventCfg:
params={
"asset_cfg": SceneEntityCfg("robot"),
"pose_range": {
"roll": (-1.57, 1.57), # 左右侧卧
"roll": (-1.57, 1.57),
"pitch": tuple(numpy.array([1.4, 1.6], dtype=np.float32) * random.choice([-1 , 1])), # 仰卧/俯卧
"yaw": (-3.14, 3.14), # 全向旋转
"yaw": (-3.14, 3.14),
"x": (0.0, 0.0),
"y": (0.0, 0.0),
"z": (0.3, 0.4),
},
"velocity_range": {},
},
mode="reset",
)
@@ -201,61 +206,41 @@ class T1EventCfg:
@configclass
class T1ActionCfg:
"""关键修改：降低 scale 让动作变丝滑，增大阻尼效果"""
joint_pos = JointPositionActionCfg(
asset_name="robot",
joint_names=T1_JOINT_NAMES,
scale=0.5,
use_default_offset=True
asset_name="robot", joint_names=T1_JOINT_NAMES, scale=0.5, use_default_offset=True
)
@configclass
class T1GetUpRewardCfg:
# 1. 姿态基础奖 (引导身体变正)
upright = RewTerm(func=mdp.flat_orientation_l2, weight=2.0)
# --- 1. 动态策略整合奖励 (包含了翻身、缩手、撑地的逻辑切换) ---
adaptive_strategy = RewTerm(
func=dynamic_getup_strategy_reward,
weight=1.0 # 内部已经有细分权重
)
# 2. 【条件高度奖】：双高度判定（头+盆骨），且必须脚踩地
# --- 2. 核心高度目标 (维持最高优先级) ---
height_with_feet = RewTerm(
func=standing_with_feet_reward,
weight=20.0, # 作为核心引导，增加权重
weight=15.0,
params={
"min_head_height": 1.10,
"min_head_height": 1.1,
"min_pelvis_height": 0.7,
"sensor_cfg": SceneEntityCfg("contact_sensor", body_names=[".*_foot_link"]),
"force_threshold": 20.0,
"force_threshold": 30.0,
"max_v_z": 0.3
}
)
# 3. 手臂撑地奖：辅助脱离地面阶段
arm_push_support = RewTerm(
func=universal_arm_support_reward,
weight=15.0, # 显著增加权重（从 3.0 提到 15.0），让它成为起步的关键
params={
"sensor_cfg": SceneEntityCfg("contact_sensor", body_names=[".*_hand_link", "AL3", "AR3"]),
"height_threshold": 0.65, # 躯干升到 0.6m 前都鼓励手臂用力
"min_force": 8.0 # 只要有 15N 的力就触发
}
)
# --- 3. 辅助约束与惩罚 ---
upright = RewTerm(func=mdp.flat_orientation_l2, weight=1.0)
joint_limits = RewTerm(func=mdp.joint_pos_limits, weight=-20.0, params={"asset_cfg": SceneEntityCfg("robot")})
action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.01)
# 4. 关节限位惩罚 (新增：防止关节撞死导致数值问题)
joint_limits = RewTerm(
func=mdp.joint_pos_limits,
weight=-1.0,
params={"asset_cfg": SceneEntityCfg("robot")}
)
# 5. 时间惩罚 (强制效率)
time_penalty = RewTerm(
func=mdp.is_alive,
weight=-1.2
)
# 6. 成功终极大奖
is_success = RewTerm(
# --- 4. 成功奖励 ---
is_success_bonus = RewTerm(
func=is_standing_still,
weight=800.0,
weight=1000.0,
params={
"min_head_height": 1.05,
"min_pelvis_height": 0.75,
@@ -268,11 +253,6 @@ class T1GetUpRewardCfg:
@configclass
class T1GetUpTerminationsCfg:
time_out = DoneTerm(func=mdp.time_out)
# 失败判定：躯干倾斜超过 45 度重置
#base_crash = DoneTerm(func=mdp.bad_orientation, params={"limit_angle": 0.785})
# 成功判定：双高度 + 稳定
standing_success = DoneTerm(
func=is_standing_still,
params={
@@ -287,7 +267,7 @@ class T1GetUpTerminationsCfg:
@configclass
class T1EnvCfg(ManagerBasedRLEnvCfg):
scene = T1SceneCfg(num_envs=8192, env_spacing=2.5) # 5090 性能全开
scene = T1SceneCfg(num_envs=8192, env_spacing=2.5)
def __post_init__(self):
super().__post_init__()